From f8750dd8ed3288e27f653f0d58f396a3bf7b92ab Mon Sep 17 00:00:00 2001 From: Areloch Date: Sun, 7 Jul 2019 02:43:49 -0500 Subject: [PATCH] Updated SDL, Bullet and OpenAL soft libs Fixed case sensitivity problem Fixed clang compiler problem with having the class namespace used in an inline for the == operator Tweaked some theme stuff to be more consistent. Added initial test of no-pie for linux test sidestep of getTexCoord in shadergen hlsl feature so we don't assert when getting the terrain's shaderstuffs(which uses float3 instead of normal float2) --- Engine/lib/bullet/.travis.yml | 79 +- Engine/lib/bullet/CMakeLists.txt | 67 +- Engine/lib/bullet/README.md | 9 +- Engine/lib/bullet/VERSION | 2 +- .../b3BroadphaseCallback.h | 38 + .../BroadPhaseCollision/b3DynamicBvh.cpp | 1352 ++ .../BroadPhaseCollision/b3DynamicBvh.h | 1332 ++ .../b3DynamicBvhBroadphase.cpp | 808 + .../b3DynamicBvhBroadphase.h | 197 + .../BroadPhaseCollision/b3OverlappingPair.h | 70 + .../b3OverlappingPairCache.cpp | 559 + .../b3OverlappingPairCache.h | 427 + .../BroadPhaseCollision/shared/b3Aabb.h | 56 + .../src/Bullet3Collision/CMakeLists.txt | 93 + .../NarrowPhaseCollision/b3Config.h | 39 + .../NarrowPhaseCollision/b3Contact4.h | 55 + .../NarrowPhaseCollision/b3ConvexUtility.cpp | 500 + .../NarrowPhaseCollision/b3ConvexUtility.h | 55 + .../NarrowPhaseCollision/b3CpuNarrowPhase.cpp | 297 + .../NarrowPhaseCollision/b3CpuNarrowPhase.h | 92 + .../NarrowPhaseCollision/b3RaycastInfo.h | 25 + .../NarrowPhaseCollision/b3RigidBodyCL.h | 28 + .../shared/b3BvhSubtreeInfoData.h | 19 + .../shared/b3BvhTraversal.h | 123 + .../NarrowPhaseCollision/shared/b3ClipFaces.h | 171 + .../shared/b3Collidable.h | 69 + .../shared/b3Contact4Data.h | 36 + .../shared/b3ContactConvexConvexSAT.h | 486 + .../shared/b3ContactSphereSphere.h | 153 + .../shared/b3ConvexPolyhedronData.h | 38 + .../shared/b3FindConcaveSatAxis.h | 797 + .../shared/b3FindSeparatingAxis.h | 197 + .../shared/b3MprPenetration.h | 888 + .../shared/b3NewContactReduction.h | 175 + .../shared/b3QuantizedBvhNodeData.h | 88 + .../shared/b3ReduceContacts.h | 89 + .../shared/b3RigidBodyData.h | 31 + .../shared/b3UpdateAabbs.h | 35 + .../bullet/src/Bullet3Collision/premake4.lua | 16 + .../bullet/src/Bullet3Common/CMakeLists.txt | 63 + .../src/Bullet3Common/b3AlignedAllocator.cpp | 186 + .../src/Bullet3Common/b3AlignedAllocator.h | 110 + .../src/Bullet3Common/b3AlignedObjectArray.h | 522 + .../src/Bullet3Common/b3CommandLineArgs.h | 106 + .../bullet/src/Bullet3Common/b3FileUtils.h | 133 + .../lib/bullet/src/Bullet3Common/b3HashMap.h | 462 + .../bullet/src/Bullet3Common/b3Logging.cpp | 145 + .../lib/bullet/src/Bullet3Common/b3Logging.h | 86 + .../bullet/src/Bullet3Common/b3Matrix3x3.h | 1354 ++ .../lib/bullet/src/Bullet3Common/b3MinMax.h | 69 + .../src/Bullet3Common/b3PoolAllocator.h | 121 + .../lib/bullet/src/Bullet3Common/b3QuadWord.h | 242 + .../bullet/src/Bullet3Common/b3Quaternion.h | 908 + .../lib/bullet/src/Bullet3Common/b3Random.h | 46 + .../src/Bullet3Common/b3ResizablePool.h | 171 + .../lib/bullet/src/Bullet3Common/b3Scalar.h | 685 + .../bullet/src/Bullet3Common/b3StackAlloc.h | 118 + .../bullet/src/Bullet3Common/b3Transform.h | 286 + .../src/Bullet3Common/b3TransformUtil.h | 210 + .../bullet/src/Bullet3Common/b3Vector3.cpp | 1637 ++ .../lib/bullet/src/Bullet3Common/b3Vector3.h | 1303 ++ .../lib/bullet/src/Bullet3Common/premake4.lua | 16 + .../src/Bullet3Common/shared/b3Float4.h | 90 + .../bullet/src/Bullet3Common/shared/b3Int2.h | 63 + .../bullet/src/Bullet3Common/shared/b3Int4.h | 71 + .../src/Bullet3Common/shared/b3Mat3x3.h | 157 + .../shared/b3PlatformDefinitions.h | 41 + .../bullet/src/Bullet3Common/shared/b3Quat.h | 100 + .../bullet/src/Bullet3Dynamics/CMakeLists.txt | 61 + .../ConstraintSolver/b3ContactSolverInfo.h | 149 + .../ConstraintSolver/b3FixedConstraint.cpp | 103 + .../ConstraintSolver/b3FixedConstraint.h | 34 + .../b3Generic6DofConstraint.cpp | 737 + .../b3Generic6DofConstraint.h | 517 + .../ConstraintSolver/b3JacobianEntry.h | 150 + .../ConstraintSolver/b3PgsJacobiSolver.cpp | 1696 ++ .../ConstraintSolver/b3PgsJacobiSolver.h | 133 + .../b3Point2PointConstraint.cpp | 190 + .../b3Point2PointConstraint.h | 153 + .../ConstraintSolver/b3SolverBody.h | 281 + .../ConstraintSolver/b3SolverConstraint.h | 73 + .../ConstraintSolver/b3TypedConstraint.cpp | 151 + .../ConstraintSolver/b3TypedConstraint.h | 469 + .../b3CpuRigidBodyPipeline.cpp | 447 + .../Bullet3Dynamics/b3CpuRigidBodyPipeline.h | 62 + .../bullet/src/Bullet3Dynamics/premake4.lua | 18 + .../shared/b3ContactConstraint4.h | 31 + .../shared/b3ConvertConstraint4.h | 148 + .../src/Bullet3Dynamics/shared/b3Inertia.h | 14 + .../shared/b3IntegrateTransforms.h | 106 + .../bullet/src/Bullet3Geometry/CMakeLists.txt | 47 + .../bullet/src/Bullet3Geometry/b3AabbUtil.h | 217 + .../Bullet3Geometry/b3ConvexHullComputer.cpp | 2745 +++ .../Bullet3Geometry/b3ConvexHullComputer.h | 99 + .../src/Bullet3Geometry/b3GeometryUtil.cpp | 174 + .../src/Bullet3Geometry/b3GeometryUtil.h | 36 + .../b3GrahamScan2dConvexHull.h | 116 + .../bullet/src/Bullet3Geometry/premake4.lua | 16 + .../b3GpuBroadphaseInterface.h | 42 + .../b3GpuGridBroadphase.cpp | 338 + .../BroadphaseCollision/b3GpuGridBroadphase.h | 80 + .../b3GpuParallelLinearBvh.cpp | 557 + .../b3GpuParallelLinearBvh.h | 125 + .../b3GpuParallelLinearBvhBroadphase.cpp | 76 + .../b3GpuParallelLinearBvhBroadphase.h | 66 + .../b3GpuSapBroadphase.cpp | 1298 ++ .../BroadphaseCollision/b3GpuSapBroadphase.h | 143 + .../BroadphaseCollision/b3SapAabb.h | 13 + .../kernels/gridBroadphase.cl | 216 + .../kernels/gridBroadphaseKernels.h | 198 + .../kernels/parallelLinearBvh.cl | 767 + .../kernels/parallelLinearBvhKernels.h | 728 + .../BroadphaseCollision/kernels/sap.cl | 389 + .../BroadphaseCollision/kernels/sapKernels.h | 341 + .../bullet/src/Bullet3OpenCL/CMakeLists.txt | 77 + .../Initialize/b3OpenCLInclude.h | 51 + .../Initialize/b3OpenCLUtils.cpp | 963 + .../Bullet3OpenCL/Initialize/b3OpenCLUtils.h | 190 + .../NarrowphaseCollision/b3BvhInfo.h | 17 + .../NarrowphaseCollision/b3ContactCache.cpp | 253 + .../NarrowphaseCollision/b3ContactCache.h | 62 + .../b3ConvexHullContact.cpp | 4408 ++++ .../b3ConvexHullContact.h | 106 + .../b3ConvexPolyhedronCL.h | 7 + .../NarrowphaseCollision/b3GjkEpa.cpp | 1062 + .../NarrowphaseCollision/b3GjkEpa.h | 79 + .../NarrowphaseCollision/b3OptimizedBvh.cpp | 358 + .../NarrowphaseCollision/b3OptimizedBvh.h | 56 + .../NarrowphaseCollision/b3QuantizedBvh.cpp | 1254 ++ .../NarrowphaseCollision/b3QuantizedBvh.h | 511 + .../b3StridingMeshInterface.cpp | 207 + .../b3StridingMeshInterface.h | 158 + .../NarrowphaseCollision/b3SupportMappings.h | 34 + .../b3TriangleCallback.cpp | 24 + .../NarrowphaseCollision/b3TriangleCallback.h | 37 + .../b3TriangleIndexVertexArray.cpp | 90 + .../b3TriangleIndexVertexArray.h | 128 + .../NarrowphaseCollision/b3VectorFloat4.h | 10 + .../b3VoronoiSimplexSolver.cpp | 574 + .../b3VoronoiSimplexSolver.h | 164 + .../kernels/bvhTraversal.cl | 283 + .../kernels/bvhTraversal.h | 257 + .../NarrowphaseCollision/kernels/mpr.cl | 311 + .../NarrowphaseCollision/kernels/mprKernels.h | 1445 ++ .../kernels/primitiveContacts.cl | 1374 ++ .../kernels/primitiveContacts.h | 1288 ++ .../NarrowphaseCollision/kernels/sat.cl | 2018 ++ .../kernels/satClipHullContacts.cl | 1888 ++ .../kernels/satClipHullContacts.h | 2098 ++ .../kernels/satConcave.cl | 1220 ++ .../kernels/satConcaveKernels.h | 1456 ++ .../NarrowphaseCollision/kernels/satKernels.h | 2103 ++ .../ParallelPrimitives/b3BoundSearchCL.cpp | 203 + .../ParallelPrimitives/b3BoundSearchCL.h | 64 + .../ParallelPrimitives/b3BufferInfoCL.h | 18 + .../ParallelPrimitives/b3FillCL.cpp | 119 + .../ParallelPrimitives/b3FillCL.h | 52 + .../ParallelPrimitives/b3LauncherCL.cpp | 296 + .../ParallelPrimitives/b3LauncherCL.h | 128 + .../ParallelPrimitives/b3OpenCLArray.h | 300 + .../ParallelPrimitives/b3PrefixScanCL.cpp | 120 + .../ParallelPrimitives/b3PrefixScanCL.h | 35 + .../b3PrefixScanFloat4CL.cpp | 120 + .../ParallelPrimitives/b3PrefixScanFloat4CL.h | 36 + .../ParallelPrimitives/b3RadixSort32CL.cpp | 646 + .../ParallelPrimitives/b3RadixSort32CL.h | 84 + .../kernels/BoundSearchKernels.cl | 106 + .../kernels/BoundSearchKernelsCL.h | 86 + .../ParallelPrimitives/kernels/CopyKernels.cl | 128 + .../kernels/CopyKernelsCL.h | 131 + .../ParallelPrimitives/kernels/FillKernels.cl | 107 + .../kernels/FillKernelsCL.h | 90 + .../kernels/PrefixScanFloat4Kernels.cl | 154 + .../kernels/PrefixScanKernels.cl | 154 + .../kernels/PrefixScanKernelsCL.h | 128 + .../kernels/PrefixScanKernelsFloat4CL.h | 128 + .../kernels/RadixSort32Kernels.cl | 1071 + .../kernels/RadixSort32KernelsCL.h | 909 + .../Bullet3OpenCL/Raycast/b3GpuRaycast.cpp | 374 + .../src/Bullet3OpenCL/Raycast/b3GpuRaycast.h | 28 + .../Raycast/kernels/rayCastKernels.cl | 439 + .../Raycast/kernels/rayCastKernels.h | 380 + .../RigidBody/b3GpuConstraint4.h | 17 + .../RigidBody/b3GpuGenericConstraint.cpp | 134 + .../RigidBody/b3GpuGenericConstraint.h | 128 + .../RigidBody/b3GpuJacobiContactSolver.cpp | 1305 ++ .../RigidBody/b3GpuJacobiContactSolver.h | 56 + .../RigidBody/b3GpuNarrowPhase.cpp | 1013 + .../RigidBody/b3GpuNarrowPhase.h | 101 + .../RigidBody/b3GpuNarrowPhaseInternalData.h | 89 + .../RigidBody/b3GpuPgsConstraintSolver.cpp | 1068 + .../RigidBody/b3GpuPgsConstraintSolver.h | 76 + .../RigidBody/b3GpuPgsContactSolver.cpp | 1529 ++ .../RigidBody/b3GpuPgsContactSolver.h | 37 + .../RigidBody/b3GpuRigidBodyPipeline.cpp | 677 + .../RigidBody/b3GpuRigidBodyPipeline.h | 70 + .../b3GpuRigidBodyPipelineInternalData.h | 68 + .../Bullet3OpenCL/RigidBody/b3GpuSolverBody.h | 210 + .../RigidBody/b3GpuSolverConstraint.h | 73 + .../src/Bullet3OpenCL/RigidBody/b3Solver.cpp | 1128 + .../src/Bullet3OpenCL/RigidBody/b3Solver.h | 110 + .../RigidBody/kernels/batchingKernels.cl | 353 + .../RigidBody/kernels/batchingKernels.h | 387 + .../RigidBody/kernels/batchingKernelsNew.cl | 231 + .../RigidBody/kernels/batchingKernelsNew.h | 290 + .../RigidBody/kernels/integrateKernel.cl | 32 + .../RigidBody/kernels/integrateKernel.h | 432 + .../RigidBody/kernels/jointSolver.cl | 877 + .../RigidBody/kernels/jointSolver.h | 720 + .../RigidBody/kernels/solveContact.cl | 501 + .../RigidBody/kernels/solveContact.h | 392 + .../RigidBody/kernels/solveFriction.cl | 527 + .../RigidBody/kernels/solveFriction.h | 420 + .../RigidBody/kernels/solverSetup.cl | 277 + .../RigidBody/kernels/solverSetup.h | 702 + .../RigidBody/kernels/solverSetup2.cl | 613 + .../RigidBody/kernels/solverSetup2.h | 600 + .../RigidBody/kernels/solverUtils.cl | 968 + .../RigidBody/kernels/solverUtils.h | 908 + .../RigidBody/kernels/updateAabbsKernel.cl | 22 + .../RigidBody/kernels/updateAabbsKernel.h | 482 + .../lib/bullet/src/Bullet3OpenCL/premake4.lua | 32 + .../Bullet2FileLoader/CMakeLists.txt | 55 + .../Bullet2FileLoader/autogenerated/bullet2.h | 987 + .../Bullet2FileLoader/b3BulletFile.cpp | 400 + .../Bullet2FileLoader/b3BulletFile.h | 74 + .../Bullet2FileLoader/b3Chunk.cpp | 69 + .../Bullet2FileLoader/b3Chunk.h | 84 + .../Bullet2FileLoader/b3Common.h | 40 + .../Bullet2FileLoader/b3DNA.cpp | 616 + .../Bullet2FileLoader/b3DNA.h | 101 + .../Bullet2FileLoader/b3Defines.h | 149 + .../Bullet2FileLoader/b3File.cpp | 1653 ++ .../Bullet2FileLoader/b3File.h | 158 + .../Bullet2FileLoader/b3Serializer.cpp | 18062 ++++++++++++++++ .../Bullet2FileLoader/b3Serializer.h | 601 + .../Bullet2FileLoader/premake4.lua | 16 + .../BroadphaseCollision/btAxisSweep3.cpp | 12 +- .../BroadphaseCollision/btAxisSweep3.h | 8 +- .../btAxisSweep3Internal.h | 528 +- .../btBroadphaseInterface.h | 53 +- .../BroadphaseCollision/btBroadphaseProxy.cpp | 2 +- .../BroadphaseCollision/btBroadphaseProxy.h | 190 +- .../btCollisionAlgorithm.cpp | 1 - .../btCollisionAlgorithm.h | 42 +- .../BroadphaseCollision/btDbvt.cpp | 1422 +- .../BroadphaseCollision/btDbvt.h | 1637 +- .../BroadphaseCollision/btDbvtBroadphase.cpp | 862 +- .../BroadphaseCollision/btDbvtBroadphase.h | 149 +- .../BroadphaseCollision/btDispatcher.cpp | 2 - .../BroadphaseCollision/btDispatcher.h | 85 +- .../btOverlappingPairCache.cpp | 363 +- .../btOverlappingPairCache.h | 364 +- .../btOverlappingPairCallback.h | 16 +- .../BroadphaseCollision/btQuantizedBvh.cpp | 600 +- .../BroadphaseCollision/btQuantizedBvh.h | 338 +- .../btSimpleBroadphase.cpp | 152 +- .../BroadphaseCollision/btSimpleBroadphase.h | 107 +- .../bullet/src/BulletCollision/CMakeLists.txt | 2 + .../SphereTriangleDetector.cpp | 152 +- .../SphereTriangleDetector.h | 24 +- .../btActivatingCollisionAlgorithm.cpp | 28 +- .../btActivatingCollisionAlgorithm.h | 12 +- .../btBox2dBox2dCollisionAlgorithm.cpp | 106 +- .../btBox2dBox2dCollisionAlgorithm.h | 25 +- .../btBoxBoxCollisionAlgorithm.cpp | 30 +- .../btBoxBoxCollisionAlgorithm.h | 25 +- .../CollisionDispatch/btBoxBoxDetector.cpp | 1187 +- .../CollisionDispatch/btBoxBoxDetector.h | 12 +- .../btCollisionConfiguration.h | 12 +- .../CollisionDispatch/btCollisionCreateFunc.h | 10 +- .../btCollisionDispatcher.cpp | 201 +- .../CollisionDispatch/btCollisionDispatcher.h | 92 +- .../btCollisionDispatcherMt.cpp | 216 +- .../btCollisionDispatcherMt.h | 16 +- .../CollisionDispatch/btCollisionObject.cpp | 97 +- .../CollisionDispatch/btCollisionObject.h | 397 +- .../btCollisionObjectWrapper.h | 26 +- .../CollisionDispatch/btCollisionWorld.cpp | 1400 +- .../CollisionDispatch/btCollisionWorld.h | 345 +- .../btCollisionWorldImporter.cpp | 866 +- .../btCollisionWorldImporter.h | 98 +- .../btCompoundCollisionAlgorithm.cpp | 255 +- .../btCompoundCollisionAlgorithm.h | 45 +- .../btCompoundCompoundCollisionAlgorithm.cpp | 327 +- .../btCompoundCompoundCollisionAlgorithm.h | 47 +- .../btConvex2dConvex2dAlgorithm.cpp | 99 +- .../btConvex2dConvex2dAlgorithm.h | 49 +- .../btConvexConcaveCollisionAlgorithm.cpp | 256 +- .../btConvexConcaveCollisionAlgorithm.h | 74 +- .../btConvexConvexAlgorithm.cpp | 947 +- .../btConvexConvexAlgorithm.h | 46 +- .../btConvexPlaneCollisionAlgorithm.cpp | 96 +- .../btConvexPlaneCollisionAlgorithm.h | 46 +- .../btDefaultCollisionConfiguration.cpp | 217 +- .../btDefaultCollisionConfiguration.h | 87 +- .../btEmptyCollisionAlgorithm.cpp | 8 +- .../btEmptyCollisionAlgorithm.h | 18 +- .../CollisionDispatch/btGhostObject.cpp | 97 +- .../CollisionDispatch/btGhostObject.h | 79 +- .../btHashedSimplePairCache.cpp | 90 +- .../btHashedSimplePairCache.h | 105 +- .../btInternalEdgeUtility.cpp | 767 +- .../CollisionDispatch/btInternalEdgeUtility.h | 18 +- .../CollisionDispatch/btManifoldResult.cpp | 171 +- .../CollisionDispatch/btManifoldResult.h | 85 +- .../btSimulationIslandManager.cpp | 276 +- .../btSimulationIslandManager.h | 38 +- .../btSphereBoxCollisionAlgorithm.cpp | 89 +- .../btSphereBoxCollisionAlgorithm.h | 38 +- .../btSphereSphereCollisionAlgorithm.cpp | 36 +- .../btSphereSphereCollisionAlgorithm.h | 26 +- .../btSphereTriangleCollisionAlgorithm.cpp | 32 +- .../btSphereTriangleCollisionAlgorithm.h | 30 +- .../CollisionDispatch/btUnionFind.cpp | 50 +- .../CollisionDispatch/btUnionFind.h | 156 +- .../CollisionShapes/btBox2dShape.cpp | 25 +- .../CollisionShapes/btBox2dShape.h | 347 +- .../CollisionShapes/btBoxShape.cpp | 30 +- .../CollisionShapes/btBoxShape.h | 315 +- .../btBvhTriangleMeshShape.cpp | 328 +- .../CollisionShapes/btBvhTriangleMeshShape.h | 87 +- .../CollisionShapes/btCapsuleShape.cpp | 81 +- .../CollisionShapes/btCapsuleShape.h | 110 +- .../CollisionShapes/btCollisionMargin.h | 7 +- .../CollisionShapes/btCollisionShape.cpp | 57 +- .../CollisionShapes/btCollisionShape.h | 89 +- .../CollisionShapes/btCompoundShape.cpp | 194 +- .../CollisionShapes/btCompoundShape.h | 99 +- .../CollisionShapes/btConcaveShape.cpp | 3 - .../CollisionShapes/btConcaveShape.h | 20 +- .../CollisionShapes/btConeShape.cpp | 107 +- .../CollisionShapes/btConeShape.h | 143 +- .../CollisionShapes/btConvex2dShape.cpp | 46 +- .../CollisionShapes/btConvex2dShape.h | 55 +- .../CollisionShapes/btConvexHullShape.cpp | 150 +- .../CollisionShapes/btConvexHullShape.h | 61 +- .../CollisionShapes/btConvexInternalShape.cpp | 124 +- .../CollisionShapes/btConvexInternalShape.h | 123 +- .../btConvexPointCloudShape.cpp | 94 +- .../CollisionShapes/btConvexPointCloudShape.h | 46 +- .../CollisionShapes/btConvexPolyhedron.cpp | 193 +- .../CollisionShapes/btConvexPolyhedron.h | 46 +- .../CollisionShapes/btConvexShape.cpp | 673 +- .../CollisionShapes/btConvexShape.h | 56 +- .../btConvexTriangleMeshShape.cpp | 298 +- .../btConvexTriangleMeshShape.h | 59 +- .../CollisionShapes/btCylinderShape.cpp | 225 +- .../CollisionShapes/btCylinderShape.h | 131 +- .../CollisionShapes/btEmptyShape.cpp | 18 +- .../CollisionShapes/btEmptyShape.h | 31 +- .../btHeightfieldTerrainShape.cpp | 758 +- .../btHeightfieldTerrainShape.h | 115 +- .../CollisionShapes/btMaterial.h | 19 +- .../CollisionShapes/btMiniSDF.cpp | 522 + .../CollisionShapes/btMiniSDF.h | 127 + .../CollisionShapes/btMinkowskiSumShape.cpp | 50 +- .../CollisionShapes/btMinkowskiSumShape.h | 45 +- .../CollisionShapes/btMultiSphereShape.cpp | 134 +- .../CollisionShapes/btMultiSphereShape.h | 56 +- .../btMultimaterialTriangleMeshShape.cpp | 36 +- .../btMultimaterialTriangleMeshShape.h | 138 +- .../CollisionShapes/btOptimizedBvh.cpp | 302 +- .../CollisionShapes/btOptimizedBvh.h | 29 +- .../btPolyhedralConvexShape.cpp | 421 +- .../CollisionShapes/btPolyhedralConvexShape.h | 74 +- .../btScaledBvhTriangleMeshShape.cpp | 62 +- .../btScaledBvhTriangleMeshShape.h | 53 +- .../CollisionShapes/btSdfCollisionShape.cpp | 92 + .../CollisionShapes/btSdfCollisionShape.h | 29 + .../CollisionShapes/btShapeHull.cpp | 421 +- .../CollisionShapes/btShapeHull.h | 23 +- .../CollisionShapes/btSphereShape.cpp | 36 +- .../CollisionShapes/btSphereShape.h | 37 +- .../CollisionShapes/btStaticPlaneShape.cpp | 58 +- .../CollisionShapes/btStaticPlaneShape.h | 72 +- .../btStridingMeshInterface.cpp | 370 +- .../CollisionShapes/btStridingMeshInterface.h | 137 +- .../CollisionShapes/btTetrahedronShape.cpp | 179 +- .../CollisionShapes/btTetrahedronShape.h | 45 +- .../CollisionShapes/btTriangleBuffer.cpp | 25 +- .../CollisionShapes/btTriangleBuffer.h | 32 +- .../CollisionShapes/btTriangleCallback.cpp | 4 - .../CollisionShapes/btTriangleCallback.h | 9 +- .../btTriangleIndexVertexArray.cpp | 41 +- .../btTriangleIndexVertexArray.h | 109 +- .../btTriangleIndexVertexMaterialArray.cpp | 82 +- .../btTriangleIndexVertexMaterialArray.h | 77 +- .../CollisionShapes/btTriangleInfoMap.h | 122 +- .../CollisionShapes/btTriangleMesh.cpp | 96 +- .../CollisionShapes/btTriangleMesh.h | 65 +- .../CollisionShapes/btTriangleMeshShape.cpp | 106 +- .../CollisionShapes/btTriangleMeshShape.h | 36 +- .../CollisionShapes/btTriangleShape.h | 135 +- .../CollisionShapes/btUniformScalingShape.cpp | 118 +- .../CollisionShapes/btUniformScalingShape.h | 61 +- .../BulletCollision/Gimpact/btBoxCollision.h | 475 +- .../BulletCollision/Gimpact/btClipPolygon.h | 139 +- .../Gimpact/btCompoundFromGimpact.h | 150 +- .../Gimpact/btContactProcessing.cpp | 116 +- .../Gimpact/btContactProcessing.h | 20 +- .../Gimpact/btContactProcessingStructs.h | 88 +- .../BulletCollision/Gimpact/btGImpactBvh.cpp | 286 +- .../BulletCollision/Gimpact/btGImpactBvh.h | 109 +- .../Gimpact/btGImpactBvhStructs.h | 32 +- .../Gimpact/btGImpactCollisionAlgorithm.cpp | 648 +- .../Gimpact/btGImpactCollisionAlgorithm.h | 246 +- .../Gimpact/btGImpactMassUtil.h | 34 +- .../Gimpact/btGImpactQuantizedBvh.cpp | 304 +- .../Gimpact/btGImpactQuantizedBvh.h | 121 +- .../Gimpact/btGImpactQuantizedBvhStructs.h | 28 +- .../Gimpact/btGImpactShape.cpp | 198 +- .../BulletCollision/Gimpact/btGImpactShape.h | 655 +- .../Gimpact/btGenericPoolAllocator.cpp | 125 +- .../Gimpact/btGenericPoolAllocator.h | 52 +- .../Gimpact/btGeometryOperations.h | 214 +- .../BulletCollision/Gimpact/btQuantization.h | 43 +- .../Gimpact/btTriangleShapeEx.cpp | 239 +- .../Gimpact/btTriangleShapeEx.h | 71 +- .../src/BulletCollision/Gimpact/gim_array.h | 300 +- .../Gimpact/gim_basic_geometry_operations.h | 530 +- .../src/BulletCollision/Gimpact/gim_bitset.h | 50 +- .../Gimpact/gim_box_collision.h | 401 +- .../BulletCollision/Gimpact/gim_box_set.cpp | 88 +- .../src/BulletCollision/Gimpact/gim_box_set.h | 274 +- .../Gimpact/gim_clip_polygon.h | 181 +- .../BulletCollision/Gimpact/gim_contact.cpp | 74 +- .../src/BulletCollision/Gimpact/gim_contact.h | 122 +- .../BulletCollision/Gimpact/gim_geom_types.h | 7 +- .../BulletCollision/Gimpact/gim_geometry.h | 2 +- .../BulletCollision/Gimpact/gim_hash_table.h | 1227 +- .../BulletCollision/Gimpact/gim_linear_math.h | 2263 +- .../src/BulletCollision/Gimpact/gim_math.h | 107 +- .../BulletCollision/Gimpact/gim_memory.cpp | 69 +- .../src/BulletCollision/Gimpact/gim_memory.h | 165 +- .../src/BulletCollision/Gimpact/gim_pair.h | 28 + .../BulletCollision/Gimpact/gim_radixsort.h | 214 +- .../Gimpact/gim_tri_collision.cpp | 321 +- .../Gimpact/gim_tri_collision.h | 260 +- .../btComputeGjkEpaPenetration.h | 639 +- .../btContinuousConvexCollision.cpp | 148 +- .../btContinuousConvexCollision.h | 36 +- .../NarrowPhaseCollision/btConvexCast.h | 71 +- .../btConvexPenetrationDepthSolver.h | 21 +- .../btDiscreteCollisionDetectorInterface.h | 57 +- .../btGjkCollisionDescription.h | 32 +- .../NarrowPhaseCollision/btGjkConvexCast.cpp | 76 +- .../NarrowPhaseCollision/btGjkConvexCast.h | 26 +- .../NarrowPhaseCollision/btGjkEpa2.cpp | 1853 +- .../NarrowPhaseCollision/btGjkEpa2.h | 62 +- .../NarrowPhaseCollision/btGjkEpa3.h | 1836 +- .../btGjkEpaPenetrationDepthSolver.cpp | 71 +- .../btGjkEpaPenetrationDepthSolver.h | 25 +- .../btGjkPairDetector.cpp | 1338 +- .../NarrowPhaseCollision/btGjkPairDetector.h | 52 +- .../NarrowPhaseCollision/btManifoldPoint.h | 265 +- .../btMinkowskiPenetrationDepthSolver.cpp | 253 +- .../btMinkowskiPenetrationDepthSolver.h | 18 +- .../NarrowPhaseCollision/btMprPenetration.h | 1078 +- .../btPersistentManifold.cpp | 353 +- .../btPersistentManifold.h | 203 +- .../NarrowPhaseCollision/btPointCollector.h | 24 +- .../btPolyhedralContactClipping.cpp | 354 +- .../btPolyhedralContactClipping.h | 17 +- .../btRaycastCallback.cpp | 135 +- .../NarrowPhaseCollision/btRaycastCallback.h | 46 +- .../btSimplexSolverInterface.h | 19 +- .../btSubSimplexConvexCast.cpp | 117 +- .../btSubSimplexConvexCast.h | 23 +- .../btVoronoiSimplexSolver.cpp | 507 +- .../btVoronoiSimplexSolver.h | 154 +- .../bullet/src/BulletCollision/premake4.lua | 3 + .../bullet/src/BulletDynamics/CMakeLists.txt | 29 +- .../btCharacterControllerInterface.h | 29 +- .../btKinematicCharacterController.cpp | 364 +- .../btKinematicCharacterController.h | 116 +- .../ConstraintSolver/btBatchedConstraints.cpp | 1082 + .../ConstraintSolver/btBatchedConstraints.h | 62 + .../btConeTwistConstraint.cpp | 615 +- .../ConstraintSolver/btConeTwistConstraint.h | 264 +- .../ConstraintSolver/btConstraintSolver.h | 32 +- .../ConstraintSolver/btContactConstraint.cpp | 132 +- .../ConstraintSolver/btContactConstraint.h | 30 +- .../ConstraintSolver/btContactSolverInfo.h | 178 +- .../ConstraintSolver/btFixedConstraint.cpp | 19 +- .../ConstraintSolver/btFixedConstraint.h | 11 +- .../ConstraintSolver/btGearConstraint.cpp | 28 +- .../ConstraintSolver/btGearConstraint.h | 94 +- .../btGeneric6DofConstraint.cpp | 702 +- .../btGeneric6DofConstraint.h | 662 +- .../btGeneric6DofSpring2Constraint.cpp | 670 +- .../btGeneric6DofSpring2Constraint.h | 527 +- .../btGeneric6DofSpringConstraint.cpp | 66 +- .../btGeneric6DofSpringConstraint.h | 96 +- .../ConstraintSolver/btHinge2Constraint.cpp | 29 +- .../ConstraintSolver/btHinge2Constraint.h | 27 +- .../ConstraintSolver/btHingeConstraint.cpp | 675 +- .../ConstraintSolver/btHingeConstraint.h | 415 +- .../ConstraintSolver/btJacobianEntry.h | 103 +- .../btNNCGConstraintSolver.cpp | 262 +- .../ConstraintSolver/btNNCGConstraintSolver.h | 35 +- .../btPoint2PointConstraint.cpp | 186 +- .../btPoint2PointConstraint.h | 133 +- .../btSequentialImpulseConstraintSolver.cpp | 2849 +-- .../btSequentialImpulseConstraintSolver.h | 325 +- .../btSequentialImpulseConstraintSolverMt.cpp | 1554 ++ .../btSequentialImpulseConstraintSolverMt.h | 150 + .../ConstraintSolver/btSliderConstraint.cpp | 640 +- .../ConstraintSolver/btSliderConstraint.h | 211 +- .../btSolve2LinearConstraint.cpp | 164 +- .../btSolve2LinearConstraint.h | 80 +- .../ConstraintSolver/btSolverBody.h | 209 +- .../ConstraintSolver/btSolverConstraint.h | 64 +- .../ConstraintSolver/btTypedConstraint.cpp | 102 +- .../ConstraintSolver/btTypedConstraint.h | 227 +- .../btUniversalConstraint.cpp | 43 +- .../ConstraintSolver/btUniversalConstraint.h | 30 +- .../Dynamics/btActionInterface.h | 11 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 1019 +- .../Dynamics/btDiscreteDynamicsWorld.h | 161 +- .../Dynamics/btDiscreteDynamicsWorldMt.cpp | 344 +- .../Dynamics/btDiscreteDynamicsWorldMt.h | 129 +- .../BulletDynamics/Dynamics/btDynamicsWorld.h | 224 +- .../BulletDynamics/Dynamics/btRigidBody.cpp | 263 +- .../src/BulletDynamics/Dynamics/btRigidBody.h | 504 +- .../Dynamics/btSimpleDynamicsWorld.cpp | 116 +- .../Dynamics/btSimpleDynamicsWorld.h | 59 +- .../Dynamics/btSimulationIslandManagerMt.cpp | 964 +- .../Dynamics/btSimulationIslandManagerMt.h | 130 +- .../Featherstone/btMultiBody.cpp | 2059 +- .../BulletDynamics/Featherstone/btMultiBody.h | 945 +- .../Featherstone/btMultiBodyConstraint.cpp | 680 +- .../Featherstone/btMultiBodyConstraint.h | 163 +- .../btMultiBodyConstraintSolver.cpp | 1634 +- .../btMultiBodyConstraintSolver.h | 106 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 878 +- .../Featherstone/btMultiBodyDynamicsWorld.h | 65 +- .../btMultiBodyFixedConstraint.cpp | 212 +- .../Featherstone/btMultiBodyFixedConstraint.h | 79 +- .../btMultiBodyGearConstraint.cpp | 119 +- .../Featherstone/btMultiBodyGearConstraint.h | 88 +- .../Featherstone/btMultiBodyJointFeedback.h | 6 +- .../btMultiBodyJointLimitConstraint.cpp | 124 +- .../btMultiBodyJointLimitConstraint.h | 13 +- .../Featherstone/btMultiBodyJointMotor.cpp | 133 +- .../Featherstone/btMultiBodyJointMotor.h | 46 +- .../Featherstone/btMultiBodyLink.h | 187 +- .../Featherstone/btMultiBodyLinkCollider.h | 99 +- .../btMultiBodyMLCPConstraintSolver.cpp | 966 + .../btMultiBodyMLCPConstraintSolver.h | 187 + .../Featherstone/btMultiBodyPoint2Point.cpp | 139 +- .../Featherstone/btMultiBodyPoint2Point.h | 30 +- .../btMultiBodySliderConstraint.cpp | 250 +- .../btMultiBodySliderConstraint.h | 101 +- .../btMultiBodySolverConstraint.h | 82 +- .../btMultiBodySphericalJointMotor.cpp | 172 + .../btMultiBodySphericalJointMotor.h | 77 + .../MLCPSolvers/btDantzigLCP.cpp | 3496 +-- .../BulletDynamics/MLCPSolvers/btDantzigLCP.h | 12 +- .../MLCPSolvers/btDantzigSolver.h | 32 +- .../MLCPSolvers/btLemkeAlgorithm.cpp | 468 +- .../MLCPSolvers/btLemkeAlgorithm.h | 82 +- .../MLCPSolvers/btLemkeSolver.h | 508 +- .../MLCPSolvers/btMLCPSolver.cpp | 395 +- .../BulletDynamics/MLCPSolvers/btMLCPSolver.h | 40 +- .../MLCPSolvers/btMLCPSolverInterface.h | 4 +- .../BulletDynamics/MLCPSolvers/btPATHSolver.h | 61 +- .../MLCPSolvers/btSolveProjectedGaussSeidel.h | 65 +- .../Vehicle/btRaycastVehicle.cpp | 556 +- .../BulletDynamics/Vehicle/btRaycastVehicle.h | 156 +- .../Vehicle/btVehicleRaycaster.h | 20 +- .../BulletDynamics/Vehicle/btWheelInfo.cpp | 21 +- .../src/BulletDynamics/Vehicle/btWheelInfo.h | 103 +- .../bullet/src/BulletDynamics/premake4.lua | 3 + .../src/BulletInverseDynamics/IDConfig.hpp | 26 +- .../BulletInverseDynamics/IDConfigBuiltin.hpp | 19 +- .../BulletInverseDynamics/IDConfigEigen.hpp | 3 +- .../BulletInverseDynamics/IDErrorMessages.hpp | 22 +- .../src/BulletInverseDynamics/IDMath.cpp | 367 +- .../src/BulletInverseDynamics/IDMath.hpp | 15 +- .../BulletInverseDynamics/MultiBodyTree.cpp | 381 +- .../BulletInverseDynamics/MultiBodyTree.hpp | 94 +- .../details/IDEigenInterface.hpp | 23 +- .../details/IDLinearMathInterface.hpp | 164 +- .../details/IDMatVec.hpp | 326 +- .../details/MultiBodyTreeImpl.cpp | 956 +- .../details/MultiBodyTreeImpl.hpp | 93 +- .../details/MultiBodyTreeInitCache.cpp | 72 +- .../details/MultiBodyTreeInitCache.hpp | 14 +- .../src/BulletInverseDynamics/premake4.lua | 3 + .../btDefaultSoftBodySolver.cpp | 69 +- .../BulletSoftBody/btDefaultSoftBodySolver.h | 27 +- .../bullet/src/BulletSoftBody/btSoftBody.cpp | 3726 ++-- .../bullet/src/BulletSoftBody/btSoftBody.h | 1422 +- .../btSoftBodyConcaveCollisionAlgorithm.cpp | 198 +- .../btSoftBodyConcaveCollisionAlgorithm.h | 70 +- .../src/BulletSoftBody/btSoftBodyData.h | 307 +- .../src/BulletSoftBody/btSoftBodyHelpers.cpp | 1288 +- .../src/BulletSoftBody/btSoftBodyHelpers.h | 231 +- .../src/BulletSoftBody/btSoftBodyInternals.h | 1108 +- ...oftBodyRigidBodyCollisionConfiguration.cpp | 73 +- ...tSoftBodyRigidBodyCollisionConfiguration.h | 21 +- .../btSoftBodySolverVertexBuffer.h | 13 +- .../src/BulletSoftBody/btSoftBodySolvers.h | 34 +- .../btSoftMultiBodyDynamicsWorld.cpp | 243 +- .../btSoftMultiBodyDynamicsWorld.h | 67 +- .../btSoftRigidCollisionAlgorithm.cpp | 35 +- .../btSoftRigidCollisionAlgorithm.h | 28 +- .../btSoftRigidDynamicsWorld.cpp | 237 +- .../BulletSoftBody/btSoftRigidDynamicsWorld.h | 67 +- .../btSoftSoftCollisionAlgorithm.cpp | 12 +- .../btSoftSoftCollisionAlgorithm.h | 28 +- .../bullet/src/BulletSoftBody/btSparseSDF.h | 378 +- .../bullet/src/BulletSoftBody/premake4.lua | 3 + .../lib/bullet/src/LinearMath/CMakeLists.txt | 4 + .../TaskScheduler/btTaskScheduler.cpp | 792 + .../TaskScheduler/btThreadSupportInterface.h | 64 + .../TaskScheduler/btThreadSupportPosix.cpp | 353 + .../TaskScheduler/btThreadSupportWin32.cpp | 452 + .../lib/bullet/src/LinearMath/btAabbUtil2.h | 173 +- .../src/LinearMath/btAlignedAllocator.cpp | 278 +- .../src/LinearMath/btAlignedAllocator.h | 100 +- .../src/LinearMath/btAlignedObjectArray.h | 724 +- .../bullet/src/LinearMath/btConvexHull.cpp | 947 +- .../lib/bullet/src/LinearMath/btConvexHull.h | 178 +- .../src/LinearMath/btConvexHullComputer.cpp | 1786 +- .../src/LinearMath/btConvexHullComputer.h | 100 +- .../src/LinearMath/btCpuFeatureUtility.h | 38 +- .../src/LinearMath/btDefaultMotionState.h | 28 +- .../bullet/src/LinearMath/btGeometryUtil.cpp | 113 +- .../bullet/src/LinearMath/btGeometryUtil.h | 22 +- .../src/LinearMath/btGrahamScan2dConvexHull.h | 91 +- Engine/lib/bullet/src/LinearMath/btHashMap.h | 268 +- .../lib/bullet/src/LinearMath/btIDebugDraw.h | 352 +- Engine/lib/bullet/src/LinearMath/btList.h | 86 +- .../lib/bullet/src/LinearMath/btMatrix3x3.h | 1540 +- Engine/lib/bullet/src/LinearMath/btMatrixX.h | 392 +- Engine/lib/bullet/src/LinearMath/btMinMax.h | 32 +- .../lib/bullet/src/LinearMath/btMotionState.h | 24 +- .../src/LinearMath/btPolarDecomposition.cpp | 126 +- .../src/LinearMath/btPolarDecomposition.h | 29 +- .../bullet/src/LinearMath/btPoolAllocator.h | 96 +- Engine/lib/bullet/src/LinearMath/btQuadWord.h | 244 +- .../lib/bullet/src/LinearMath/btQuaternion.h | 873 +- .../lib/bullet/src/LinearMath/btQuickprof.cpp | 627 +- .../lib/bullet/src/LinearMath/btQuickprof.h | 211 +- Engine/lib/bullet/src/LinearMath/btRandom.h | 13 +- Engine/lib/bullet/src/LinearMath/btScalar.h | 24 +- .../bullet/src/LinearMath/btSerializer.cpp | 1135 +- .../lib/bullet/src/LinearMath/btSerializer.h | 1267 +- .../bullet/src/LinearMath/btSerializer64.cpp | 1134 +- .../bullet/src/LinearMath/btSpatialAlgebra.h | 248 +- .../lib/bullet/src/LinearMath/btStackAlloc.h | 96 +- .../lib/bullet/src/LinearMath/btThreads.cpp | 854 +- Engine/lib/bullet/src/LinearMath/btThreads.h | 120 +- .../lib/bullet/src/LinearMath/btTransform.h | 217 +- .../bullet/src/LinearMath/btTransformUtil.h | 158 +- .../lib/bullet/src/LinearMath/btVector3.cpp | 3014 ++- Engine/lib/bullet/src/LinearMath/btVector3.h | 1066 +- Engine/lib/bullet/src/LinearMath/premake4.lua | 7 +- .../lib/bullet/src/btBulletCollisionAll.cpp | 96 + .../lib/bullet/src/btBulletCollisionCommon.h | 4 +- Engine/lib/bullet/src/btBulletDynamicsAll.cpp | 42 + .../lib/bullet/src/btBulletDynamicsCommon.h | 10 +- Engine/lib/bullet/src/btLinearMathAll.cpp | 14 + Engine/lib/bullet/src/clew/clew.c | 582 +- Engine/lib/bullet/src/clew/clew.h | 4257 ++-- Engine/lib/openal-soft/Alc/ALc.c | 219 +- Engine/lib/openal-soft/Alc/ALu.c | 208 +- Engine/lib/openal-soft/Alc/alconfig.c | 23 + Engine/lib/openal-soft/Alc/backends/alsa.c | 28 +- Engine/lib/openal-soft/Alc/backends/base.c | 1 + Engine/lib/openal-soft/Alc/backends/base.h | 16 +- .../lib/openal-soft/Alc/backends/coreaudio.c | 22 +- Engine/lib/openal-soft/Alc/backends/dsound.c | 19 +- Engine/lib/openal-soft/Alc/backends/jack.c | 4 +- .../lib/openal-soft/Alc/backends/loopback.c | 4 +- Engine/lib/openal-soft/Alc/backends/null.c | 7 +- Engine/lib/openal-soft/Alc/backends/opensl.c | 142 +- Engine/lib/openal-soft/Alc/backends/oss.c | 33 +- .../lib/openal-soft/Alc/backends/portaudio.c | 9 +- .../lib/openal-soft/Alc/backends/pulseaudio.c | 21 +- Engine/lib/openal-soft/Alc/backends/qsa.c | 31 +- Engine/lib/openal-soft/Alc/backends/sdl2.c | 9 +- Engine/lib/openal-soft/Alc/backends/sndio.c | 366 +- Engine/lib/openal-soft/Alc/backends/solaris.c | 6 +- Engine/lib/openal-soft/Alc/backends/wasapi.c | 19 +- Engine/lib/openal-soft/Alc/backends/wave.c | 6 +- Engine/lib/openal-soft/Alc/backends/winmm.c | 24 +- Engine/lib/openal-soft/Alc/bformatdec.c | 10 +- Engine/lib/openal-soft/Alc/compat.h | 8 - Engine/lib/openal-soft/Alc/effects/autowah.c | 321 + Engine/lib/openal-soft/Alc/effects/chorus.c | 4 +- .../lib/openal-soft/Alc/effects/compressor.c | 119 +- .../lib/openal-soft/Alc/effects/dedicated.c | 2 +- .../lib/openal-soft/Alc/effects/distortion.c | 3 +- Engine/lib/openal-soft/Alc/effects/echo.c | 4 +- .../lib/openal-soft/Alc/effects/equalizer.c | 18 +- Engine/lib/openal-soft/Alc/effects/fshifter.c | 329 + .../lib/openal-soft/Alc/effects/modulator.c | 58 +- Engine/lib/openal-soft/Alc/effects/pshifter.c | 153 +- Engine/lib/openal-soft/Alc/effects/reverb.c | 1176 +- Engine/lib/openal-soft/Alc/helpers.c | 45 +- Engine/lib/openal-soft/Alc/hrtf.c | 56 +- Engine/lib/openal-soft/Alc/hrtf.h | 11 +- Engine/lib/openal-soft/Alc/inprogext.h | 8 + Engine/lib/openal-soft/Alc/mastering.c | 634 +- Engine/lib/openal-soft/Alc/mastering.h | 72 +- Engine/lib/openal-soft/Alc/mixer/defs.h | 2 +- Engine/lib/openal-soft/Alc/mixer/hrtf_inc.c | 39 +- Engine/lib/openal-soft/Alc/mixer/mixer_c.c | 99 +- Engine/lib/openal-soft/Alc/mixer/mixer_neon.c | 152 +- Engine/lib/openal-soft/Alc/mixer/mixer_sse.c | 100 +- Engine/lib/openal-soft/Alc/mixer/mixer_sse2.c | 30 +- .../lib/openal-soft/Alc/mixer/mixer_sse41.c | 33 +- Engine/lib/openal-soft/Alc/mixvoice.c | 37 +- Engine/lib/openal-soft/Alc/panning.c | 80 +- Engine/lib/openal-soft/CMakeLists.txt | 62 +- Engine/lib/openal-soft/COPYING | 60 +- Engine/lib/openal-soft/ChangeLog | 23 +- .../OpenAL32/Include/alAuxEffectSlot.h | 6 +- .../openal-soft/OpenAL32/Include/alEffect.h | 19 +- .../lib/openal-soft/OpenAL32/Include/alMain.h | 156 +- Engine/lib/openal-soft/OpenAL32/Include/alu.h | 88 +- .../openal-soft/OpenAL32/alAuxEffectSlot.c | 18 +- Engine/lib/openal-soft/OpenAL32/alEffect.c | 15 + Engine/lib/openal-soft/OpenAL32/alFilter.c | 9 +- Engine/lib/openal-soft/OpenAL32/alSource.c | 130 +- Engine/lib/openal-soft/OpenAL32/event.c | 63 +- Engine/lib/openal-soft/appveyor.yml | 2 +- Engine/lib/openal-soft/common/alcomplex.c | 92 + Engine/lib/openal-soft/common/alcomplex.h | 71 + Engine/lib/openal-soft/common/math_defs.h | 23 +- Engine/lib/openal-soft/common/threads.c | 6 +- Engine/lib/openal-soft/config.h.in | 3 + Engine/lib/sdl/Android.mk | 25 +- Engine/lib/sdl/CMakeLists.txt | 96 +- Engine/lib/sdl/Makefile.in | 3 +- Engine/lib/sdl/Makefile.minimal | 2 + Engine/lib/sdl/Makefile.os2 | 126 + Engine/lib/sdl/Makefile.pandora | 41 +- Engine/lib/sdl/Makefile.psp | 2 + Engine/lib/sdl/Makefile.wiz | 35 +- Engine/lib/sdl/SDL2.spec | 2 +- Engine/lib/sdl/WhatsNew.txt | 41 + Engine/lib/sdl/acinclude/ax_gcc_x86_cpuid.m4 | 79 + Engine/lib/sdl/build-scripts/install-sh | 532 +- Engine/lib/sdl/build-scripts/ltmain.sh | 7 +- Engine/lib/sdl/build-scripts/mkinstalldirs | 173 +- Engine/lib/sdl/build-scripts/winrtbuild.ps1 | 2 +- Engine/lib/sdl/cmake/sdlchecks.cmake | 58 +- Engine/lib/sdl/configure | 349 +- Engine/lib/sdl/configure.in | 235 +- Engine/lib/sdl/debian/changelog | 6 + Engine/lib/sdl/include/SDL.h | 4 +- Engine/lib/sdl/include/SDL_audio.h | 3 +- Engine/lib/sdl/include/SDL_config.h | 4 +- Engine/lib/sdl/include/SDL_config.h.cmake | 12 + Engine/lib/sdl/include/SDL_config.h.in | 11 + Engine/lib/sdl/include/SDL_config_android.h | 6 + Engine/lib/sdl/include/SDL_config_iphoneos.h | 10 + Engine/lib/sdl/include/SDL_config_macosx.h | 6 + Engine/lib/sdl/include/SDL_config_minimal.h | 3 + Engine/lib/sdl/include/SDL_config_os2.h | 170 + Engine/lib/sdl/include/SDL_config_pandora.h | 3 + Engine/lib/sdl/include/SDL_config_psp.h | 17 +- Engine/lib/sdl/include/SDL_config_windows.h | 9 + Engine/lib/sdl/include/SDL_config_winrt.h | 10 + Engine/lib/sdl/include/SDL_config_wiz.h | 4 + Engine/lib/sdl/include/SDL_cpuinfo.h | 13 +- Engine/lib/sdl/include/SDL_events.h | 34 + Engine/lib/sdl/include/SDL_gamecontroller.h | 28 + Engine/lib/sdl/include/SDL_haptic.h | 17 +- Engine/lib/sdl/include/SDL_hints.h | 115 +- Engine/lib/sdl/include/SDL_joystick.h | 34 +- Engine/lib/sdl/include/SDL_revision.h | 4 +- Engine/lib/sdl/include/SDL_sensor.h | 251 + Engine/lib/sdl/include/SDL_stdinc.h | 48 +- Engine/lib/sdl/include/SDL_surface.h | 7 + Engine/lib/sdl/include/SDL_system.h | 32 + Engine/lib/sdl/include/SDL_syswm.h | 12 +- Engine/lib/sdl/include/SDL_thread.h | 41 +- Engine/lib/sdl/include/SDL_version.h | 2 +- Engine/lib/sdl/include/SDL_video.h | 51 +- Engine/lib/sdl/include/SDL_vulkan.h | 17 +- Engine/lib/sdl/src/SDL.c | 48 +- Engine/lib/sdl/src/SDL_assert.c | 16 +- Engine/lib/sdl/src/SDL_assert_c.h | 5 + Engine/lib/sdl/src/atomic/SDL_atomic.c | 3 +- Engine/lib/sdl/src/atomic/SDL_spinlock.c | 28 +- Engine/lib/sdl/src/audio/SDL_audio.c | 111 +- Engine/lib/sdl/src/audio/SDL_audiocvt.c | 4 +- Engine/lib/sdl/src/audio/SDL_audiodev_c.h | 6 + Engine/lib/sdl/src/audio/SDL_audiotypecvt.c | 736 +- Engine/lib/sdl/src/audio/SDL_sysaudio.h | 4 +- .../lib/sdl/src/audio/alsa/SDL_alsa_audio.c | 150 +- .../sdl/src/audio/android/SDL_androidaudio.c | 25 +- Engine/lib/sdl/src/audio/arts/SDL_artsaudio.c | 2 +- .../sdl/src/audio/coreaudio/SDL_coreaudio.h | 8 +- .../sdl/src/audio/coreaudio/SDL_coreaudio.m | 224 +- .../src/audio/directsound/SDL_directsound.c | 6 +- Engine/lib/sdl/src/audio/jack/SDL_jackaudio.c | 35 +- Engine/lib/sdl/src/audio/jack/SDL_jackaudio.h | 1 - .../sdl/src/audio/pulseaudio/SDL_pulseaudio.c | 2 +- Engine/lib/sdl/src/audio/wasapi/SDL_wasapi.c | 6 + .../sdl/src/audio/wasapi/SDL_wasapi_win32.c | 68 +- .../sdl/src/audio/wasapi/SDL_wasapi_winrt.cpp | 65 +- Engine/lib/sdl/src/audio/winmm/SDL_winmm.c | 5 +- Engine/lib/sdl/src/core/android/SDL_android.c | 430 +- Engine/lib/sdl/src/core/android/SDL_android.h | 29 +- Engine/lib/sdl/src/core/linux/SDL_dbus.c | 18 +- Engine/lib/sdl/src/core/linux/SDL_dbus.h | 9 +- Engine/lib/sdl/src/core/linux/SDL_evdev.c | 1 + Engine/lib/sdl/src/core/linux/SDL_evdev_kbd.c | 171 +- Engine/lib/sdl/src/core/linux/SDL_evdev_kbd.h | 5 + Engine/lib/sdl/src/core/linux/SDL_udev.c | 80 +- Engine/lib/sdl/src/core/linux/SDL_udev.h | 32 +- Engine/lib/sdl/src/core/windows/SDL_windows.c | 3 + .../sdl/src/core/winrt/SDL_winrtapp_xaml.cpp | 2 +- Engine/lib/sdl/src/cpuinfo/SDL_cpuinfo.c | 105 +- Engine/lib/sdl/src/cpuinfo/SDL_simd.h | 88 + Engine/lib/sdl/src/dynapi/SDL_dynapi.c | 35 +- .../lib/sdl/src/dynapi/SDL_dynapi_overrides.h | 32 + Engine/lib/sdl/src/dynapi/SDL_dynapi_procs.h | 48 + Engine/lib/sdl/src/events/SDL_displayevents.c | 60 + .../lib/sdl/src/events/SDL_displayevents_c.h | 30 + Engine/lib/sdl/src/events/SDL_events.c | 23 + Engine/lib/sdl/src/events/SDL_events_c.h | 9 + Engine/lib/sdl/src/events/SDL_mouse.c | 52 +- Engine/lib/sdl/src/events/SDL_mouse_c.h | 5 +- Engine/lib/sdl/src/events/SDL_windowevents.c | 25 +- Engine/lib/sdl/src/events/scancodes_xfree86.h | 6 + Engine/lib/sdl/src/haptic/SDL_haptic.c | 7 +- Engine/lib/sdl/src/haptic/SDL_haptic_c.h | 5 + .../sdl/src/haptic/android/SDL_syshaptic.c | 13 +- .../lib/sdl/src/haptic/linux/SDL_syshaptic.c | 12 +- .../src/haptic/windows/SDL_windowshaptic.c | 15 +- Engine/lib/sdl/src/hidapi/AUTHORS.txt | 16 + Engine/lib/sdl/src/hidapi/HACKING.txt | 15 + Engine/lib/sdl/src/hidapi/LICENSE-bsd.txt | 26 + Engine/lib/sdl/src/hidapi/LICENSE-gpl3.txt | 674 + Engine/lib/sdl/src/hidapi/LICENSE-orig.txt | 9 + Engine/lib/sdl/src/hidapi/LICENSE.txt | 13 + Engine/lib/sdl/src/hidapi/Makefile.am | 85 + Engine/lib/sdl/src/hidapi/README.txt | 339 + Engine/lib/sdl/src/hidapi/android/hid.cpp | 1159 + .../lib/sdl/src/hidapi/android/jni/Android.mk | 16 + .../sdl/src/hidapi/android/jni/Application.mk | 2 + .../sdl/src/hidapi/android/project.properties | 14 + Engine/lib/sdl/src/hidapi/bootstrap | 2 + Engine/lib/sdl/src/hidapi/configure.ac | 236 + Engine/lib/sdl/src/hidapi/doxygen/Doxyfile | 1630 ++ Engine/lib/sdl/src/hidapi/hidapi/hidapi.h | 398 + Engine/lib/sdl/src/hidapi/hidtest/Makefile.am | 20 + Engine/lib/sdl/src/hidapi/hidtest/hidtest.cpp | 194 + Engine/lib/sdl/src/hidapi/ios/Makefile-manual | 32 + Engine/lib/sdl/src/hidapi/ios/Makefile.am | 9 + Engine/lib/sdl/src/hidapi/ios/hid.m | 914 + .../lib/sdl/src/hidapi/libusb/Makefile-manual | 18 + Engine/lib/sdl/src/hidapi/libusb/Makefile.am | 27 + .../sdl/src/hidapi/libusb/Makefile.freebsd | 46 + .../lib/sdl/src/hidapi/libusb/Makefile.linux | 49 + Engine/lib/sdl/src/hidapi/libusb/hid.c | 1620 ++ Engine/lib/sdl/src/hidapi/libusb/hidusb.cpp | 3 + .../lib/sdl/src/hidapi/linux/Makefile-manual | 49 + Engine/lib/sdl/src/hidapi/linux/Makefile.am | 10 + Engine/lib/sdl/src/hidapi/linux/README.txt | 59 + Engine/lib/sdl/src/hidapi/linux/hid.c | 898 + Engine/lib/sdl/src/hidapi/linux/hid.cpp | 333 + Engine/lib/sdl/src/hidapi/linux/hidraw.cpp | 3 + Engine/lib/sdl/src/hidapi/m4/ax_pthread.m4 | 309 + Engine/lib/sdl/src/hidapi/m4/pkg.m4 | 157 + Engine/lib/sdl/src/hidapi/mac/Makefile-manual | 32 + Engine/lib/sdl/src/hidapi/mac/Makefile.am | 9 + Engine/lib/sdl/src/hidapi/mac/hid.c | 1191 + .../lib/sdl/src/hidapi/pc/hidapi-hidraw.pc.in | 10 + .../lib/sdl/src/hidapi/pc/hidapi-libusb.pc.in | 10 + Engine/lib/sdl/src/hidapi/pc/hidapi.pc.in | 10 + .../sdl/src/hidapi/testgui/Makefile-manual | 26 + Engine/lib/sdl/src/hidapi/testgui/Makefile.am | 43 + .../sdl/src/hidapi/testgui/Makefile.freebsd | 33 + .../lib/sdl/src/hidapi/testgui/Makefile.linux | 32 + .../lib/sdl/src/hidapi/testgui/Makefile.mac | 46 + .../lib/sdl/src/hidapi/testgui/Makefile.mingw | 32 + .../TestGUI.app.in/Contents/Info.plist | 28 + .../testgui/TestGUI.app.in/Contents/PkgInfo | 1 + .../Resources/English.lproj/InfoPlist.strings | Bin 0 -> 92 bytes .../Contents/Resources/Signal11.icns | Bin 0 -> 21918 bytes .../sdl/src/hidapi/testgui/copy_to_bundle.sh | 97 + .../sdl/src/hidapi/testgui/mac_support.cpp | 134 + .../lib/sdl/src/hidapi/testgui/mac_support.h | 17 + .../src/hidapi/testgui/mac_support_cocoa.m | 94 + Engine/lib/sdl/src/hidapi/testgui/start.sh | 2 + Engine/lib/sdl/src/hidapi/testgui/test.cpp | 532 + Engine/lib/sdl/src/hidapi/udev/99-hid.rules | 33 + .../sdl/src/hidapi/windows/Makefile-manual | 14 + Engine/lib/sdl/src/hidapi/windows/Makefile.am | 16 + .../lib/sdl/src/hidapi/windows/Makefile.mingw | 35 + .../src/hidapi/windows/ddk_build/hidapi.def | 17 + .../sdl/src/hidapi/windows/ddk_build/makefile | 49 + .../sdl/src/hidapi/windows/ddk_build/sources | 23 + Engine/lib/sdl/src/hidapi/windows/hid.c | 988 + .../lib/sdl/src/joystick/SDL_gamecontroller.c | 322 +- .../sdl/src/joystick/SDL_gamecontrollerdb.h | 450 +- Engine/lib/sdl/src/joystick/SDL_joystick.c | 466 +- Engine/lib/sdl/src/joystick/SDL_joystick_c.h | 46 +- Engine/lib/sdl/src/joystick/SDL_sysjoystick.h | 119 +- .../src/joystick/android/SDL_sysjoystick.c | 347 +- .../src/joystick/android/SDL_sysjoystick_c.h | 6 +- .../sdl/src/joystick/bsd/SDL_sysjoystick.c | 113 +- Engine/lib/sdl/src/joystick/controller_type.h | 427 + .../sdl/src/joystick/darwin/SDL_sysjoystick.c | 354 +- .../src/joystick/darwin/SDL_sysjoystick_c.h | 6 + .../sdl/src/joystick/dummy/SDL_sysjoystick.c | 111 +- .../src/joystick/emscripten/SDL_sysjoystick.c | 130 +- .../src/joystick/haiku/SDL_haikujoystick.cc | 90 +- .../sdl/src/joystick/hidapi/SDL_hidapi_ps4.c | 566 + .../src/joystick/hidapi/SDL_hidapi_switch.c | 905 + .../src/joystick/hidapi/SDL_hidapi_xbox360.c | 787 + .../src/joystick/hidapi/SDL_hidapi_xboxone.c | 324 + .../src/joystick/hidapi/SDL_hidapijoystick.c | 1071 + .../joystick/hidapi/SDL_hidapijoystick_c.h | 74 + .../src/joystick/iphoneos/SDL_sysjoystick.m | 283 +- .../src/joystick/iphoneos/SDL_sysjoystick_c.h | 4 +- .../sdl/src/joystick/linux/SDL_sysjoystick.c | 156 +- .../src/joystick/linux/SDL_sysjoystick_c.h | 9 + .../sdl/src/joystick/psp/SDL_sysjoystick.c | 6 - .../lib/sdl/src/joystick/sort_controllers.py | 22 +- .../src/joystick/steam/SDL_steamcontroller.h | 6 + .../src/joystick/windows/SDL_dinputjoystick.c | 541 +- .../joystick/windows/SDL_dinputjoystick_c.h | 1 + .../sdl/src/joystick/windows/SDL_mmjoystick.c | 11 +- .../joystick/windows/SDL_windowsjoystick.c | 129 +- .../joystick/windows/SDL_windowsjoystick_c.h | 8 +- .../src/joystick/windows/SDL_xinputjoystick.c | 104 +- .../joystick/windows/SDL_xinputjoystick_c.h | 1 + Engine/lib/sdl/src/libm/e_exp.c | 187 + Engine/lib/sdl/src/libm/e_rem_pio2.c | 2 +- Engine/lib/sdl/src/libm/k_rem_pio2.c | 22 +- Engine/lib/sdl/src/libm/math_libm.h | 7 + Engine/lib/sdl/src/libm/math_private.h | 7 +- Engine/lib/sdl/src/main/haiku/SDL_BApp.h | 10 +- Engine/lib/sdl/src/main/haiku/SDL_BeApp.cc | 48 +- .../sdl/src/main/windows/SDL_windows_main.c | 82 +- Engine/lib/sdl/src/main/windows/version.rc | 8 +- Engine/lib/sdl/src/power/SDL_power.c | 12 +- Engine/lib/sdl/src/power/SDL_syspower.h | 4 +- Engine/lib/sdl/src/power/linux/SDL_syspower.c | 6 +- Engine/lib/sdl/src/render/SDL_render.c | 119 +- Engine/lib/sdl/src/render/SDL_sysrender.h | 10 + Engine/lib/sdl/src/render/SDL_yuv_sw_c.h | 6 + .../sdl/src/render/direct3d/SDL_render_d3d.c | 14 +- .../src/render/direct3d11/SDL_render_d3d11.c | 17 +- .../sdl/src/render/metal/SDL_render_metal.m | 44 +- .../lib/sdl/src/render/opengl/SDL_render_gl.c | 14 +- .../sdl/src/render/opengl/SDL_shaders_gl.h | 6 + .../sdl/src/render/opengles/SDL_render_gles.c | 14 +- .../src/render/opengles2/SDL_render_gles2.c | 41 +- .../src/render/opengles2/SDL_shaders_gles2.c | 14 +- .../lib/sdl/src/render/psp/SDL_render_psp.c | 14 +- .../src/render/software/SDL_blendfillrect.h | 6 + .../sdl/src/render/software/SDL_blendline.h | 6 + .../sdl/src/render/software/SDL_blendpoint.h | 6 + .../sdl/src/render/software/SDL_drawline.h | 6 + .../sdl/src/render/software/SDL_drawpoint.h | 6 + .../sdl/src/render/software/SDL_render_sw.c | 19 +- .../sdl/src/render/software/SDL_render_sw_c.h | 5 + .../lib/sdl/src/render/software/SDL_rotate.c | 10 +- .../lib/sdl/src/render/software/SDL_rotate.h | 4 + Engine/lib/sdl/src/sensor/SDL_sensor.c | 546 + Engine/lib/sdl/src/sensor/SDL_sensor_c.h | 44 + Engine/lib/sdl/src/sensor/SDL_syssensor.h | 105 + .../src/sensor/android/SDL_androidsensor.c | 211 + .../src/sensor/android/SDL_androidsensor.h | 31 + .../sensor/coremotion/SDL_coremotionsensor.h | 30 + .../sensor/coremotion/SDL_coremotionsensor.m | 234 + .../sdl/src/sensor/dummy/SDL_dummysensor.c | 110 + .../sdl/src/sensor/dummy/SDL_dummysensor.h | 23 + Engine/lib/sdl/src/stdlib/SDL_iconv.c | 16 +- Engine/lib/sdl/src/stdlib/SDL_stdlib.c | 24 +- Engine/lib/sdl/src/stdlib/SDL_string.c | 132 +- Engine/lib/sdl/src/test/SDL_test_common.c | 45 +- Engine/lib/sdl/src/test/SDL_test_harness.c | 3 + Engine/lib/sdl/src/thread/SDL_thread.c | 6 +- Engine/lib/sdl/src/thread/psp/SDL_systhread.c | 2 + .../lib/sdl/src/thread/pthread/SDL_sysmutex.c | 11 +- .../sdl/src/thread/pthread/SDL_systhread.c | 102 +- .../sdl/src/thread/windows/SDL_systhread.c | 2 + Engine/lib/sdl/src/timer/SDL_timer_c.h | 6 + Engine/lib/sdl/src/video/SDL_RLEaccel_c.h | 7 + Engine/lib/sdl/src/video/SDL_blit.h | 20 +- Engine/lib/sdl/src/video/SDL_blit_1.c | 220 +- Engine/lib/sdl/src/video/SDL_blit_A.c | 998 +- Engine/lib/sdl/src/video/SDL_blit_N.c | 53 + Engine/lib/sdl/src/video/SDL_blit_copy.h | 5 + Engine/lib/sdl/src/video/SDL_blit_slow.h | 6 + Engine/lib/sdl/src/video/SDL_egl.c | 88 +- Engine/lib/sdl/src/video/SDL_egl_c.h | 1 + Engine/lib/sdl/src/video/SDL_pixels.c | 2 +- Engine/lib/sdl/src/video/SDL_pixels_c.h | 6 + Engine/lib/sdl/src/video/SDL_rect_c.h | 6 + .../lib/sdl/src/video/SDL_shape_internals.h | 12 +- Engine/lib/sdl/src/video/SDL_surface.c | 16 +- Engine/lib/sdl/src/video/SDL_sysvideo.h | 22 +- Engine/lib/sdl/src/video/SDL_video.c | 110 +- Engine/lib/sdl/src/video/SDL_vulkan_utils.c | 8 +- Engine/lib/sdl/src/video/SDL_yuv.c | 27 +- Engine/lib/sdl/src/video/SDL_yuv_c.h | 6 + .../sdl/src/video/android/SDL_androidmouse.c | 157 +- .../sdl/src/video/android/SDL_androidmouse.h | 3 +- .../sdl/src/video/android/SDL_androidvideo.c | 33 +- .../sdl/src/video/android/SDL_androidvideo.h | 8 +- .../sdl/src/video/android/SDL_androidwindow.c | 37 +- .../lib/sdl/src/video/cocoa/SDL_cocoaevents.m | 4 +- .../sdl/src/video/cocoa/SDL_cocoametalview.h | 8 +- .../sdl/src/video/cocoa/SDL_cocoametalview.m | 63 +- .../lib/sdl/src/video/cocoa/SDL_cocoamouse.m | 10 + .../sdl/src/video/cocoa/SDL_cocoamousetap.m | 9 +- .../lib/sdl/src/video/cocoa/SDL_cocoaopengl.m | 16 +- .../lib/sdl/src/video/cocoa/SDL_cocoavideo.h | 2 +- .../lib/sdl/src/video/cocoa/SDL_cocoavideo.m | 9 + .../lib/sdl/src/video/cocoa/SDL_cocoavulkan.m | 25 +- .../lib/sdl/src/video/cocoa/SDL_cocoawindow.h | 1 + .../lib/sdl/src/video/cocoa/SDL_cocoawindow.m | 38 +- .../src/video/directfb/SDL_DirectFB_video.c | 2 +- .../sdl/src/video/dummy/SDL_nullevents_c.h | 6 + .../src/video/dummy/SDL_nullframebuffer_c.h | 6 + .../video/emscripten/SDL_emscriptenmouse.c | 1 + .../video/emscripten/SDL_emscriptenopengles.c | 4 +- Engine/lib/sdl/src/video/haiku/SDL_BWin.h | 4 +- .../lib/sdl/src/video/haiku/SDL_bclipboard.cc | 90 +- .../lib/sdl/src/video/haiku/SDL_bclipboard.h | 6 +- Engine/lib/sdl/src/video/haiku/SDL_bevents.cc | 4 +- Engine/lib/sdl/src/video/haiku/SDL_bevents.h | 2 +- .../sdl/src/video/haiku/SDL_bframebuffer.cc | 314 +- .../sdl/src/video/haiku/SDL_bframebuffer.h | 8 +- .../lib/sdl/src/video/haiku/SDL_bkeyboard.cc | 260 +- .../lib/sdl/src/video/haiku/SDL_bkeyboard.h | 8 +- Engine/lib/sdl/src/video/haiku/SDL_bmodes.cc | 406 +- Engine/lib/sdl/src/video/haiku/SDL_bmodes.h | 14 +- Engine/lib/sdl/src/video/haiku/SDL_bopengl.cc | 20 +- Engine/lib/sdl/src/video/haiku/SDL_bopengl.h | 20 +- Engine/lib/sdl/src/video/haiku/SDL_bvideo.cc | 147 +- Engine/lib/sdl/src/video/haiku/SDL_bvideo.h | 8 +- Engine/lib/sdl/src/video/haiku/SDL_bwindow.cc | 196 +- Engine/lib/sdl/src/video/haiku/SDL_bwindow.h | 40 +- .../sdl/src/video/kmsdrm/SDL_kmsdrmmouse.c | 10 +- .../sdl/src/video/kmsdrm/SDL_kmsdrmopengles.c | 24 +- .../sdl/src/video/kmsdrm/SDL_kmsdrmvideo.c | 87 +- .../sdl/src/video/kmsdrm/SDL_kmsdrmvideo.h | 1 + .../sdl/src/video/raspberry/SDL_rpivideo.c | 18 +- .../src/video/uikit/SDL_uikitappdelegate.m | 25 +- .../sdl/src/video/uikit/SDL_uikitmessagebox.m | 2 + .../sdl/src/video/uikit/SDL_uikitmetalview.m | 18 +- .../lib/sdl/src/video/uikit/SDL_uikitmodes.h | 4 + .../lib/sdl/src/video/uikit/SDL_uikitmodes.m | 56 + .../sdl/src/video/uikit/SDL_uikitopenglview.m | 2 +- .../lib/sdl/src/video/uikit/SDL_uikitvideo.m | 11 + .../lib/sdl/src/video/uikit/SDL_uikitview.m | 36 +- .../src/video/uikit/SDL_uikitviewcontroller.m | 109 +- .../sdl/src/video/wayland/SDL_waylandevents.c | 19 +- .../sdl/src/video/wayland/SDL_waylandtouch.c | 6 +- .../sdl/src/video/wayland/SDL_waylandtouch.h | 10 +- .../sdl/src/video/wayland/SDL_waylandvideo.c | 26 +- .../sdl/src/video/wayland/SDL_waylandvideo.h | 12 +- .../sdl/src/video/wayland/SDL_waylandwindow.c | 176 +- .../sdl/src/video/wayland/SDL_waylandwindow.h | 12 +- .../sdl/src/video/windows/SDL_windowsevents.c | 52 +- .../src/video/windows/SDL_windowsmessagebox.c | 469 +- .../sdl/src/video/windows/SDL_windowsmouse.c | 2 - .../sdl/src/video/windows/SDL_windowsopengl.h | 48 +- .../src/video/windows/SDL_windowstaskdialog.h | 156 + .../sdl/src/video/windows/SDL_windowsvideo.c | 11 + .../sdl/src/video/windows/SDL_windowsvulkan.c | 2 +- .../sdl/src/video/windows/SDL_windowswindow.c | 62 +- .../sdl/src/video/windows/SDL_windowswindow.h | 3 + .../sdl/src/video/x11/SDL_x11framebuffer.h | 6 + .../lib/sdl/src/video/x11/SDL_x11keyboard.c | 8 +- .../lib/sdl/src/video/x11/SDL_x11messagebox.c | 41 +- .../lib/sdl/src/video/x11/SDL_x11messagebox.h | 5 + Engine/lib/sdl/src/video/x11/SDL_x11opengl.h | 16 +- Engine/lib/sdl/src/video/x11/SDL_x11sym.h | 6 +- Engine/lib/sdl/src/video/x11/SDL_x11video.c | 1 + Engine/lib/sdl/src/video/x11/SDL_x11window.c | 22 +- Engine/lib/sdl/src/video/x11/SDL_x11window.h | 1 + .../sdl/src/video/yuv2rgb/yuv_rgb_std_func.h | 58 +- Engine/lib/sdl/test/Makefile.in | 12 +- Engine/lib/sdl/test/testcustomcursor.c | 55 +- Engine/lib/sdl/test/testgamecontroller.c | 5 + Engine/lib/sdl/test/testjoystick.c | 4 + Engine/lib/sdl/test/testplatform.c | 1 + Engine/lib/sdl/test/testresample.c | 2 +- Engine/lib/sdl/test/testsensor.c | 117 + Engine/lib/sdl/test/testthread.c | 32 + Engine/lib/sdl/test/testvulkan.c | 4 +- Engine/lib/sdl/test/testwm2.c | 3 + .../T3D/systems/render/meshRenderSystem.h | 4 +- Engine/source/renderInstance/renderProbeMgr.h | 2 +- .../shaderGen/HLSL/shaderFeatureHLSL.cpp | 7 +- .../BaseGame/game/tools/gui/profiles.ed.cs | 5 +- Templates/BaseGame/game/tools/settings.xml | 252 +- Tools/CMake/torque3d.cmake | 4 + 1102 files changed, 205083 insertions(+), 62836 deletions(-) create mode 100644 Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/CMakeLists.txt create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h create mode 100644 Engine/lib/bullet/src/Bullet3Collision/premake4.lua create mode 100644 Engine/lib/bullet/src/Bullet3Common/CMakeLists.txt create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3AlignedAllocator.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3AlignedAllocator.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3AlignedObjectArray.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3CommandLineArgs.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3FileUtils.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3HashMap.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3Logging.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3Logging.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3Matrix3x3.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3MinMax.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3PoolAllocator.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3QuadWord.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3Quaternion.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3Random.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3ResizablePool.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3Scalar.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3StackAlloc.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3Transform.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3TransformUtil.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3Vector3.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Common/b3Vector3.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/premake4.lua create mode 100644 Engine/lib/bullet/src/Bullet3Common/shared/b3Float4.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/shared/b3Int2.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/shared/b3Int4.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/shared/b3Mat3x3.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/shared/b3PlatformDefinitions.h create mode 100644 Engine/lib/bullet/src/Bullet3Common/shared/b3Quat.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/CMakeLists.txt create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/premake4.lua create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/shared/b3Inertia.h create mode 100644 Engine/lib/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h create mode 100644 Engine/lib/bullet/src/Bullet3Geometry/CMakeLists.txt create mode 100644 Engine/lib/bullet/src/Bullet3Geometry/b3AabbUtil.h create mode 100644 Engine/lib/bullet/src/Bullet3Geometry/b3ConvexHullComputer.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Geometry/b3ConvexHullComputer.h create mode 100644 Engine/lib/bullet/src/Bullet3Geometry/b3GeometryUtil.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Geometry/b3GeometryUtil.h create mode 100644 Engine/lib/bullet/src/Bullet3Geometry/b3GrahamScan2dConvexHull.h create mode 100644 Engine/lib/bullet/src/Bullet3Geometry/premake4.lua create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvh.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/CMakeLists.txt create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLInclude.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcave.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcaveKernels.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BufferInfoCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernels.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernelsCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernels.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernelsCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernels.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernelsCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanFloat4Kernels.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanKernels.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanKernelsCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanKernelsFloat4CL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/RadixSort32Kernels.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/RadixSort32KernelsCL.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.cpp create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveFriction.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h create mode 100644 Engine/lib/bullet/src/Bullet3OpenCL/premake4.lua create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/CMakeLists.txt create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.h create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.h create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Common.h create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.h create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Defines.h create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.h create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.cpp create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.h create mode 100644 Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/premake4.lua create mode 100644 Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.cpp create mode 100644 Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.h create mode 100644 Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp create mode 100644 Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h create mode 100644 Engine/lib/bullet/src/BulletCollision/Gimpact/gim_pair.h create mode 100644 Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp create mode 100644 Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h create mode 100644 Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp create mode 100644 Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h create mode 100644 Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp create mode 100644 Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h create mode 100644 Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp create mode 100644 Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h create mode 100644 Engine/lib/bullet/src/LinearMath/TaskScheduler/btTaskScheduler.cpp create mode 100644 Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportInterface.h create mode 100644 Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp create mode 100644 Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp create mode 100644 Engine/lib/bullet/src/btBulletCollisionAll.cpp create mode 100644 Engine/lib/bullet/src/btBulletDynamicsAll.cpp create mode 100644 Engine/lib/bullet/src/btLinearMathAll.cpp create mode 100644 Engine/lib/openal-soft/Alc/effects/autowah.c create mode 100644 Engine/lib/openal-soft/Alc/effects/fshifter.c create mode 100644 Engine/lib/openal-soft/common/alcomplex.c create mode 100644 Engine/lib/openal-soft/common/alcomplex.h create mode 100644 Engine/lib/sdl/Makefile.os2 create mode 100644 Engine/lib/sdl/acinclude/ax_gcc_x86_cpuid.m4 create mode 100644 Engine/lib/sdl/include/SDL_config_os2.h create mode 100644 Engine/lib/sdl/include/SDL_sensor.h create mode 100644 Engine/lib/sdl/src/cpuinfo/SDL_simd.h create mode 100644 Engine/lib/sdl/src/events/SDL_displayevents.c create mode 100644 Engine/lib/sdl/src/events/SDL_displayevents_c.h create mode 100644 Engine/lib/sdl/src/hidapi/AUTHORS.txt create mode 100644 Engine/lib/sdl/src/hidapi/HACKING.txt create mode 100644 Engine/lib/sdl/src/hidapi/LICENSE-bsd.txt create mode 100644 Engine/lib/sdl/src/hidapi/LICENSE-gpl3.txt create mode 100644 Engine/lib/sdl/src/hidapi/LICENSE-orig.txt create mode 100644 Engine/lib/sdl/src/hidapi/LICENSE.txt create mode 100644 Engine/lib/sdl/src/hidapi/Makefile.am create mode 100644 Engine/lib/sdl/src/hidapi/README.txt create mode 100644 Engine/lib/sdl/src/hidapi/android/hid.cpp create mode 100644 Engine/lib/sdl/src/hidapi/android/jni/Android.mk create mode 100644 Engine/lib/sdl/src/hidapi/android/jni/Application.mk create mode 100644 Engine/lib/sdl/src/hidapi/android/project.properties create mode 100644 Engine/lib/sdl/src/hidapi/bootstrap create mode 100644 Engine/lib/sdl/src/hidapi/configure.ac create mode 100644 Engine/lib/sdl/src/hidapi/doxygen/Doxyfile create mode 100644 Engine/lib/sdl/src/hidapi/hidapi/hidapi.h create mode 100644 Engine/lib/sdl/src/hidapi/hidtest/Makefile.am create mode 100644 Engine/lib/sdl/src/hidapi/hidtest/hidtest.cpp create mode 100644 Engine/lib/sdl/src/hidapi/ios/Makefile-manual create mode 100644 Engine/lib/sdl/src/hidapi/ios/Makefile.am create mode 100644 Engine/lib/sdl/src/hidapi/ios/hid.m create mode 100644 Engine/lib/sdl/src/hidapi/libusb/Makefile-manual create mode 100644 Engine/lib/sdl/src/hidapi/libusb/Makefile.am create mode 100644 Engine/lib/sdl/src/hidapi/libusb/Makefile.freebsd create mode 100644 Engine/lib/sdl/src/hidapi/libusb/Makefile.linux create mode 100644 Engine/lib/sdl/src/hidapi/libusb/hid.c create mode 100644 Engine/lib/sdl/src/hidapi/libusb/hidusb.cpp create mode 100644 Engine/lib/sdl/src/hidapi/linux/Makefile-manual create mode 100644 Engine/lib/sdl/src/hidapi/linux/Makefile.am create mode 100644 Engine/lib/sdl/src/hidapi/linux/README.txt create mode 100644 Engine/lib/sdl/src/hidapi/linux/hid.c create mode 100644 Engine/lib/sdl/src/hidapi/linux/hid.cpp create mode 100644 Engine/lib/sdl/src/hidapi/linux/hidraw.cpp create mode 100644 Engine/lib/sdl/src/hidapi/m4/ax_pthread.m4 create mode 100644 Engine/lib/sdl/src/hidapi/m4/pkg.m4 create mode 100644 Engine/lib/sdl/src/hidapi/mac/Makefile-manual create mode 100644 Engine/lib/sdl/src/hidapi/mac/Makefile.am create mode 100644 Engine/lib/sdl/src/hidapi/mac/hid.c create mode 100644 Engine/lib/sdl/src/hidapi/pc/hidapi-hidraw.pc.in create mode 100644 Engine/lib/sdl/src/hidapi/pc/hidapi-libusb.pc.in create mode 100644 Engine/lib/sdl/src/hidapi/pc/hidapi.pc.in create mode 100644 Engine/lib/sdl/src/hidapi/testgui/Makefile-manual create mode 100644 Engine/lib/sdl/src/hidapi/testgui/Makefile.am create mode 100644 Engine/lib/sdl/src/hidapi/testgui/Makefile.freebsd create mode 100644 Engine/lib/sdl/src/hidapi/testgui/Makefile.linux create mode 100644 Engine/lib/sdl/src/hidapi/testgui/Makefile.mac create mode 100644 Engine/lib/sdl/src/hidapi/testgui/Makefile.mingw create mode 100644 Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/Info.plist create mode 100644 Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/PkgInfo create mode 100644 Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/Resources/English.lproj/InfoPlist.strings create mode 100644 Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/Resources/Signal11.icns create mode 100644 Engine/lib/sdl/src/hidapi/testgui/copy_to_bundle.sh create mode 100644 Engine/lib/sdl/src/hidapi/testgui/mac_support.cpp create mode 100644 Engine/lib/sdl/src/hidapi/testgui/mac_support.h create mode 100644 Engine/lib/sdl/src/hidapi/testgui/mac_support_cocoa.m create mode 100644 Engine/lib/sdl/src/hidapi/testgui/start.sh create mode 100644 Engine/lib/sdl/src/hidapi/testgui/test.cpp create mode 100644 Engine/lib/sdl/src/hidapi/udev/99-hid.rules create mode 100644 Engine/lib/sdl/src/hidapi/windows/Makefile-manual create mode 100644 Engine/lib/sdl/src/hidapi/windows/Makefile.am create mode 100644 Engine/lib/sdl/src/hidapi/windows/Makefile.mingw create mode 100644 Engine/lib/sdl/src/hidapi/windows/ddk_build/hidapi.def create mode 100644 Engine/lib/sdl/src/hidapi/windows/ddk_build/makefile create mode 100644 Engine/lib/sdl/src/hidapi/windows/ddk_build/sources create mode 100644 Engine/lib/sdl/src/hidapi/windows/hid.c create mode 100644 Engine/lib/sdl/src/joystick/controller_type.h create mode 100644 Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_ps4.c create mode 100644 Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_switch.c create mode 100644 Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_xbox360.c create mode 100644 Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_xboxone.c create mode 100644 Engine/lib/sdl/src/joystick/hidapi/SDL_hidapijoystick.c create mode 100644 Engine/lib/sdl/src/joystick/hidapi/SDL_hidapijoystick_c.h create mode 100644 Engine/lib/sdl/src/libm/e_exp.c create mode 100644 Engine/lib/sdl/src/sensor/SDL_sensor.c create mode 100644 Engine/lib/sdl/src/sensor/SDL_sensor_c.h create mode 100644 Engine/lib/sdl/src/sensor/SDL_syssensor.h create mode 100644 Engine/lib/sdl/src/sensor/android/SDL_androidsensor.c create mode 100644 Engine/lib/sdl/src/sensor/android/SDL_androidsensor.h create mode 100644 Engine/lib/sdl/src/sensor/coremotion/SDL_coremotionsensor.h create mode 100644 Engine/lib/sdl/src/sensor/coremotion/SDL_coremotionsensor.m create mode 100644 Engine/lib/sdl/src/sensor/dummy/SDL_dummysensor.c create mode 100644 Engine/lib/sdl/src/sensor/dummy/SDL_dummysensor.h create mode 100644 Engine/lib/sdl/src/video/windows/SDL_windowstaskdialog.h create mode 100644 Engine/lib/sdl/test/testsensor.c diff --git a/Engine/lib/bullet/.travis.yml b/Engine/lib/bullet/.travis.yml index d2ad176f4..fbaf6d90f 100644 --- a/Engine/lib/bullet/.travis.yml +++ b/Engine/lib/bullet/.travis.yml @@ -1,27 +1,56 @@ language: cpp -os: - - linux - - osx -compiler: - - gcc - - clang -addons: - apt: - packages: - - python3 - +matrix: + include: + - os: linux + compiler: gcc + env: + - BUILD_NAME=TRUSTY_GCC + - SUDO=sudo + - os: linux + compiler: clang + env: + - BUILD_NAME=TRUSTY_CLANG + - SUDO=sudo + - os: linux + compiler: gcc + env: + - BUILD_NAME=XENIAL_GCC + - DOCKER_FILE="ubuntu-xenial" + services: docker + - os: linux + compiler: clang + env: + - BUILD_NAME=XENIAL_CLANG + - DOCKER_FILE="ubuntu-xenial" + services: docker + - os: linux + compiler: gcc + env: + - BUILD_NAME=BIONIC_GCC + - DOCKER_FILE="ubuntu-bionic" + services: docker + - os: linux + compiler: clang + env: + - BUILD_NAME=BIONIC_CLANG + - DOCKER_FILE="ubuntu-bionic" + services: docker + - os: osx + compiler: gcc + env: + - BUILD_NAME=OSX_GCC + - os: osx + compiler: clang + env: + - BUILD_NAME=OSX_CLANG +before_install: + - if [ -n "$DOCKER_FILE" ]; then + docker build -t "$DOCKER_FILE" -f ".ci/docker/$DOCKER_FILE" .; + docker run -itd -v $TRAVIS_BUILD_DIR:$TRAVIS_BUILD_DIR --env-file .ci/docker/env.list --name bullet-docker "$DOCKER_FILE"; + fi script: - - echo "CXX="$CXX - - echo "CC="$CC - - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror - - make -j8 - - ctest -j8 --output-on-failure - # Build again with double precision - - cmake . -G "Unix Makefiles" -DUSE_DOUBLE_PRECISION=ON #-DCMAKE_CXX_FLAGS=-Werror - - make -j8 - - ctest -j8 --output-on-failure - # Build again with shared libraries - - cmake . -G "Unix Makefiles" -DBUILD_SHARED_LIBS=ON - - make -j8 - - ctest -j8 --output-on-failure - - sudo make install + - if [ -n "$DOCKER_FILE" ]; then + docker exec bullet-docker /bin/sh -c "cd $TRAVIS_BUILD_DIR && ./.ci/script.sh"; + else + '.ci/script.sh'; + fi diff --git a/Engine/lib/bullet/CMakeLists.txt b/Engine/lib/bullet/CMakeLists.txt index f0e62d396..68e0a93bc 100644 --- a/Engine/lib/bullet/CMakeLists.txt +++ b/Engine/lib/bullet/CMakeLists.txt @@ -26,16 +26,16 @@ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG") OPTION(USE_DOUBLE_PRECISION "Use double precision" OFF) OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON) OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF) -OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" OFF) +OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" ON) -OPTION(BULLET2_USE_THREAD_LOCKS "Build Bullet 2 libraries with mutex locking around certain operations (required for multi-threading)" OFF) -IF (BULLET2_USE_THREAD_LOCKS) +OPTION(BULLET2_MULTITHREADING "Build Bullet 2 libraries with mutex locking around certain operations (required for multi-threading)" OFF) +IF (BULLET2_MULTITHREADING) OPTION(BULLET2_USE_OPEN_MP_MULTITHREADING "Build Bullet 2 with support for multi-threading with OpenMP (requires a compiler with OpenMP support)" OFF) OPTION(BULLET2_USE_TBB_MULTITHREADING "Build Bullet 2 with support for multi-threading with Intel Threading Building Blocks (requires the TBB library to be already installed)" OFF) IF (MSVC) OPTION(BULLET2_USE_PPL_MULTITHREADING "Build Bullet 2 with support for multi-threading with Microsoft Parallel Patterns Library (requires MSVC compiler)" OFF) ENDIF (MSVC) -ENDIF (BULLET2_USE_THREAD_LOCKS) +ENDIF (BULLET2_MULTITHREADING) IF(NOT WIN32) @@ -108,7 +108,7 @@ IF(MSVC) IF (CMAKE_CL_64) ADD_DEFINITIONS(-D_WIN64) ELSE() - OPTION(USE_MSVC_SSE "Use MSVC /arch:sse option" ON) + OPTION(USE_MSVC_SSE "Use MSVC /arch:sse option" OFF) option(USE_MSVC_SSE2 "Compile your program with SSE2 instructions" ON) IF (USE_MSVC_SSE) @@ -217,20 +217,23 @@ ADD_DEFINITIONS( -DBT_USE_DOUBLE_PRECISION) SET( BULLET_DOUBLE_DEF "-DBT_USE_DOUBLE_PRECISION") ENDIF (USE_DOUBLE_PRECISION) -IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) -ADD_DEFINITIONS(-DUSE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) -ENDIF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) +IF (NOT USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) +ADD_DEFINITIONS(-DSKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) +ENDIF () IF(USE_GRAPHICAL_BENCHMARK) ADD_DEFINITIONS( -DUSE_GRAPHICAL_BENCHMARK) ENDIF (USE_GRAPHICAL_BENCHMARK) -IF(BULLET2_USE_THREAD_LOCKS) +IF(BULLET2_MULTITHREADING) ADD_DEFINITIONS( -DBT_THREADSAFE=1 ) IF (NOT MSVC) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") ENDIF (NOT MSVC) -ENDIF (BULLET2_USE_THREAD_LOCKS) + IF (NOT WIN32) + LINK_LIBRARIES( pthread ) + ENDIF (NOT WIN32) +ENDIF (BULLET2_MULTITHREADING) IF (BULLET2_USE_OPEN_MP_MULTITHREADING) ADD_DEFINITIONS("-DBT_USE_OPENMP=1") @@ -301,25 +304,26 @@ IF (APPLE) ENDIF() OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON) + # Optional Python configuration -# builds pybullet automatically if all the requirements are met -SET(PYTHON_VERSION_PYBULLET "" CACHE STRING "Python version pybullet will use.") -SET(Python_ADDITIONAL_VERSIONS 3 3.6 3.5 3.4 3.3 3.2 3.1 3.0 2.7 2.7.12 2.7.10 2.7.3 ) -SET_PROPERTY(CACHE PYTHON_VERSION_PYBULLET PROPERTY STRINGS ${Python_ADDITIONAL_VERSIONS}) -SET(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/build3/cmake ${CMAKE_MODULE_PATH}) -OPTION(EXACT_PYTHON_VERSION "Require Python and match PYTHON_VERSION_PYBULLET exactly, e.g. 2.7.12" OFF) -IF(EXACT_PYTHON_VERSION) - set(EXACT_PYTHON_VERSION_FLAG EXACT REQUIRED) -ENDIF(EXACT_PYTHON_VERSION) -# first find the python interpreter -FIND_PACKAGE(PythonInterp ${PYTHON_VERSION_PYBULLET} ${EXACT_PYTHON_VERSION_FLAG}) -# python library should exactly match that of the interpreter -FIND_PACKAGE(PythonLibs ${PYTHON_VERSION_STRING} EXACT) -SET(DEFAULT_BUILD_PYBULLET OFF) -IF(PYTHONLIBS_FOUND) - SET(DEFAULT_BUILD_PYBULLET ON) -ENDIF(PYTHONLIBS_FOUND) -OPTION(BUILD_PYBULLET "Set when you want to build pybullet (Python bindings for Bullet)" ${DEFAULT_BUILD_PYBULLET}) +# Will not probe environment for Python configuration (which can abort the +# build process) unless you explicitly turn on BUILD_PYBULLET. +OPTION(BUILD_PYBULLET "Set when you want to build pybullet (Python bindings for Bullet)" OFF) +IF(BUILD_PYBULLET) + SET(PYTHON_VERSION_PYBULLET "" CACHE STRING "Python version pybullet will use.") + SET(Python_ADDITIONAL_VERSIONS 3 3.6 3.5 3.4 3.3 3.2 3.1 3.0 2.7 2.7.12 2.7.10 2.7.3 ) + SET_PROPERTY(CACHE PYTHON_VERSION_PYBULLET PROPERTY STRINGS ${Python_ADDITIONAL_VERSIONS}) + SET(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/build3/cmake ${CMAKE_MODULE_PATH}) + OPTION(EXACT_PYTHON_VERSION "Require Python and match PYTHON_VERSION_PYBULLET exactly, e.g. 2.7.12" OFF) + IF(EXACT_PYTHON_VERSION) + set(EXACT_PYTHON_VERSION_FLAG EXACT REQUIRED) + ENDIF(EXACT_PYTHON_VERSION) + # first find the python interpreter + FIND_PACKAGE(PythonInterp ${PYTHON_VERSION_PYBULLET} ${EXACT_PYTHON_VERSION_FLAG}) + # python library should exactly match that of the interpreter + # the following can result in fatal error if you don't have the right python configuration + FIND_PACKAGE(PythonLibs ${PYTHON_VERSION_STRING} EXACT) +ENDIF(BUILD_PYBULLET) OPTION(BUILD_ENET "Set when you want to build apps with enet UDP networking support" ON) OPTION(BUILD_CLSOCKET "Set when you want to build apps with enet TCP networking support" ON) @@ -365,6 +369,13 @@ IF(BUILD_PYBULLET) ENDIF(BUILD_PYBULLET) +IF(NOT WIN32 AND NOT APPLE) + OPTION(BUILD_EGL "Build OpenGL/EGL" ON) + IF(BUILD_EGL) + ADD_DEFINITIONS(-DBT_USE_EGL) + ENDIF(BUILD_EGL) +ENDIF() + IF(BUILD_BULLET3) IF(APPLE) MESSAGE("Mac OSX Version is ${_CURRENT_OSX_VERSION}") diff --git a/Engine/lib/bullet/README.md b/Engine/lib/bullet/README.md index 6224da8b5..6efe9e5e4 100644 --- a/Engine/lib/bullet/README.md +++ b/Engine/lib/bullet/README.md @@ -1,4 +1,3 @@ - [![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) @@ -6,7 +5,7 @@ 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 +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: @@ -80,9 +79,9 @@ Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the followin Using premake: ``` cd build3 - ./premake4_linux gmake --double - ./premake4_linux64 gmake --double - ./premake4_osx gmake --double --enable_pybullet + ./premake4_linux --double gmake + ./premake4_linux64 --double gmake + ./premake4_osx --double --enable_pybullet gmake ``` Then ``` diff --git a/Engine/lib/bullet/VERSION b/Engine/lib/bullet/VERSION index 3274853c8..663ae72bc 100644 --- a/Engine/lib/bullet/VERSION +++ b/Engine/lib/bullet/VERSION @@ -1 +1 @@ -2.87 +2.88 diff --git a/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h new file mode 100644 index 000000000..bec0800a6 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h @@ -0,0 +1,38 @@ +/* +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/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp new file mode 100644 index 000000000..a0dc1da95 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp @@ -0,0 +1,1352 @@ +/* +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; i < ni; ++i) + { + b3Merge(volume, leaves[i]->volume, 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; i < count; ++i) + { + for (int j = i + 1; j < count; ++j) + { + const b3Scalar sz = b3Size(b3Merge(leaves[i]->volume, leaves[j]->volume)); + if (sz < minsize) + { + minsize = sz; + minidx[0] = i; + minidx[1] = j; + } + } + } + b3DbvtNode* n[] = {leaves[minidx[0]], leaves[minidx[1]]}; + b3DbvtNode* p = b3CreateNode(pdbvt, 0, n[0]->volume, n[1]->volume, 0); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leaves[minidx[0]] = p; + leaves[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; i < count; ++i) + { + const b3Vector3 x = leaves[i]->volume.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 < bestmidp) + { + bestaxis = i; + bestmidp = midp; + } + } + } + if (bestaxis >= 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; (i < lookahead) && root->parent; ++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; (i < m_lkhd) && root->parent; ++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; i < nodes.nodes.size(); ++i) + { + const b3DbvtNode* n = nodes.nodes[i]; + int p = -1; + if (n->parent) p = nodes.nodes.findLinearSearch(n->parent); + if (n->isinternal()) + { + const int c0 = nodes.nodes.findLinearSearch(n->childs[0]); + const int c1 = nodes.nodes.findLinearSearch(n->childs[1]); + iwriter->WriteNode(n, i, p, c0, c1); + } + else + { + iwriter->WriteLeaf(n, i, p); + } + } +} + +// +void 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.depth < b.depth) return (+1); + if (a.depth > b.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.depth < b.depth) return (+1); + if (a.depth > b.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 < leaves; ++i) + { + dbvt.insert(RandVolume(cs, eb, es), 0); + } + } +}; + +void b3DynamicBvh::benchmark() +{ + static const b3Scalar cfgVolumeCenterScale = 100; + static const b3Scalar cfgVolumeExentsBase = 1; + static const b3Scalar cfgVolumeExentsScale = 4; + static const int cfgLeaves = 8192; + static const bool cfgEnable = true; + + //[1] b3DbvtVolume intersections + bool cfgBenchmark1_Enable = cfgEnable; + static const int cfgBenchmark1_Iterations = 8; + static const int cfgBenchmark1_Reference = 3499; + //[2] b3DbvtVolume merges + bool cfgBenchmark2_Enable = cfgEnable; + static const int cfgBenchmark2_Iterations = 4; + static const int cfgBenchmark2_Reference = 1945; + //[3] b3DynamicBvh::collideTT + bool cfgBenchmark3_Enable = cfgEnable; + static const int cfgBenchmark3_Iterations = 512; + static const int cfgBenchmark3_Reference = 5485; + //[4] b3DynamicBvh::collideTT self + bool cfgBenchmark4_Enable = cfgEnable; + static const int cfgBenchmark4_Iterations = 512; + static const int cfgBenchmark4_Reference = 2814; + //[5] b3DynamicBvh::collideTT xform + bool cfgBenchmark5_Enable = cfgEnable; + static const int cfgBenchmark5_Iterations = 512; + static const b3Scalar cfgBenchmark5_OffsetScale = 2; + static const int cfgBenchmark5_Reference = 7379; + //[6] b3DynamicBvh::collideTT xform,self + bool cfgBenchmark6_Enable = cfgEnable; + static const int cfgBenchmark6_Iterations = 512; + static const b3Scalar cfgBenchmark6_OffsetScale = 2; + static const int cfgBenchmark6_Reference = 7270; + //[7] b3DynamicBvh::rayTest + bool cfgBenchmark7_Enable = cfgEnable; + static const int cfgBenchmark7_Passes = 32; + static const int cfgBenchmark7_Iterations = 65536; + static const int cfgBenchmark7_Reference = 6307; + //[8] insert/remove + bool cfgBenchmark8_Enable = cfgEnable; + static const int cfgBenchmark8_Passes = 32; + static const int cfgBenchmark8_Iterations = 65536; + static const int cfgBenchmark8_Reference = 2105; + //[9] updates (teleport) + bool cfgBenchmark9_Enable = cfgEnable; + static const int cfgBenchmark9_Passes = 32; + static const int cfgBenchmark9_Iterations = 65536; + static const int cfgBenchmark9_Reference = 1879; + //[10] updates (jitter) + bool cfgBenchmark10_Enable = cfgEnable; + static const b3Scalar cfgBenchmark10_Scale = cfgVolumeCenterScale / 10000; + static const int cfgBenchmark10_Passes = 32; + static const int cfgBenchmark10_Iterations = 65536; + static const int cfgBenchmark10_Reference = 1244; + //[11] optimize (incremental) + bool cfgBenchmark11_Enable = cfgEnable; + static const int cfgBenchmark11_Passes = 64; + static const int cfgBenchmark11_Iterations = 65536; + static const int cfgBenchmark11_Reference = 2510; + //[12] b3DbvtVolume notequal + bool cfgBenchmark12_Enable = cfgEnable; + static const int cfgBenchmark12_Iterations = 32; + static const int cfgBenchmark12_Reference = 3677; + //[13] culling(OCL+fullsort) + bool cfgBenchmark13_Enable = cfgEnable; + static const int cfgBenchmark13_Iterations = 1024; + static const int cfgBenchmark13_Reference = 2231; + //[14] culling(OCL+qsort) + bool cfgBenchmark14_Enable = cfgEnable; + static const int cfgBenchmark14_Iterations = 8192; + static const int cfgBenchmark14_Reference = 3500; + //[15] culling(KDOP+qsort) + bool cfgBenchmark15_Enable = cfgEnable; + static const int cfgBenchmark15_Iterations = 8192; + static const int cfgBenchmark15_Reference = 1151; + //[16] insert/remove batch + bool cfgBenchmark16_Enable = cfgEnable; + static const int cfgBenchmark16_BatchCount = 256; + static const int cfgBenchmark16_Passes = 16384; + static const int cfgBenchmark16_Reference = 5138; + //[17] select + bool cfgBenchmark17_Enable = cfgEnable; + static const int cfgBenchmark17_Iterations = 4; + static const int cfgBenchmark17_Reference = 3390; + + b3Clock wallclock; + printf("Benchmarking dbvt...\r\n"); + printf("\tWorld scale: %f\r\n", cfgVolumeCenterScale); + printf("\tExtents base: %f\r\n", cfgVolumeExentsBase); + printf("\tExtents range: %f\r\n", cfgVolumeExentsScale); + printf("\tLeaves: %u\r\n", cfgLeaves); + printf("\tsizeof(b3DbvtVolume): %u bytes\r\n", sizeof(b3DbvtVolume)); + printf("\tsizeof(b3DbvtNode): %u bytes\r\n", sizeof(b3DbvtNode)); + if (cfgBenchmark1_Enable) + { // Benchmark 1 + srand(380843); + b3AlignedObjectArray volumes; + b3AlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for (int i = 0; i < cfgLeaves; ++i) + { + volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale); + } + printf("[1] b3DbvtVolume intersections: "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark1_Iterations; ++i) + { + for (int j = 0; j < cfgLeaves; ++j) + { + for (int k = 0; k < cfgLeaves; ++k) + { + results[k] = Intersect(volumes[j], volumes[k]); + } + } + } + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark1_Reference) * 100 / time); + } + if (cfgBenchmark2_Enable) + { // Benchmark 2 + srand(380843); + b3AlignedObjectArray volumes; + b3AlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for (int i = 0; i < cfgLeaves; ++i) + { + volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale); + } + printf("[2] b3DbvtVolume merges: "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark2_Iterations; ++i) + { + for (int j = 0; j < cfgLeaves; ++j) + { + for (int k = 0; k < cfgLeaves; ++k) + { + Merge(volumes[j], volumes[k], results[k]); + } + } + } + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark2_Reference) * 100 / time); + } + if (cfgBenchmark3_Enable) + { // Benchmark 3 + srand(380843); + b3DynamicBvh dbvt[2]; + b3DbvtBenchmark::NilPolicy policy; + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[0]); + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[1]); + dbvt[0].optimizeTopDown(); + dbvt[1].optimizeTopDown(); + printf("[3] b3DynamicBvh::collideTT: "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark3_Iterations; ++i) + { + b3DynamicBvh::collideTT(dbvt[0].m_root, dbvt[1].m_root, policy); + } + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark3_Reference) * 100 / time); + } + if (cfgBenchmark4_Enable) + { // Benchmark 4 + srand(380843); + b3DynamicBvh dbvt; + b3DbvtBenchmark::NilPolicy policy; + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); + dbvt.optimizeTopDown(); + printf("[4] b3DynamicBvh::collideTT self: "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark4_Iterations; ++i) + { + b3DynamicBvh::collideTT(dbvt.m_root, dbvt.m_root, policy); + } + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark4_Reference) * 100 / time); + } + if (cfgBenchmark5_Enable) + { // Benchmark 5 + srand(380843); + b3DynamicBvh dbvt[2]; + b3AlignedObjectArray transforms; + b3DbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark5_Iterations); + for (int i = 0; i < transforms.size(); ++i) + { + transforms[i] = b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark5_OffsetScale); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[0]); + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[1]); + dbvt[0].optimizeTopDown(); + dbvt[1].optimizeTopDown(); + printf("[5] b3DynamicBvh::collideTT xform: "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark5_Iterations; ++i) + { + b3DynamicBvh::collideTT(dbvt[0].m_root, dbvt[1].m_root, transforms[i], policy); + } + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark5_Reference) * 100 / time); + } + if (cfgBenchmark6_Enable) + { // Benchmark 6 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray transforms; + b3DbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark6_Iterations); + for (int i = 0; i < transforms.size(); ++i) + { + transforms[i] = b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark6_OffsetScale); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); + dbvt.optimizeTopDown(); + printf("[6] b3DynamicBvh::collideTT xform,self: "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark6_Iterations; ++i) + { + b3DynamicBvh::collideTT(dbvt.m_root, dbvt.m_root, transforms[i], policy); + } + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark6_Reference) * 100 / time); + } + if (cfgBenchmark7_Enable) + { // Benchmark 7 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray rayorg; + b3AlignedObjectArray raydir; + b3DbvtBenchmark::NilPolicy policy; + rayorg.resize(cfgBenchmark7_Iterations); + raydir.resize(cfgBenchmark7_Iterations); + for (int i = 0; i < rayorg.size(); ++i) + { + rayorg[i] = b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2); + raydir[i] = b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); + dbvt.optimizeTopDown(); + printf("[7] b3DynamicBvh::rayTest: "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark7_Passes; ++i) + { + for (int j = 0; j < cfgBenchmark7_Iterations; ++j) + { + b3DynamicBvh::rayTest(dbvt.m_root, rayorg[j], rayorg[j] + raydir[j], policy); + } + } + const int time = (int)wallclock.getTimeMilliseconds(); + unsigned rays = cfgBenchmark7_Passes * cfgBenchmark7_Iterations; + printf("%u ms (%i%%),(%u r/s)\r\n", time, (time - cfgBenchmark7_Reference) * 100 / time, (rays * 1000) / time); + } + if (cfgBenchmark8_Enable) + { // Benchmark 8 + srand(380843); + b3DynamicBvh dbvt; + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); + dbvt.optimizeTopDown(); + printf("[8] insert/remove: "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark8_Passes; ++i) + { + for (int j = 0; j < cfgBenchmark8_Iterations; ++j) + { + dbvt.remove(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale), 0)); + } + } + const int time = (int)wallclock.getTimeMilliseconds(); + const int ir = cfgBenchmark8_Passes * cfgBenchmark8_Iterations; + printf("%u ms (%i%%),(%u ir/s)\r\n", time, (time - cfgBenchmark8_Reference) * 100 / time, ir * 1000 / time); + } + if (cfgBenchmark9_Enable) + { // Benchmark 9 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray 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 < cfgBenchmark9_Passes; ++i) + { + for (int j = 0; j < cfgBenchmark9_Iterations; ++j) + { + dbvt.update(const_cast(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 < vectors.size(); ++i) + { + vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)) * cfgBenchmark10_Scale; + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root, leaves); + printf("[10] updates (jitter): "); + wallclock.reset(); + + for (int i = 0; i < cfgBenchmark10_Passes; ++i) + { + for (int j = 0; j < cfgBenchmark10_Iterations; ++j) + { + const b3Vector3& d = vectors[j]; + b3DbvtNode* l = const_cast(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 < cfgBenchmark11_Passes; ++i) + { + dbvt.optimizeIncremental(cfgBenchmark11_Iterations); + } + const int time = (int)wallclock.getTimeMilliseconds(); + const int op = cfgBenchmark11_Passes * cfgBenchmark11_Iterations; + printf("%u ms (%i%%),(%u o/s)\r\n", time, (time - cfgBenchmark11_Reference) * 100 / time, op / time * 1000); + } + if (cfgBenchmark12_Enable) + { // Benchmark 12 + srand(380843); + b3AlignedObjectArray volumes; + b3AlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for (int i = 0; i < cfgLeaves; ++i) + { + volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale); + } + printf("[12] b3DbvtVolume notequal: "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark12_Iterations; ++i) + { + for (int j = 0; j < cfgLeaves; ++j) + { + for (int k = 0; k < cfgLeaves; ++k) + { + results[k] = NotEqual(volumes[j], volumes[k]); + } + } + } + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark12_Reference) * 100 / time); + } + if (cfgBenchmark13_Enable) + { // Benchmark 13 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray vectors; + b3DbvtBenchmark::NilPolicy policy; + vectors.resize(cfgBenchmark13_Iterations); + for (int i = 0; i < vectors.size(); ++i) + { + vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized(); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); + dbvt.optimizeTopDown(); + printf("[13] culling(OCL+fullsort): "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark13_Iterations; ++i) + { + static const b3Scalar offset = 0; + policy.m_depth = -B3_INFINITY; + dbvt.collideOCL(dbvt.m_root, &vectors[i], &offset, vectors[i], 1, policy); + } + const int time = (int)wallclock.getTimeMilliseconds(); + const int t = cfgBenchmark13_Iterations; + printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark13_Reference) * 100 / time, (t * 1000) / time); + } + if (cfgBenchmark14_Enable) + { // Benchmark 14 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray vectors; + b3DbvtBenchmark::P14 policy; + vectors.resize(cfgBenchmark14_Iterations); + for (int i = 0; i < vectors.size(); ++i) + { + vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized(); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); + dbvt.optimizeTopDown(); + policy.m_nodes.reserve(cfgLeaves); + printf("[14] culling(OCL+qsort): "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark14_Iterations; ++i) + { + static const b3Scalar offset = 0; + policy.m_nodes.resize(0); + dbvt.collideOCL(dbvt.m_root, &vectors[i], &offset, vectors[i], 1, policy, false); + policy.m_nodes.quickSort(b3DbvtBenchmark::P14::sortfnc); + } + const int time = (int)wallclock.getTimeMilliseconds(); + const int t = cfgBenchmark14_Iterations; + printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark14_Reference) * 100 / time, (t * 1000) / time); + } + if (cfgBenchmark15_Enable) + { // Benchmark 15 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray vectors; + b3DbvtBenchmark::P15 policy; + vectors.resize(cfgBenchmark15_Iterations); + for (int i = 0; i < vectors.size(); ++i) + { + vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized(); + } + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); + dbvt.optimizeTopDown(); + policy.m_nodes.reserve(cfgLeaves); + printf("[15] culling(KDOP+qsort): "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark15_Iterations; ++i) + { + static const b3Scalar offset = 0; + policy.m_nodes.resize(0); + policy.m_axis = vectors[i]; + dbvt.collideKDOP(dbvt.m_root, &vectors[i], &offset, 1, policy); + policy.m_nodes.quickSort(b3DbvtBenchmark::P15::sortfnc); + } + const int time = (int)wallclock.getTimeMilliseconds(); + const int t = cfgBenchmark15_Iterations; + printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark15_Reference) * 100 / time, (t * 1000) / time); + } + if (cfgBenchmark16_Enable) + { // Benchmark 16 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray 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 < cfgBenchmark16_Passes; ++i) + { + for (int j = 0; j < cfgBenchmark16_BatchCount; ++j) + { + batch.push_back(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale), 0)); + } + for (int j = 0; j < cfgBenchmark16_BatchCount; ++j) + { + dbvt.remove(batch[j]); + } + batch.resize(0); + } + const int time = (int)wallclock.getTimeMilliseconds(); + const int ir = cfgBenchmark16_Passes * cfgBenchmark16_BatchCount; + printf("%u ms (%i%%),(%u bir/s)\r\n", time, (time - cfgBenchmark16_Reference) * 100 / time, int(ir * 1000.0 / time)); + } + if (cfgBenchmark17_Enable) + { // Benchmark 17 + srand(380843); + b3AlignedObjectArray volumes; + b3AlignedObjectArray results; + b3AlignedObjectArray indices; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + indices.resize(cfgLeaves); + for (int i = 0; i < cfgLeaves; ++i) + { + indices[i] = i; + volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale); + } + for (int i = 0; i < cfgLeaves; ++i) + { + b3Swap(indices[i], indices[rand() % cfgLeaves]); + } + printf("[17] b3DbvtVolume select: "); + wallclock.reset(); + for (int i = 0; i < cfgBenchmark17_Iterations; ++i) + { + for (int j = 0; j < cfgLeaves; ++j) + { + for (int k = 0; k < cfgLeaves; ++k) + { + const int idx = indices[k]; + results[idx] = Select(volumes[idx], volumes[j], volumes[k]); + } + } + } + const int time = (int)wallclock.getTimeMilliseconds(); + printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark17_Reference) * 100 / time); + } + printf("\r\n\r\n"); +} +#endif diff --git a/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h new file mode 100644 index 000000000..f44e3377f --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h @@ -0,0 +1,1332 @@ +/* +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 + +#ifndef B3_DYNAMIC_BOUNDING_VOLUME_TREE_H +#define B3_DYNAMIC_BOUNDING_VOLUME_TREE_H + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Geometry/b3AabbUtil.h" + +// +// Compile time configuration +// + +// Implementation profiles +#define B3_DBVT_IMPL_GENERIC 0 // Generic implementation +#define B3_DBVT_IMPL_SSE 1 // SSE + +// Template implementation of ICollide +#ifdef _WIN32 +#if (defined(_MSC_VER) && _MSC_VER >= 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 < h) + { + m = (l + h) >> 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; i < n; ++i) + { + box.mi.setMin(pts[i]); + box.mx.setMax(pts[i]); + } + return (box); +} + +// +inline b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3** ppts, int n) +{ + b3DbvtAabbMm box; + box.mi = box.mx = *ppts[0]; + for (int i = 1; i < n; ++i) + { + box.mi.setMin(*ppts[i]); + box.mx.setMax(*ppts[i]); + } + return (box); +} + +// +B3_DBVT_INLINE void b3DbvtAabbMm::Expand(const b3Vector3& e) +{ + mi -= e; + mx += e; +} + +// +B3_DBVT_INLINE void b3DbvtAabbMm::SignedExpand(const b3Vector3& e) +{ + if (e.x > 0) + mx.setX(mx.x + e[0]); + else + mi.setX(mi.x + e[0]); + if (e.y > 0) + mx.setY(mx.y + e[1]); + else + mi.setY(mi.y + e[1]); + if (e.z > 0) + mx.setZ(mx.z + e[2]); + else + mi.setZ(mi.z + e[2]); +} + +// +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) < b3Proximity(o, b) ? 0 : 1); +#endif +} + +// +B3_DBVT_INLINE void b3Merge(const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b, + b3DbvtAabbMm& r) +{ +#if B3_DBVT_MERGE_IMPL == B3_DBVT_IMPL_SSE + __m128 ami(_mm_load_ps(a.mi)); + __m128 amx(_mm_load_ps(a.mx)); + __m128 bmi(_mm_load_ps(b.mi)); + __m128 bmx(_mm_load_ps(b.mx)); + ami = _mm_min_ps(ami, bmi); + amx = _mm_max_ps(amx, bmx); + _mm_store_ps(r.mi, ami); + _mm_store_ps(r.mx, amx); +#else + for (int i = 0; i < 3; ++i) + { + if (a.mi[i] < b.mi[i]) + r.mi[i] = a.mi[i]; + else + r.mi[i] = b.mi[i]; + if (a.mx[i] > b.mx[i]) + r.mx[i] = a.mx[i]; + else + r.mx[i] = b.mx[i]; + } +#endif +} + +// +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 << count) - 1; + b3AlignedObjectArray stack; + int signs[sizeof(unsigned) * 8]; + b3Assert(count < int(sizeof(signs) / sizeof(signs[0]))); + for (int i = 0; i < count; ++i) + { + signs[i] = ((normals[i].x >= 0) ? 1 : 0) + + ((normals[i].y >= 0) ? 2 : 0) + + ((normals[i].z >= 0) ? 4 : 0); + } + stack.reserve(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) && (i < count); ++i, j <<= 1) + { + if (0 == (se.mask & j)) + { + const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]); + switch (side) + { + case -1: + out = true; + break; + case +1: + se.mask |= j; + break; + } + } + } + if (!out) + { + if ((se.mask != inside) && (se.node->isinternal())) + { + stack.push_back(sStkNP(se.node->childs[0], se.mask)); + stack.push_back(sStkNP(se.node->childs[1], se.mask)); + } + else + { + if (policy.AllLeaves(se.node)) enumLeaves(se.node, policy); + } + } + } while (stack.size()); + } +} + +// +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 << count) - 1; + b3AlignedObjectArray stock; + b3AlignedObjectArray ifree; + b3AlignedObjectArray stack; + int signs[sizeof(unsigned) * 8]; + b3Assert(count < int(sizeof(signs) / sizeof(signs[0]))); + for (int i = 0; i < count; ++i) + { + signs[i] = ((normals[i].x >= 0) ? 1 : 0) + + ((normals[i].y >= 0) ? 2 : 0) + + ((normals[i].z >= 0) ? 4 : 0); + } + stock.reserve(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) && (i < count); ++i, j <<= 1) + { + if (0 == (se.mask & j)) + { + const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]); + switch (side) + { + case -1: + out = true; + break; + case +1: + se.mask |= j; + break; + } + } + } + if (out) continue; + } + if (policy.Descent(se.node)) + { + if (se.node->isinternal()) + { + const 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].value < nes[1].value ? 1 : 0; + int j = stack.size(); + if (fsort && (j > 0)) + { + /* Insert 0 */ + j = nearest(&stack[0], &stock[0], nes[q].value, 0, stack.size()); + stack.push_back(0); +#if 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/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp new file mode 100644 index 000000000..dea2ddb0f --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp @@ -0,0 +1,808 @@ +/* +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; i < overlappingPairArray.size(); i++) + { + b3BroadphasePair& pair = overlappingPairArray[i]; + + bool isDuplicate = (pair == previousPair); + + previousPair = pair; + + bool needsRemoval = false; + + if (!isDuplicate) + { + //important to perform AABB check that is consistent with the broadphase + b3DbvtProxy* pa = &m_proxies[pair.x]; + b3DbvtProxy* pb = &m_proxies[pair.y]; + bool hasOverlap = b3Intersect(pa->leaf->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; i < ni; ++i) + { + b3BroadphasePair& p = pairs[(m_cid + i) % pairs.size()]; + b3DbvtProxy* pa = &m_proxies[p.x]; + b3DbvtProxy* pb = &m_proxies[p.y]; + if (!b3Intersect(pa->leaf->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; iexp < nexperiments; ++iexp) + { + const b3BroadphaseBenchmark::Experiment& experiment = experiments[iexp]; + const int object_count = experiment.object_count; + const int update_count = (object_count * experiment.update_count) / 100; + const int spawn_count = (object_count * experiment.spawn_count) / 100; + const b3Scalar speed = experiment.speed; + const b3Scalar amplitude = experiment.amplitude; + printf("Experiment #%u '%s':\r\n", iexp, experiment.name); + printf("\tObjects: %u\r\n", object_count); + printf("\tUpdate: %u\r\n", update_count); + printf("\tSpawn: %u\r\n", spawn_count); + printf("\tSpeed: %f\r\n", speed); + printf("\tAmplitude: %f\r\n", amplitude); + srand(180673); + /* Create objects */ + wallclock.reset(); + objects.reserve(object_count); + for (int i = 0; i < object_count; ++i) + { + b3BroadphaseBenchmark::Object* po = new b3BroadphaseBenchmark::Object(); + po->center[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; i < objects.size(); ++i) + { + objects[i]->update(speed, amplitude, pbi); + } + b3BroadphaseBenchmark::OutputTime("\tFirst update", wallclock); + /* Updates */ + wallclock.reset(); + for (int i = 0; i < experiment.iterations; ++i) + { + for (int j = 0; j < update_count; ++j) + { + objects[j]->update(speed, amplitude, pbi); + } + pbi->calculateOverlappingPairs(0); + } + b3BroadphaseBenchmark::OutputTime("\tUpdate", wallclock, experiment.iterations); + /* Clean up */ + wallclock.reset(); + for (int i = 0; i < objects.size(); ++i) + { + pbi->destroyProxy(objects[i]->proxy, 0); + delete objects[i]; + } + objects.resize(0); + b3BroadphaseBenchmark::OutputTime("\tRelease", wallclock); + } +} +#else +/*void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*) +{} +*/ +#endif + +#if B3_DBVT_BP_PROFILE +#undef b3SPC +#endif diff --git a/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h new file mode 100644 index 000000000..c235e4014 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h @@ -0,0 +1,197 @@ +/* +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/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h new file mode 100644 index 000000000..4ff9ebae8 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h @@ -0,0 +1,70 @@ +/* +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/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp new file mode 100644 index 000000000..19773244b --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp @@ -0,0 +1,559 @@ +/* +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 < curHashtableSize; i++) + { + const b3BroadphasePair& pair = m_overlappingPairArray[i]; + int proxyId1 = pair.x; + int proxyId2 = pair.y; + /*if (proxyId1 > 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; i < m_overlappingPairArray.size();) + { + b3BroadphasePair* pair = &m_overlappingPairArray[i]; + if (callback->processOverlap(*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; i < m_overlappingPairArray.size(); i++) + { + tmpPairs.push_back(m_overlappingPairArray[i]); + } + + for (i = 0; i < tmpPairs.size(); i++) + { + removeOverlappingPair(tmpPairs[i].x, tmpPairs[i].y, dispatcher); + } + + for (i = 0; i < m_next.size(); i++) + { + m_next[i] = B3_NULL_PAIR; + } + + tmpPairs.quickSort(b3BroadphasePairSortPredicate()); + + for (i = 0; i < tmpPairs.size(); i++) + { + addOverlappingPair(tmpPairs[i].x, tmpPairs[i].y); + } +} + +void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher) +{ + if (!hasDeferredRemoval()) + { + b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0, proxy1); + + int findIndex = m_overlappingPairArray.findLinearSearch(findPair); + if (findIndex < m_overlappingPairArray.size()) + { + b3g_overlappingPairs--; + b3BroadphasePair& pair = m_overlappingPairArray[findIndex]; + + cleanOverlappingPair(pair, dispatcher); + //if (m_ghostPairCallback) + // m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); + + m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1); + m_overlappingPairArray.pop_back(); + return 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; i < m_overlappingPairArray.size();) + { + b3BroadphasePair* pair = &m_overlappingPairArray[i]; + if (callback->processOverlap(*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/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h new file mode 100644 index 000000000..f1de1d94e --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h @@ -0,0 +1,427 @@ +/* +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/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h new file mode 100644 index 000000000..343a2c0e2 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h @@ -0,0 +1,56 @@ + +#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/Engine/lib/bullet/src/Bullet3Collision/CMakeLists.txt b/Engine/lib/bullet/src/Bullet3Collision/CMakeLists.txt new file mode 100644 index 000000000..130095cc0 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h new file mode 100644 index 000000000..518da89c5 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h @@ -0,0 +1,39 @@ +#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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h new file mode 100644 index 000000000..c2cd3c729 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h @@ -0,0 +1,55 @@ +/* +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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp new file mode 100644 index 000000000..a5dab74a3 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp @@ -0,0 +1,500 @@ +/* +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; p < numVertices; p++) + { + m_vertices[p] = convexUtil->vertices[p]; + } + + for (int i = 0; i < numFaces; i++) + { + int face = convexUtil->faces[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; v < tmpFaces[i].m_indices.size(); v++) + { + b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]); + if (planeEq > eq) + { + 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 < tmpFaces.size(); i++) + todoFaces.push_back(i); + + while (todoFaces.size()) + { + b3AlignedObjectArray 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; i < coplanarFaceGroup.size(); i++) + { + // m_polyhedron->m_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 < face.m_indices.size(); f++) + { + int orgIndex = face.m_indices[f]; + b3Vector3 pt = m_vertices[orgIndex]; + + bool found = false; + + for (int i = 0; i < orgpoints.size(); i++) + { + //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001)) + if (orgpoints[i].m_orgIndex == orgIndex) + { + found = true; + break; + } + } + if (!found) + orgpoints.push_back(b3GrahamVector3(pt, orgIndex)); + } + } + + b3MyFace combinedFace; + for (int i = 0; i < 4; i++) + combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i]; + + b3AlignedObjectArray hull; + + averageFaceNormal.normalize(); + b3GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal); + + for (int i = 0; i < hull.size(); i++) + { + combinedFace.m_indices.push_back(hull[i].m_orgIndex); + for (int k = 0; k < orgpoints.size(); k++) + { + if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex) + { + orgpoints[k].m_orgIndex = -1; // invalidate... + break; + } + } + } + + // are there rejected vertices? + bool reject_merge = false; + + for (int i = 0; i < orgpoints.size(); i++) + { + if (orgpoints[i].m_orgIndex == -1) + continue; // this is in the hull... + // this vertex is rejected -- is anybody else using this vertex? + for (int j = 0; j < tmpFaces.size(); j++) + { + b3MyFace& face = tmpFaces[j]; + // is this a face of the current coplanar group? + bool is_in_current_group = false; + for (int k = 0; k < coplanarFaceGroup.size(); k++) + { + if (coplanarFaceGroup[k] == j) + { + is_in_current_group = true; + break; + } + } + if (is_in_current_group) // ignore this face... + continue; + // does this face use this rejected vertex? + for (int v = 0; v < face.m_indices.size(); v++) + { + if (face.m_indices[v] == orgpoints[i].m_orgIndex) + { + // this rejected vertex is used in another face -- reject merge + reject_merge = true; + break; + } + } + if (reject_merge) + break; + } + if (reject_merge) + break; + } + + if (!reject_merge) + { + // do this merge! + did_merge = true; + m_faces.push_back(combinedFace); + } + } + if (!did_merge) + { + for (int i = 0; i < coplanarFaceGroup.size(); i++) + { + b3MyFace face = tmpFaces[coplanarFaceGroup[i]]; + m_faces.push_back(face); + } + } + } + + initialize(); + + return true; +} + +inline bool IsAlmostZero(const b3Vector3& v) +{ + if (fabsf(v.getX()) > 1e-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; i < m_faces.size(); i++) + { + const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]); + const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3]; + if (d > 0.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; i < m_faces.size(); i++) + { + int numVertices = m_faces[i].m_indices.size(); + int NbTris = numVertices; + for (int j = 0; j < NbTris; j++) + { + int k = (j + 1) % numVertices; + b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]); + b3InternalEdge* edptr = edges.find(vp); + b3Vector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0]; + edge.normalize(); + + bool found = false; + b3Vector3 diff, diff2; + + for (int p = 0; p < m_uniqueEdges.size(); p++) + { + diff = m_uniqueEdges[p] - edge; + diff2 = m_uniqueEdges[p] + edge; + + // if ((diff.length2()==0.f) || + // (diff2.length2()==0.f)) + + if (IsAlmostZero(diff) || + IsAlmostZero(diff2)) + { + found = true; + break; + } + } + + if (!found) + { + m_uniqueEdges.push_back(edge); + } + + if (edptr) + { + //TBD: figure out why I added this assert + // b3Assert(edptr->m_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; i < m_faces.size(); i++) + { + int numVertices = m_faces[i].m_indices.size(); + m_faces[i].m_connectedFaces.resize(numVertices); + + for (int j = 0; j < numVertices; j++) + { + int k = (j + 1) % numVertices; + b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]); + b3InternalEdge* edptr = edges.find(vp); + b3Assert(edptr); + b3Assert(edptr->m_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; i < m_faces.size(); i++) + { + int numVertices = m_faces[i].m_indices.size(); + int NbTris = numVertices - 2; + + const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]]; + for (int j = 1; j <= NbTris; j++) + { + int k = (j + 1) % numVertices; + const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]]; + const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]]; + b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f; + b3Vector3 Center = (p0 + p1 + p2) / 3.0f; + m_localCenter += Area * Center; + TotalArea += Area; + } + } + m_localCenter /= TotalArea; + +#ifdef TEST_INTERNAL_OBJECTS + if (1) + { + m_radius = FLT_MAX; + for (int i = 0; i < m_faces.size(); i++) + { + const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]); + const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]); + if (dist < m_radius) + m_radius = dist; + } + + b3Scalar MinX = FLT_MAX; + b3Scalar MinY = FLT_MAX; + b3Scalar MinZ = FLT_MAX; + b3Scalar MaxX = -FLT_MAX; + b3Scalar MaxY = -FLT_MAX; + b3Scalar MaxZ = -FLT_MAX; + for (int i = 0; i < m_vertices.size(); i++) + { + const b3Vector3& pt = m_vertices[i]; + if (pt.getX() < MinX) MinX = pt.getX(); + if (pt.getX() > MaxX) MaxX = pt.getX(); + if (pt.getY() < MinY) MinY = pt.getY(); + if (pt.getY() > MaxY) MaxY = pt.getY(); + if (pt.getZ() < MinZ) MinZ = pt.getZ(); + 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 << LargestExtent) & 3; + const int e1 = (1 << e0) & 3; + + for (int j = 0; j < 1024; j++) + { + const b3Scalar Saved0 = m_extents[e0]; + const b3Scalar Saved1 = m_extents[e1]; + m_extents[e0] += Step; + m_extents[e1] += Step; + + if (!testContainment()) + { + m_extents[e0] = Saved0; + m_extents[e1] = Saved1; + break; + } + } + } + } +#endif +} diff --git a/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h new file mode 100644 index 000000000..4c8a88cbd --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h @@ -0,0 +1,55 @@ + +/* +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 + +#ifndef _BT_CONVEX_UTILITY_H +#define _BT_CONVEX_UTILITY_H + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Transform.h" + +struct b3MyFace +{ + b3AlignedObjectArray 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 diff --git a/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp new file mode 100644 index 000000000..e0b216110 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp @@ -0,0 +1,297 @@ +#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; i < nPairs; i++) + { + int bodyIndexA = pairs[i].x; + int bodyIndexB = pairs[i].y; + int collidableIndexA = bodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = bodies[bodyIndexB].m_collidableIdx; + + if (m_data->m_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; i < utilPtr->m_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; i < utilPtr->m_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 (curSize < m_data->m_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; i < numVertices; i++) + { + float* vertex = (float*)&vts[i * strideInBytes]; + verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2])); + } + + b3ConvexUtility* utilPtr = new b3ConvexUtility(); + bool merge = true; + if (numVertices) + { + utilPtr->initializePolyhedralFeatures(&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; i < convexPtr->m_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; i < convexPtr->m_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; p < numIndices; p++) + { + m_data->m_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; i < convexPtr->m_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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h new file mode 100644 index 000000000..f02353c26 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h @@ -0,0 +1,92 @@ +#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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h new file mode 100644 index 000000000..b50c0eca4 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h @@ -0,0 +1,25 @@ + +#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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h new file mode 100644 index 000000000..be1be57f0 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h @@ -0,0 +1,28 @@ +/* +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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h new file mode 100644 index 000000000..d6beb662b --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h @@ -0,0 +1,19 @@ + +#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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h new file mode 100644 index 000000000..7c2507cc9 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h @@ -0,0 +1,123 @@ + + +#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 < numSubtreeHeaders; i++) + { + b3BvhSubtreeInfoData subtree = subtreeHeaders[i]; + + int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax); + if (overlap != 0) + { + int startNodeIndex = subtree.m_rootNodeIndex; + int endNodeIndex = subtree.m_rootNodeIndex + subtree.m_subtreeSize; + int curIndex = startNodeIndex; + int escapeIndex; + int isLeafNode; + int aabbOverlap; + while (curIndex < endNodeIndex) + { + b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex]; + aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.m_quantizedAabbMin, rootNode.m_quantizedAabbMax); + isLeafNode = b3IsLeaf(&rootNode); + if (aabbOverlap) + { + if (isLeafNode) + { + int triangleIndex = b3GetTriangleIndex(&rootNode); + if (shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + int numChildrenB = collidables[collidableIndexB].m_numChildShapes; + int pairIdx = b3AtomicAdd(numConcavePairsOut, numChildrenB); + for (int b = 0; b < numChildrenB; b++) + { + if ((pairIdx + b) < maxNumConcavePairsCapacity) + { + int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b; + b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, childShapeIndexB); + concavePairsOut[pairIdx + b] = newPair; + } + } + } + else + { + int pairIdx = b3AtomicInc(numConcavePairsOut); + if (pairIdx < maxNumConcavePairsCapacity) + { + b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, 0); + concavePairsOut[pairIdx] = newPair; + } + } + } + curIndex++; + } + else + { + if (isLeafNode) + { + curIndex++; + } + else + { + escapeIndex = b3GetEscapeIndex(&rootNode); + curIndex += escapeIndex; + } + } + } + } + } +} \ No newline at end of file diff --git a/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h new file mode 100644 index 000000000..0d9b13f1d --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h @@ -0,0 +1,171 @@ +#ifndef B3_CLIP_FACES_H +#define B3_CLIP_FACES_H + +#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 b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t) +{ + return b3MakeFloat4(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 clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut) +{ + int ve; + float ds, de; + int numVertsOut = 0; + //double-check next test + // if (numVertsIn < 2) + // return 0; + + b3Float4 firstVertex = pVtxIn[numVertsIn - 1]; + b3Float4 endVertex = pVtxIn[0]; + + ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS; + + for (ve = 0; ve < numVertsIn; ve++) + { + endVertex = pVtxIn[ve]; + de = b3Dot(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; +} + +__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; e0 < numVertsInA; e0++) + { + const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0]; + const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)]; + const b3Float4 WorldEdge0 = aw - bw; + b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex]; + b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1); + b3Float4 worldA1 = aw; + float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1); + b3Float4 planeNormalWS = planeNormalWS1; + float planeEqWS = planeEqWS1; + numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut); + __global b3Float4* tmp = pVtxOut; + pVtxOut = pVtxIn; + pVtxIn = tmp; + numVertsInB = numVertsOut; + numVertsOut = 0; + } + + b3Float4 planeNormalWS = worldNormalsA1[pairIndex]; + float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]); + + for (int i = 0; i < numVertsInB; i++) + { + float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS; + if (depth <= minDist) + { + depth = minDist; + } + /* + static float maxDepth = 0.f; + if (depth < maxDepth) + { + maxDepth = depth; + if (maxDepth < -10) + { + printf("error at framecount %d?\n",myframecount); + } + printf("maxDepth = %f\n", maxDepth); + + } +*/ + if (depth <= maxDist) + { + b3Float4 pointInWorld = pVtxIn[i]; + pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth); + } + } + } + clippingFaces[pairIndex].w = numLocalContactsOut; + } + + for (int i = 0; i < numLocalContactsOut; i++) + pVtxIn[i] = pVtxOut[i]; + + } // if (hasSeparatingAxis[i]) + } // if (im_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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h new file mode 100644 index 000000000..ca68f4bc4 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h @@ -0,0 +1,486 @@ + +#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; face < hullA->m_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; e0 < numVerticesA; e0++) + { + const b3Float4 a = verticesA[hullA->m_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 < numVertsIn; i++) + { + float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS; + if (depth <= minDist) + { + depth = minDist; + } + if (numContactsOut < contactCapacity) + { + if (depth <= maxDist) + { + b3Float4 pointInWorld = pVtxIn[i]; + //resultOut.addContactPoint(separatingNormal,point,depth); + contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth); + //printf("depth=%f\n",depth); + } + } + else + { + b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity); + } + } + } + + return numContactsOut; +} + +inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal, + const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB, + b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts, + const float minDist, float maxDist, + const b3AlignedObjectArray& 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; face < hullB.m_numFaces; face++) + { +#ifdef BT_DEBUG_SAT_FACE + if (once) + printf("face %d\n", face); + const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face]; + if (once) + { + for (int i = 0; i < faceB->m_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 < numVertices; e0++) + { + const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]]; + worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB); + } + } + + if (closestFaceB >= 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 (nContacts < maxContactCapacity) + { + contactIndex = nContacts; + globalContactOut->expand(); + 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 < numPoints; p++) + { + contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]]; //check if it is actually on B + contact.m_worldNormalOnB = normalOnSurfaceB; + } + //printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints); + contact.m_worldNormalOnB.w = (b3Scalar)numPoints; + nContacts++; + } + else + { + b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity); + } + } + } + return contactIndex; +} + +inline int b3ContactConvexConvexSAT( + 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) +{ + 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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h new file mode 100644 index 000000000..acf7c1b18 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h @@ -0,0 +1,153 @@ + +#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 (l2 < radius * radius) + { + dist = b3Sqrt(l2); + if (dist > minDist) + { + 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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h new file mode 100644 index 000000000..d5a73bd4f --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h @@ -0,0 +1,38 @@ + +#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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h new file mode 100644 index 000000000..983554eb2 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h @@ -0,0 +1,797 @@ +#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; i < numVerts; i++) + { + float dp = b3Dot(vertices[hull->m_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 (Max0 < Min1 || Max1 < Min0) + return false; + + float d0 = Max0 - Min1; + float d1 = Max1 - Min0; + *depth = d0 < d1 ? d0 : d1; + return true; +} + +bool b3FindSeparatingAxis(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) +{ + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; + /* + static int maxFaceVertex = 0; + + int curFaceVertexAB = hullA->m_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; i < numFacesA; i++) + { + const b3Float4 normal = facesA[hullA->m_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; e0 < hullA->m_numUniqueEdges; e0++) + { + const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset + e0]; + b3Float4 edge0World = b3QuatRotate(ornA, edge0); + + for (int e1 = 0; e1 < hullB->m_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 (Max0 < Min1 || Max1 < Min0) + return false; + + float d0 = Max0 - Min1; + float d1 = Max1 - Min0; + dist = d0 < d1 ? d0 : d1; + result = true; + } + + if (dist < *dmin) + { + *dmin = dist; + *sep = crossje; + } + } + } + } + } + else + { + int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3); + //printf("numDirections =%d\n",numDirections ); + + for (int i = 0; i < numDirections; i++) + { + b3Float4 crossje = unitSphere162[i]; + { + //if (b3Dot(DeltaC2,crossje)>0) + { + 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 (Max0 < Min1 || Max1 < Min0) + return false; + + float d0 = Max0 - Min1; + float d1 = Max1 - Min0; + dist = d0 < d1 ? d0 : d1; + result = true; + } + + if (dist < *dmin) + { + *dmin = dist; + *sep = crossje; + } + } + } + } + } + + if ((b3Dot(-DeltaC2, *sep)) > 0.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; face < hullB->m_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; e0 < numVertices; e0++) + { + const b3Float4 b = verticesB[hullB->m_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; face < hullA->m_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; e0 < numVerticesA; e0++) + { + const b3Float4 a = verticesA[hullA->m_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 < numVertices; i++) + { + b3Float4 v0 = verticesA[i]; + b3Float4 v1 = verticesA[prevVertex]; + + b3Float4 edgeNormal = b3Normalized(b3Cross(normal, v1 - v0)); + float c = -b3Dot(edgeNormal, v0); + + facesA[fidx].m_numIndices = 2; + facesA[fidx].m_indexOffset = curUsedIndices; + indicesA[curUsedIndices++] = i; + indicesA[curUsedIndices++] = prevVertex; + + facesA[fidx].m_plane.x = edgeNormal.x; + facesA[fidx].m_plane.y = edgeNormal.y; + facesA[fidx].m_plane.z = edgeNormal.z; + facesA[fidx].m_plane.w = c; + fidx++; + prevVertex = i; + } + } + convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES; + convexPolyhedronA.m_localCenter = localCenter * (1.f / 3.f); + + b3Float4 posA = rigidBodies[bodyIndexA].m_pos; + posA.w = 0.f; + b3Float4 posB = rigidBodies[bodyIndexB].m_pos; + posB.w = 0.f; + + b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; + b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; + + /////////////////// + ///compound shape support + + if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + int compoundChild = concavePairs[pairIdx].w; + int childShapeIndexB = compoundChild; //collidables[collidableIndexB].m_shapeIndex+compoundChild; + int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; + b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; + b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; + b3Float4 newPosB = b3TransformPoint(childPosB, posB, ornB); + b3Quaternion newOrnB = b3QuatMul(ornB, childOrnB); + posB = newPosB; + ornB = newOrnB; + shapeIndexB = collidables[childColIndexB].m_shapeIndex; + } + ////////////////// + + b3Float4 c0local = convexPolyhedronA.m_localCenter; + b3Float4 c0 = b3TransformPoint(c0local, posA, ornA); + b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter; + b3Float4 c1 = b3TransformPoint(c1local, posB, ornB); + const b3Float4 DeltaC2 = c0 - c1; + + bool sepA = b3FindSeparatingAxis(&convexPolyhedronA, &convexShapes[shapeIndexB], + posA, ornA, + posB, ornB, + DeltaC2, + verticesA, uniqueEdgesA, facesA, indicesA, + vertices, uniqueEdges, faces, indices, + &sepAxis, &dmin); + hasSeparatingAxis = 4; + if (!sepA) + { + hasSeparatingAxis = 0; + } + else + { + bool sepB = b3FindSeparatingAxis(&convexShapes[shapeIndexB], &convexPolyhedronA, + posB, ornB, + posA, ornA, + DeltaC2, + vertices, uniqueEdges, faces, indices, + verticesA, uniqueEdgesA, facesA, indicesA, + &sepAxis, &dmin); + + if (!sepB) + { + hasSeparatingAxis = 0; + } + else + { + bool sepEE = b3FindSeparatingAxisEdgeEdge(&convexPolyhedronA, &convexShapes[shapeIndexB], + posA, ornA, + posB, ornB, + DeltaC2, + verticesA, uniqueEdgesA, facesA, indicesA, + vertices, uniqueEdges, faces, indices, + &sepAxis, &dmin, true); + + if (!sepEE) + { + hasSeparatingAxis = 0; + } + else + { + hasSeparatingAxis = 1; + } + } + } + + if (hasSeparatingAxis) + { + hasSeparatingNormals[i] = 1; + sepAxis.w = dmin; + concaveSeparatingNormalsOut[pairIdx] = sepAxis; + + //now compute clipping faces A and B, and world-space clipping vertices A and B... + + float minDist = -1e30f; + float maxDist = 0.02f; + + b3FindClippingFaces(sepAxis, + &convexPolyhedronA, + &convexShapes[shapeIndexB], + posA, ornA, + posB, ornB, + worldVertsA1Out, + worldNormalsA1Out, + worldVertsB1Out, + vertexFaceCapacity, + minDist, maxDist, + verticesA, + facesA, + indicesA, + + vertices, + faces, + indices, + clippingFacesOut, pairIdx); + } + else + { + //mark this pair as in-active + concavePairs[pairIdx].w = -1; + } + } + else + { + //mark this pair as in-active + concavePairs[pairIdx].w = -1; + } +} + +#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H diff --git a/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h new file mode 100644 index 000000000..b4981ae65 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h @@ -0,0 +1,197 @@ +#ifndef B3_FIND_SEPARATING_AXIS_H +#define B3_FIND_SEPARATING_AXIS_H + +inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray& 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 < numVerts; i++) + { + //b3Vector3 pt = trans * vertices[m_vertexOffset+i]; + //b3Scalar dp = pt.dot(dir); + //b3Vector3 vertex = vertices[hull.m_vertexOffset+i]; + b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset + i], localDir); + //b3Assert(dp==dpL); + if (dp < min) min = dp; + if (dp > 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 < Min1 || Max1 < Min0) + return false; + + b3Scalar d0 = Max0 - Min1; + b3Assert(d0 >= 0.0f); + b3Scalar d1 = Max1 - Min0; + b3Assert(d1 >= 0.0f); + depth = d0 < d1 ? d0 : d1; + return true; +} + +inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA1, + const b3Quaternion& ornA, + const b3Float4& 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"); + + 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; i < numFacesA; i++) + { + const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset + i].m_plane; + b3Float4 faceANormalWS = b3QuatRotate(ornA, normal); + + if (b3Dot3F4(deltaC2, faceANormalWS) < 0) + faceANormalWS *= -1.f; + + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar d; + if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d)) + return false; + + if (d < dmin) + { + dmin = d; + sep = (b3Vector3&)faceANormalWS; + } + } + + int numFacesB = hullB.m_numFaces; + // Test normals from hullB + for (int i = 0; i < numFacesB; i++) + { + b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset + i].m_plane; + b3Float4 WorldNormal = b3QuatRotate(ornB, normal); + + if (b3Dot3F4(deltaC2, WorldNormal) < 0) + { + WorldNormal *= -1.f; + } + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar d; + if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d)) + return false; + + if (d < dmin) + { + dmin = d; + sep = (b3Vector3&)WorldNormal; + } + } + + // b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend; + + int curEdgeEdge = 0; + // Test edges + for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++) + { + const b3Float4& edge0 = (b3Float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0]; + b3Float4 edge0World = b3QuatRotate(ornA, (b3Float4&)edge0); + + for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++) + { + const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1]; + b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1); + + b3Float4 crossje = b3Cross3(edge0World, edge1World); + + curEdgeEdge++; + if (!b3IsAlmostZero((b3Vector3&)crossje)) + { + crossje = b3FastNormalized3(crossje); + if (b3Dot3F4(deltaC2, crossje) < 0) + crossje *= -1.f; + +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar dist; + if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist)) + return false; + + if (dist < dmin) + { + dmin = dist; + sep = (b3Vector3&)crossje; + } + } + } + } + + if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f) + sep = -sep; + + return true; +} + +#endif //B3_FIND_SEPARATING_AXIS_H diff --git a/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h new file mode 100644 index 000000000..a3bfbf299 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h @@ -0,0 +1,888 @@ + +/*** + * --------------------------------- + * 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; i < B3_MPR_MAX_ITERATIONS; i++) + //while (1) + { + // compute direction outside the portal (from v0 throught v1,v2,v3 + // face) + b3PortalDir(portal, &dir); + + // test if origin is inside the portal + if (portalEncapsulesOrigin(portal, &dir)) + return 0; + + // get next support point + + b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4); + + // test if v4 can expand portal to contain origin and if portal + // expanding doesn't reach given tolerance + if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir)) + { + return -1; + } + + // v1-v2-v3 triangle must be rearranged to face outside Minkowski + // difference (direction from v0). + b3ExpandPortal(portal, &v4); + } + + return -1; +} + +B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos) +{ + b3Float4 zero = b3MakeFloat4(0, 0, 0, 0); + b3Float4 *b3mpr_vec3_origin = &zero; + + b3Float4 dir; + size_t i; + float b[4], sum, inv; + b3Float4 vec, p1, p2; + + b3PortalDir(portal, &dir); + + // use barycentric coordinates of tetrahedron to find origin + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v, + &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 < B3_MPR_MAX_ITERATIONS; i++) + //while (1) + { + // compute portal direction and obtain next support point + b3PortalDir(portal, &dir); + + b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4); + + // reached tolerance -> 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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h new file mode 100644 index 000000000..6e991e14b --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h @@ -0,0 +1,175 @@ + +#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; i < nPoints; i++) + center += p[i]; + center /= (float)nPoints; + } + + // sample 4 directions + + b3Float4 aVector = p[0] - center; + b3Float4 u = b3Cross(nearNormal, aVector); + b3Float4 v = b3Cross(nearNormal, u); + u = b3Normalized(u); + v = b3Normalized(v); + + //keep point with deepest penetration + float minW = FLT_MAX; + + int minIndex = -1; + + b3Float4 maxDots; + maxDots.x = FLT_MIN; + maxDots.y = FLT_MIN; + maxDots.z = FLT_MIN; + maxDots.w = FLT_MIN; + + // idx, distance + for (int ie = 0; ie < nPoints; ie++) + { + if (p[ie].w < minW) + { + minW = p[ie].w; + minIndex = ie; + } + float f; + b3Float4 r = p[ie] - center; + f = b3Dot(u, r); + if (f < maxDots.x) + { + maxDots.x = f; + contactIdx[0].x = ie; + } + + f = b3Dot(-u, r); + if (f < maxDots.y) + { + maxDots.y = f; + contactIdx[0].y = ie; + } + + f = b3Dot(v, r); + if (f < maxDots.z) + { + maxDots.z = f; + contactIdx[0].z = ie; + } + + f = b3Dot(-v, r); + if (f < maxDots.w) + { + maxDots.w = f; + contactIdx[0].w = ie; + } + } + + if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex) + { + //replace the first contact with minimum (todo: replace contact with least penetration) + contactIdx[0].x = minIndex; + } + + return 4; +} + +__kernel void b3NewContactReductionKernel(__global b3Int4* pairs, + __global const b3RigidBodyData_t* rigidBodies, + __global const b3Float4* separatingNormals, + __global const int* hasSeparatingAxis, + __global struct b3Contact4Data* globalContactsOut, + __global b3Int4* clippingFaces, + __global b3Float4* worldVertsB2, + volatile __global int* nGlobalContactsOut, + int vertexFaceCapacity, + int contactCapacity, + int numPairs, + int pairIndex) +{ + // int i = get_global_id(0); + //int pairIndex = i; + int i = pairIndex; + + b3Int4 contactIdx; + contactIdx = b3MakeInt4(0, 1, 2, 3); + + if (i < numPairs) + { + if (hasSeparatingAxis[i]) + { + int nPoints = clippingFaces[pairIndex].w; + + if (nPoints > 0) + { + __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/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h new file mode 100644 index 000000000..c108255b9 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h @@ -0,0 +1,89 @@ +#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; i < nPoints; i++) + center += p[i]; + center /= (float)nPoints; + } + + // sample 4 directions + + b3Float4 aVector = p[0] - center; + b3Float4 u = b3Cross3(nearNormal, aVector); + b3Float4 v = b3Cross3(nearNormal, u); + u = b3FastNormalized3(u); + v = b3FastNormalized3(v); + + //keep point with deepest penetration + float minW = FLT_MAX; + + int minIndex = -1; + + b3Float4 maxDots; + maxDots.x = FLT_MIN; + maxDots.y = FLT_MIN; + maxDots.z = FLT_MIN; + maxDots.w = FLT_MIN; + + // idx, distance + for (int ie = 0; ie < nPoints; ie++) + { + if (p[ie].w < minW) + { + minW = p[ie].w; + minIndex = ie; + } + float f; + b3Float4 r = p[ie] - center; + f = b3Dot3F4(u, r); + if (f < maxDots.x) + { + maxDots.x = f; + contactIdx[0].x = ie; + } + + f = b3Dot3F4(-u, r); + if (f < maxDots.y) + { + maxDots.y = f; + contactIdx[0].y = ie; + } + + f = b3Dot3F4(v, r); + if (f < maxDots.z) + { + maxDots.z = f; + contactIdx[0].z = ie; + } + + f = b3Dot3F4(-v, r); + if (f < maxDots.w) + { + maxDots.w = f; + contactIdx[0].w = ie; + } + } + + if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex) + { + //replace the first contact with minimum (todo: replace contact with least penetration) + contactIdx[0].x = minIndex; + } + + return 4; +} + +#endif //B3_REDUCE_CONTACTS_H diff --git a/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h new file mode 100644 index 000000000..663e946fc --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h @@ -0,0 +1,31 @@ +#ifndef B3_RIGIDBODY_DATA_H +#define B3_RIGIDBODY_DATA_H + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Quat.h" +#include "Bullet3Common/shared/b3Mat3x3.h" + +typedef struct b3RigidBodyData b3RigidBodyData_t; + +struct b3RigidBodyData +{ + b3Float4 m_pos; + b3Quat m_quat; + b3Float4 m_linVel; + b3Float4 m_angVel; + + int m_collidableIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +}; + +typedef struct b3InertiaData b3InertiaData_t; + +struct b3InertiaData +{ + b3Mat3x3 m_invInertiaWorld; + b3Mat3x3 m_initInvInertia; +}; + +#endif //B3_RIGIDBODY_DATA_H diff --git a/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h new file mode 100644 index 000000000..e0c3a5cf9 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h @@ -0,0 +1,35 @@ +#ifndef B3_UPDATE_AABBS_H +#define B3_UPDATE_AABBS_H + +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +void b3ComputeWorldAabb(int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs) +{ + __global const b3RigidBodyData_t* body = &bodies[bodyId]; + + b3Float4 position = body->m_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/Engine/lib/bullet/src/Bullet3Collision/premake4.lua b/Engine/lib/bullet/src/Bullet3Collision/premake4.lua new file mode 100644 index 000000000..bd0da541e --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Common/CMakeLists.txt b/Engine/lib/bullet/src/Bullet3Common/CMakeLists.txt new file mode 100644 index 000000000..e899e67d9 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Common/b3AlignedAllocator.cpp b/Engine/lib/bullet/src/Bullet3Common/b3AlignedAllocator.cpp new file mode 100644 index 000000000..d546d5e06 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3AlignedAllocator.cpp @@ -0,0 +1,186 @@ +/* +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" + +#ifdef B3_ALLOCATOR_STATISTICS +int b3g_numAlignedAllocs = 0; +int b3g_numAlignedFree = 0; +int b3g_totalBytesAlignedAllocs = 0; //detect memory leaks +#endif + +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; +#ifdef B3_ALLOCATOR_STATISTICS + b3g_totalBytesAlignedAllocs += size; + b3g_numAlignedAllocs++; +#endif + 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; +#ifdef B3_ALLOCATOR_STATISTICS + b3g_numAlignedFree++; +#endif + if (ptr) + { + real = *((void **)(ptr)-1); + int size = *((int *)(ptr)-2); +#ifdef B3_ALLOCATOR_STATISTICS + b3g_totalBytesAlignedAllocs -= size; +#endif + 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) +{ +#ifdef B3_ALLOCATOR_STATISTICS + b3g_numAlignedAllocs++; +#endif + void *ptr; + ptr = b3s_alignedAllocFunc(size, alignment); + // b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr); + return ptr; +} + +void b3AlignedFreeInternal(void *ptr) +{ + if (!ptr) + { + return; + } +#ifdef B3_ALLOCATOR_STATISTICS + b3g_numAlignedFree++; +#endif + // b3Printf("b3AlignedFreeInternal %x\n",ptr); + b3s_alignedFreeFunc(ptr); +} + +#endif //B3_DEBUG_MEMORY_ALLOCATIONS diff --git a/Engine/lib/bullet/src/Bullet3Common/b3AlignedAllocator.h b/Engine/lib/bullet/src/Bullet3Common/b3AlignedAllocator.h new file mode 100644 index 000000000..bcff9f128 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3AlignedAllocator.h @@ -0,0 +1,110 @@ +/* +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 +class b3AlignedAllocator +{ + typedef b3AlignedAllocator self_type; + +public: + //just going down a list: + b3AlignedAllocator() {} + /* + b3AlignedAllocator( const self_type & ) {} + */ + + template + b3AlignedAllocator(const b3AlignedAllocator&) + { + } + + 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(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(ptr)); + } + void destroy(pointer ptr) { ptr->~value_type(); } + + template + struct rebind + { + typedef b3AlignedAllocator other; + }; + template + self_type& operator=(const b3AlignedAllocator&) + { + return *this; + } + + friend bool operator==(const self_type&, const self_type&) { return true; } +}; + +#endif //B3_ALIGNED_ALLOCATOR diff --git a/Engine/lib/bullet/src/Bullet3Common/b3AlignedObjectArray.h b/Engine/lib/bullet/src/Bullet3Common/b3AlignedObjectArray.h new file mode 100644 index 000000000..249e381bf --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3AlignedObjectArray.h @@ -0,0 +1,522 @@ +/* +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 < end; ++i) +#ifdef B3_USE_PLACEMENT_NEW + new (&dest[i]) T(m_data[i]); +#else + dest[i] = m_data[i]; +#endif //B3_USE_PLACEMENT_NEW + } + + B3_FORCE_INLINE void init() + { + //PCK: added this line + m_ownsMemory = true; + m_data = 0; + m_size = 0; + m_capacity = 0; + } + B3_FORCE_INLINE void destroy(int first, int last) + { + int i; + for (i = first; i < last; i++) + { + m_data[i].~T(); + } + } + + B3_FORCE_INLINE void* allocate(int size) + { + if (size) + return m_allocator.allocate(size); + return 0; + } + + B3_FORCE_INLINE void deallocate() + { + if (m_data) + { + //PCK: enclosed the deallocation in this block + if (m_ownsMemory) + { + m_allocator.deallocate(m_data); + } + m_data = 0; + } + } + +public: + b3AlignedObjectArray() + { + init(); + } + + ~b3AlignedObjectArray() + { + clear(); + } + + ///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead. + b3AlignedObjectArray(const b3AlignedObjectArray& otherArray) + { + init(); + + int otherSize = otherArray.size(); + resize(otherSize); + otherArray.copy(0, otherSize, m_data); + } + + /// return the number of elements in the array + B3_FORCE_INLINE int size() const + { + return m_size; + } + + B3_FORCE_INLINE const T& at(int n) const + { + b3Assert(n >= 0); + b3Assert(n < size()); + return m_data[n]; + } + + B3_FORCE_INLINE T& at(int n) + { + b3Assert(n >= 0); + b3Assert(n < size()); + return m_data[n]; + } + + B3_FORCE_INLINE const T& operator[](int n) const + { + b3Assert(n >= 0); + b3Assert(n < size()); + return m_data[n]; + } + + B3_FORCE_INLINE T& operator[](int n) + { + b3Assert(n >= 0); + b3Assert(n < size()); + return m_data[n]; + } + + ///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations. + B3_FORCE_INLINE void clear() + { + destroy(0, size()); + + deallocate(); + + init(); + } + + B3_FORCE_INLINE void pop_back() + { + b3Assert(m_size > 0); + 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 < newsize; i++) + { + new (&m_data[i]) T(fillData); + } +#endif //B3_USE_PLACEMENT_NEW + } + + m_size = newsize; + } + B3_FORCE_INLINE T& expandNonInitializing() + { + int sz = size(); + if (sz == capacity()) + { + reserve(allocSize(size())); + } + m_size++; + + return m_data[sz]; + } + + B3_FORCE_INLINE T& expand(const T& fillValue = T()) + { + int sz = size(); + if (sz == capacity()) + { + reserve(allocSize(size())); + } + m_size++; +#ifdef B3_USE_PLACEMENT_NEW + new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory) +#endif + + return m_data[sz]; + } + + B3_FORCE_INLINE void push_back(const T& _Val) + { + int sz = size(); + if (sz == capacity()) + { + reserve(allocSize(size())); + } + +#ifdef B3_USE_PLACEMENT_NEW + new (&m_data[m_size]) T(_Val); +#else + m_data[size()] = _Val; +#endif //B3_USE_PLACEMENT_NEW + + m_size++; + } + + /// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve() + B3_FORCE_INLINE int capacity() const + { + return m_capacity; + } + + B3_FORCE_INLINE void reserve(int _Count) + { // determine new minimum length of allocated storage + if (capacity() < _Count) + { // not enough room, reallocate + T* s = (T*)allocate(_Count); + b3Assert(s); + if (s == 0) + { + b3Error("b3AlignedObjectArray reserve out-of-memory\n"); + _Count = 0; + m_size = 0; + } + copy(0, size(), s); + + destroy(0, size()); + + deallocate(); + + //PCK: added this line + m_ownsMemory = true; + + m_data = s; + + m_capacity = _Count; + } + } + + class less + { + public: + bool operator()(const T& a, const T& b) + { + return (a < b); + } + }; + + template + 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 < j) + quickSortInternal(CompareFunc, lo, j); + if (i < hi) + quickSortInternal(CompareFunc, i, hi); + } + + template + 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 < size(); i++) + { + if (m_data[i] == key) + { + index = i; + break; + } + } + return index; + } + + int findLinearSearch2(const T& key) const + { + int index = -1; + int i; + + for (i = 0; i < size(); i++) + { + if (m_data[i] == key) + { + index = i; + break; + } + } + return index; + } + + void remove(const T& key) + { + int findIndex = findLinearSearch(key); + if (findIndex < size()) + { + swap(findIndex, size() - 1); + pop_back(); + } + } + + //PCK: whole function + void initializeFromBuffer(void* buffer, int size, int capacity) + { + clear(); + m_ownsMemory = false; + m_data = (T*)buffer; + m_size = size; + m_capacity = capacity; + } + + void copyFromArray(const b3AlignedObjectArray& otherArray) + { + int otherSize = otherArray.size(); + resize(otherSize); + otherArray.copy(0, otherSize, m_data); + } + + void removeAtIndex(int index) + { + if (index < size()) + { + swap(index, size() - 1); + pop_back(); + } + } +}; + +#endif //B3_OBJECT_ARRAY__ diff --git a/Engine/lib/bullet/src/Bullet3Common/b3CommandLineArgs.h b/Engine/lib/bullet/src/Bullet3Common/b3CommandLineArgs.h new file mode 100644 index 000000000..5fe4f25f8 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3CommandLineArgs.h @@ -0,0 +1,106 @@ +#ifndef COMMAND_LINE_ARGS_H +#define COMMAND_LINE_ARGS_H + +/****************************************************************************** + * Command-line parsing + ******************************************************************************/ +#include +#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/Engine/lib/bullet/src/Bullet3Common/b3FileUtils.h b/Engine/lib/bullet/src/Bullet3Common/b3FileUtils.h new file mode 100644 index 000000000..9ded17eaa --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3FileUtils.h @@ -0,0 +1,133 @@ +#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 && i < numPrefixes; i++) + { +#ifdef _MSC_VER + sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName); +#else + sprintf(relativeFileName, "%s%s", prefix[i], orgFileName); +#endif + f = fopen(relativeFileName, "rb"); + if (f) + { + fileFound = true; + break; + } + } + if (f) + { + fclose(f); + } + + return fileFound; + } + + 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; + } + + static int extractPath(const char* fileName, char* path, int maxPathLength) + { + const char* stripped = strip2(fileName, "/"); + stripped = strip2(stripped, "\\"); + + ptrdiff_t len = stripped - fileName; + b3Assert((len + 1) < maxPathLength); + + if (len && ((len + 1) < maxPathLength)) + { + for (int i = 0; i < len; i++) + { + path[i] = fileName[i]; + } + path[len] = 0; + } + else + { + len = 0; + b3Assert(maxPathLength > 0); + 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 < len; i++) + { + str[i] = toLowerChar(str[i]); + } + } + + /*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; + } + */ +}; +#endif //B3_FILE_UTILS_H diff --git a/Engine/lib/bullet/src/Bullet3Common/b3HashMap.h b/Engine/lib/bullet/src/Bullet3Common/b3HashMap.h new file mode 100644 index 000000000..3009e2cf2 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3HashMap.h @@ -0,0 +1,462 @@ +/* +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_HASH_MAP_H +#define B3_HASH_MAP_H + +#include "b3AlignedObjectArray.h" + +#include + +///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 < len; i++) + { + hash = hash ^ (m_string[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 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 < curHashtableSize; i++) + { + //const Value& value = m_valueArray[i]; + //const Key& key = m_keyArray[i]; + + int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity() - 1); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + } + } + +public: + void insert(const Key& key, const Value& value) + { + int hash = key.getHash() & (m_valueArray.capacity() - 1); + + //replace value if the key is already there + int index = findIndex(key); + if (index != B3_HASH_NULL) + { + m_valueArray[index] = value; + return; + } + + int count = m_valueArray.size(); + int oldCapacity = m_valueArray.capacity(); + m_valueArray.push_back(value); + m_keyArray.push_back(key); + + int newCapacity = m_valueArray.capacity(); + if (oldCapacity < newCapacity) + { + growTables(key); + //hash with new capacity + hash = key.getHash() & (m_valueArray.capacity() - 1); + } + m_next[count] = m_hashTable[hash]; + m_hashTable[hash] = count; + } + + void remove(const Key& key) + { + int hash = key.getHash() & (m_valueArray.capacity() - 1); + + int pairIndex = findIndex(key); + + if (pairIndex == B3_HASH_NULL) + { + return; + } + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + b3Assert(index != B3_HASH_NULL); + + int previous = B3_HASH_NULL; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != B3_HASH_NULL) + { + 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_valueArray.size() - 1; + + // If the removed pair is the last pair, we are done. + if (lastPairIndex == pairIndex) + { + m_valueArray.pop_back(); + m_keyArray.pop_back(); + return; + } + + // Remove the last pair from the hash table. + int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity() - 1); + + index = m_hashTable[lastHash]; + b3Assert(index != B3_HASH_NULL); + + previous = B3_HASH_NULL; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != B3_HASH_NULL) + { + 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_valueArray[pairIndex] = m_valueArray[lastPairIndex]; + m_keyArray[pairIndex] = m_keyArray[lastPairIndex]; + + // Insert the last pair into the hash table + m_next[pairIndex] = m_hashTable[lastHash]; + m_hashTable[lastHash] = pairIndex; + + m_valueArray.pop_back(); + m_keyArray.pop_back(); + } + + int size() const + { + return m_valueArray.size(); + } + + const Value* getAtIndex(int index) const + { + b3Assert(index < m_valueArray.size()); + + return &m_valueArray[index]; + } + + Value* getAtIndex(int index) + { + b3Assert(index < m_valueArray.size()); + + return &m_valueArray[index]; + } + + Key getKeyAtIndex(int index) + { + b3Assert(index < m_keyArray.size()); + return m_keyArray[index]; + } + + const Key getKeyAtIndex(int index) const + { + b3Assert(index < m_keyArray.size()); + return m_keyArray[index]; + } + + Value* operator[](const Key& key) + { + return find(key); + } + + const Value* find(const Key& key) const + { + int index = findIndex(key); + if (index == B3_HASH_NULL) + { + return NULL; + } + return &m_valueArray[index]; + } + + Value* find(const Key& key) + { + int index = findIndex(key); + if (index == B3_HASH_NULL) + { + return NULL; + } + return &m_valueArray[index]; + } + + int findIndex(const Key& key) const + { + unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1); + + if (hash >= (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/Engine/lib/bullet/src/Bullet3Common/b3Logging.cpp b/Engine/lib/bullet/src/Bullet3Common/b3Logging.cpp new file mode 100644 index 000000000..9c9f7c09e --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3Logging.cpp @@ -0,0 +1,145 @@ +/* +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/Engine/lib/bullet/src/Bullet3Common/b3Logging.h b/Engine/lib/bullet/src/Bullet3Common/b3Logging.h new file mode 100644 index 000000000..9c92b12eb --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3Logging.h @@ -0,0 +1,86 @@ + +#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/Engine/lib/bullet/src/Bullet3Common/b3Matrix3x3.h b/Engine/lib/bullet/src/Bullet3Common/b3Matrix3x3.h new file mode 100644 index 000000000..6c46536a8 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3Matrix3x3.h @@ -0,0 +1,1354 @@ +/* +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/Engine/lib/bullet/src/Bullet3Common/b3MinMax.h b/Engine/lib/bullet/src/Bullet3Common/b3MinMax.h new file mode 100644 index 000000000..c09c3db3f --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3MinMax.h @@ -0,0 +1,69 @@ +/* +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/Engine/lib/bullet/src/Bullet3Common/b3PoolAllocator.h b/Engine/lib/bullet/src/Bullet3Common/b3PoolAllocator.h new file mode 100644 index 000000000..ed56bc627 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Common/b3QuadWord.h b/Engine/lib/bullet/src/Bullet3Common/b3QuadWord.h new file mode 100644 index 000000000..0def305fa --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3QuadWord.h @@ -0,0 +1,242 @@ +/* +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/Engine/lib/bullet/src/Bullet3Common/b3Quaternion.h b/Engine/lib/bullet/src/Bullet3Common/b3Quaternion.h new file mode 100644 index 000000000..4fdd72dcc --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3Quaternion.h @@ -0,0 +1,908 @@ +/* +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& axis1, const b3Scalar& _angle) + { + b3Vector3 axis = axis1; + axis.safeNormalize(); + + b3Scalar d = axis.length(); + b3Assert(d != b3Scalar(0.0)); + if (d < B3_EPSILON) + { + setValue(0, 0, 0, 1); + } + else + { + 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/Engine/lib/bullet/src/Bullet3Common/b3Random.h b/Engine/lib/bullet/src/Bullet3Common/b3Random.h new file mode 100644 index 000000000..c2e21496c --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3Random.h @@ -0,0 +1,46 @@ +/* +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/Engine/lib/bullet/src/Bullet3Common/b3ResizablePool.h b/Engine/lib/bullet/src/Bullet3Common/b3ResizablePool.h new file mode 100644 index 000000000..cafe3ff39 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3ResizablePool.h @@ -0,0 +1,171 @@ + +#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 < m_bodyHandles.size(); i++) + { + if (m_bodyHandles[i].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED) + { + usedHandles.push_back(i); + } + } + } + + T* getHandle(int handle) + { + b3Assert(handle >= 0); + b3Assert(handle < m_bodyHandles.size()); + if ((handle < 0) || (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()); + if ((handle < 0) || (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 diff --git a/Engine/lib/bullet/src/Bullet3Common/b3Scalar.h b/Engine/lib/bullet/src/Bullet3Common/b3Scalar.h new file mode 100644 index 000000000..0db5eb6f4 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3Scalar.h @@ -0,0 +1,685 @@ +/* +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 (x < b3Scalar(-1)) x = b3Scalar(-1); + if (x > b3Scalar(1)) x = b3Scalar(1); + return acos(x); +} +B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) +{ + if (x < b3Scalar(-1)) x = b3Scalar(-1); + if (x > b3Scalar(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 (x < b3Scalar(-1)) + x = b3Scalar(-1); + if (x > b3Scalar(1)) + x = b3Scalar(1); + return acosf(x); +} +B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) +{ + if (x < b3Scalar(-1)) + x = b3Scalar(-1); + if (x > b3Scalar(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/Engine/lib/bullet/src/Bullet3Common/b3StackAlloc.h b/Engine/lib/bullet/src/Bullet3Common/b3StackAlloc.h new file mode 100644 index 000000000..4972236ac --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3StackAlloc.h @@ -0,0 +1,118 @@ +/* +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 (nus < totalsize) + { + usedsize = nus; + return (data + (usedsize - size)); + } + b3Assert(0); + //&& (L"Not enough memory")); + + return (0); + } + B3_FORCE_INLINE b3Block* beginBlock() + { + b3Block* pb = (b3Block*)allocate(sizeof(b3Block)); + pb->previous = 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/Engine/lib/bullet/src/Bullet3Common/b3Transform.h b/Engine/lib/bullet/src/Bullet3Common/b3Transform.h new file mode 100644 index 000000000..149da9d14 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3Transform.h @@ -0,0 +1,286 @@ +/* +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/Engine/lib/bullet/src/Bullet3Common/b3TransformUtil.h b/Engine/lib/bullet/src/Bullet3Common/b3TransformUtil.h new file mode 100644 index 000000000..1850a9be5 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3TransformUtil.h @@ -0,0 +1,210 @@ +/* +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/Engine/lib/bullet/src/Bullet3Common/b3Vector3.cpp b/Engine/lib/bullet/src/Bullet3Common/b3Vector3.cpp new file mode 100644 index 000000000..100fb774c --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3Vector3.cpp @@ -0,0 +1,1637 @@ +/* + 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 < localCount / 4; i++, index++) + { // 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 + } + } +#endif //__APPLE__ + } + + // process the last few points + if (count & 3) + { + float4 v0, v1, v2, x, y, z; + switch (count & 3) + { + case 3: + { + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + + // Calculate 3 dot products, transpose, duplicate v2 + float4 lo0 = _mm_movelh_ps(v0, v1); // xyxy.lo + float4 hi0 = _mm_movehl_ps(v1, v0); // z?z?.lo + lo0 = lo0 * vLo; + z = _mm_shuffle_ps(hi0, v2, 0xa8); // z0z1z2z2 + z = z * vHi; + float4 lo1 = _mm_movelh_ps(v2, v2); // xyxy + lo1 = lo1 * vLo; + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + } + break; + case 2: + { + v0 = vertices[0]; + v1 = vertices[1]; + float4 xy = _mm_movelh_ps(v0, v1); + z = _mm_movehl_ps(v1, v0); + xy = xy * vLo; + z = _mm_shuffle_ps(z, z, 0xa8); + x = _mm_shuffle_ps(xy, xy, 0xa8); + y = _mm_shuffle_ps(xy, xy, 0xfd); + z = z * vHi; + } + break; + case 1: + { + float4 xy = vertices[0]; + z = _mm_shuffle_ps(xy, xy, 0xaa); + xy = xy * vLo; + z = z * vHi; + x = _mm_shuffle_ps(xy, xy, 0); + y = _mm_shuffle_ps(xy, xy, 0x55); + } + break; + } + 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 + index++; + } + + // if we found a new max. + if (0 == segment || 0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(max, dotMax))) + { // we found a new max. Search for it + // find max across the max vector, place in all elements of max -- big latency hit here + max = _mm_max_ps(max, (float4)_mm_shuffle_ps(max, max, 0x4e)); + max = _mm_max_ps(max, (float4)_mm_shuffle_ps(max, max, 0xb1)); + + // It is slightly faster to do this part in scalar code when count < 8. However, the common case for + // this where it actually makes a difference is handled in the early out at the top of the function, + // so it is less than a 1% difference here. I opted for improved code size, fewer branches and reduced + // complexity, and removed it. + + dotMax = max; + + // scan for the first occurence of max in the array + 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 + { + } + maxIndex = 4 * index + segment + indexTable[test]; + } + + _mm_store_ss(dotResult, dotMax); + return maxIndex; +} + +long b3_mindot_large(const float *vv, const float *vec, unsigned long count, float *dotResult); + +long b3_mindot_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 dotmin = 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 minIndex = -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 min; + // Faster loop without cleanup code for full tiles + for (segment = 0; segment + STACK_ARRAY_COUNT * 4 <= count; segment += STACK_ARRAY_COUNT * 4) + { + min = dotmin; + + 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; + 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. + } + + // If we found a new min + if (0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(min, dotmin))) + { + // copy the new min across all lanes of our min accumulator + min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0x4e)); + min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0xb1)); + + dotmin = min; + + // find first occurrence of that min + size_t test; + for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], min))); index++) // local_count must be a multiple of 4 + { + } + // record where it is. + minIndex = 4 * index + segment + indexTable[test]; + } + } + + // account for work we've already done + count -= segment; + + // Deal with the last < STACK_ARRAY_COUNT vectors + min = dotmin; + 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; + 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 < localCount / 4; i++, index++) + { // 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 max is never NaN even if x is nan + } + } + +#endif + } + + // process the last few points + if (count & 3) + { + float4 v0, v1, v2, x, y, z; + switch (count & 3) + { + case 3: + { + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + + // Calculate 3 dot products, transpose, duplicate v2 + float4 lo0 = _mm_movelh_ps(v0, v1); // xyxy.lo + float4 hi0 = _mm_movehl_ps(v1, v0); // z?z?.lo + lo0 = lo0 * vLo; + z = _mm_shuffle_ps(hi0, v2, 0xa8); // z0z1z2z2 + z = z * vHi; + float4 lo1 = _mm_movelh_ps(v2, v2); // xyxy + lo1 = lo1 * vLo; + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + } + break; + case 2: + { + v0 = vertices[0]; + v1 = vertices[1]; + float4 xy = _mm_movelh_ps(v0, v1); + z = _mm_movehl_ps(v1, v0); + xy = xy * vLo; + z = _mm_shuffle_ps(z, z, 0xa8); + x = _mm_shuffle_ps(xy, xy, 0xa8); + y = _mm_shuffle_ps(xy, xy, 0xfd); + z = z * vHi; + } + break; + case 1: + { + float4 xy = vertices[0]; + z = _mm_shuffle_ps(xy, xy, 0xaa); + xy = xy * vLo; + z = z * vHi; + x = _mm_shuffle_ps(xy, xy, 0); + y = _mm_shuffle_ps(xy, xy, 0x55); + } + break; + } + 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 + index++; + } + + // if we found a new min. + if (0 == segment || 0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(min, dotmin))) + { // we found a new min. Search for it + // find min across the min vector, place in all elements of min -- big latency hit here + min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0x4e)); + min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0xb1)); + + // It is slightly faster to do this part in scalar code when count < 8. However, the common case for + // this where it actually makes a difference is handled in the early out at the top of the function, + // so it is less than a 1% difference here. I opted for improved code size, fewer branches and reduced + // complexity, and removed it. + + dotmin = min; + + // scan for the first occurence of min in the array + size_t test; + for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], min))); index++) // local_count must be a multiple of 4 + { + } + minIndex = 4 * index + segment + indexTable[test]; + } + + _mm_store_ss(dotResult, dotmin); + return minIndex; +} + +#elif defined B3_USE_NEON +#define ARM_NEON_GCC_COMPATIBILITY 1 +#include + +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/Engine/lib/bullet/src/Bullet3Common/b3Vector3.h b/Engine/lib/bullet/src/Bullet3Common/b3Vector3.h new file mode 100644 index 000000000..a70d68d6e --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/b3Vector3.h @@ -0,0 +1,1303 @@ +/* +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)) & 0xff) +//#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] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2); + } + + /**@brief Return the axis with the largest value + * Note return values are 0,1,2 for x, y, or z */ + B3_FORCE_INLINE int maxAxis() const + { + return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0); + } + + B3_FORCE_INLINE int furthestAxis() const + { + return absolute().minAxis(); + } + + B3_FORCE_INLINE int closestAxis() const + { + return absolute().maxAxis(); + } + + B3_FORCE_INLINE void setInterpolate3(const b3Vector3& v0, const b3Vector3& v1, b3Scalar rt) + { +#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE) + __m128 vrt = _mm_load_ss(&rt); // (rt 0 0 0) + b3Scalar s = b3Scalar(1.0) - rt; + __m128 vs = _mm_load_ss(&s); // (S 0 0 0) + vs = b3_pshufd_ps(vs, 0x80); // (S S S 0.0) + __m128 r0 = _mm_mul_ps(v0.mVec128, vs); + vrt = b3_pshufd_ps(vrt, 0x80); // (rt rt rt 0.0) + __m128 r1 = _mm_mul_ps(v1.mVec128, vrt); + __m128 tmp3 = _mm_add_ps(r0, r1); + mVec128 = tmp3; +#elif defined(B3_USE_NEON) + float32x4_t vl = vsubq_f32(v1.mVec128, v0.mVec128); + vl = vmulq_n_f32(vl, rt); + mVec128 = vaddq_f32(vl, v0.mVec128); +#else + b3Scalar s = b3Scalar(1.0) - rt; + m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0]; + m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1]; + m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2]; + //don't do the unused w component + // m_co[3] = s * v0[3] + rt * v1[3]; +#endif + } + + /**@brief Return the linear interpolation between this and another vector + * @param v The other vector + * @param t The ration of this to v (t = 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/Engine/lib/bullet/src/Bullet3Common/premake4.lua b/Engine/lib/bullet/src/Bullet3Common/premake4.lua new file mode 100644 index 000000000..f03573ec2 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Common/shared/b3Float4.h b/Engine/lib/bullet/src/Bullet3Common/shared/b3Float4.h new file mode 100644 index 000000000..d8a9f4741 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/shared/b3Float4.h @@ -0,0 +1,90 @@ +#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/Engine/lib/bullet/src/Bullet3Common/shared/b3Int2.h b/Engine/lib/bullet/src/Bullet3Common/shared/b3Int2.h new file mode 100644 index 000000000..7b84de443 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/shared/b3Int2.h @@ -0,0 +1,63 @@ +/* +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/Engine/lib/bullet/src/Bullet3Common/shared/b3Int4.h b/Engine/lib/bullet/src/Bullet3Common/shared/b3Int4.h new file mode 100644 index 000000000..f6a175424 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/shared/b3Int4.h @@ -0,0 +1,71 @@ +#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/Engine/lib/bullet/src/Bullet3Common/shared/b3Mat3x3.h b/Engine/lib/bullet/src/Bullet3Common/shared/b3Mat3x3.h new file mode 100644 index 000000000..ce6482b5a --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/shared/b3Mat3x3.h @@ -0,0 +1,157 @@ + +#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/Engine/lib/bullet/src/Bullet3Common/shared/b3PlatformDefinitions.h b/Engine/lib/bullet/src/Bullet3Common/shared/b3PlatformDefinitions.h new file mode 100644 index 000000000..b72bee931 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Common/shared/b3Quat.h b/Engine/lib/bullet/src/Bullet3Common/shared/b3Quat.h new file mode 100644 index 000000000..940610c77 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Common/shared/b3Quat.h @@ -0,0 +1,100 @@ +#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/Engine/lib/bullet/src/Bullet3Dynamics/CMakeLists.txt b/Engine/lib/bullet/src/Bullet3Dynamics/CMakeLists.txt new file mode 100644 index 000000000..94c120d9b --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h new file mode 100644 index 000000000..049c9116f --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h @@ -0,0 +1,149 @@ +/* +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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp new file mode 100644 index 000000000..ace4b1838 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp @@ -0,0 +1,103 @@ + +#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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h new file mode 100644 index 000000000..64809666e --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h @@ -0,0 +1,34 @@ + +#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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp new file mode 100644 index 000000000..0d5bb2014 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp @@ -0,0 +1,737 @@ +/* +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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h new file mode 100644 index 000000000..1597809db --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h @@ -0,0 +1,517 @@ +/* +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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h new file mode 100644 index 000000000..13269debf --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h @@ -0,0 +1,150 @@ +/* +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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp new file mode 100644 index 000000000..b7050b107 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp @@ -0,0 +1,1696 @@ +/* +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; j < numContacts; j++) + { + b3ContactPoint cp; + getContactPoint(manifold, j, cp); + + if (cp.getDistance() <= getContactProcessingThreshold(manifold)) + { + b3Vector3 rel_pos1; + b3Vector3 rel_pos2; + b3Scalar relaxation; + b3Scalar rel_vel; + b3Vector3 vel; + + int frictionIndex = m_tmpSolverContactConstraintPool.size(); + b3SolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); + // b3RigidBody* rb0 = b3RigidBody::upcast(colObj0); + // b3RigidBody* rb1 = b3RigidBody::upcast(colObj1); + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + + solverConstraint.m_originalContactPoint = &cp; + + setupContactConstraint(bodies, inertias, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); + + // const b3Vector3& pos1 = cp.getPositionWorldOnA(); + // const b3Vector3& pos2 = cp.getPositionWorldOnB(); + + /////setup the friction constraints + + solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); + + b3Vector3 angVelA, angVelB; + solverBodyA->getAngularVelocity(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; i < numConstraints; i++) + { + int bodyIndexA = constraints[i]->getRigidBodyA(); + 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; i < numManifolds; i++) + { + int bodyIndexA = manifoldPtr[i].getBodyA(); + int bodyIndexB = manifoldPtr[i].getBodyB(); + if (m_usePgs) + { + m_bodyCount[bodyIndexA] = -1; + m_bodyCount[bodyIndexB] = -1; + } + else + { + if (bodies[bodyIndexA].m_invMass) + { + //m_bodyCount[bodyIndexA]+=manifoldPtr[i].getNPoints(); + m_bodyCount[bodyIndexA]++; + } + else + m_bodyCount[bodyIndexA] = -1; + + if (bodies[bodyIndexB].m_invMass) + // m_bodyCount[bodyIndexB]+=manifoldPtr[i].getNPoints(); + m_bodyCount[bodyIndexB]++; + else + m_bodyCount[bodyIndexB] = -1; + } + } + + if (1) + { + int j; + for (j = 0; j < numConstraints; j++) + { + b3TypedConstraint* constraint = constraints[j]; + + constraint->internalSetAppliedImpulse(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; i < numConstraints; i++) + { + b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + b3JointFeedback* fb = constraints[i]->getJointFeedback(); + 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; i < numConstraints; i++) + { + const b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + + if (info1.m_numConstraintRows) + { + b3Assert(currentRow < totalNumRows); + + b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; + b3TypedConstraint* constraint = constraints[i]; + + b3RigidBodyData& rbA = bodies[constraint->getRigidBodyA()]; + //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; j < info1.m_numConstraintRows; j++) + { + memset(¤tConstraintRow[j], 0, sizeof(b3SolverConstraint)); + currentConstraintRow[j].m_lowerLimit = -B3_INFINITY; + currentConstraintRow[j].m_upperLimit = B3_INFINITY; + currentConstraintRow[j].m_appliedImpulse = 0.f; + currentConstraintRow[j].m_appliedPushImpulse = 0.f; + currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; + currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; + currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; + } + + 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); + + 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 < info1.m_numConstraintRows; j++) + { + b3SolverConstraint& solverConstraint = currentConstraintRow[j]; + + if (solverConstraint.m_upperLimit >= 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; i < numManifolds; i++) + { + b3Contact4& manifold = manifoldPtr[i]; + convertContact(bodies, inertias, &manifold, infoGlobal); + } + } + } + + // b3ContactSolverInfo info = infoGlobal; + + int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); + int numConstraintPool = m_tmpSolverContactConstraintPool.size(); + int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); + + ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); + if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) + m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2); + else + m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); + + m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); + { + int i; + for (i = 0; i < numNonContactPool; i++) + { + m_orderNonContactConstraintPool[i] = i; + } + for (i = 0; i < numConstraintPool; i++) + { + m_orderTmpConstraintPool[i] = i; + } + for (i = 0; i < numFrictionPool; i++) + { + m_orderFrictionConstraintPool[i] = i; + } + } + + return 0.f; +} + +b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) +{ + int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); + int numConstraintPool = m_tmpSolverContactConstraintPool.size(); + int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); + + if (infoGlobal.m_solverMode & B3_SOLVER_RANDMIZE_ORDER) + { + if (1) // uncomment this for a bit less random ((iteration & 7) == 0) + { + for (int j = 0; j < numNonContactPool; ++j) + { + int tmp = m_orderNonContactConstraintPool[j]; + int swapi = b3RandInt2(j + 1); + m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; + m_orderNonContactConstraintPool[swapi] = tmp; + } + + //contact/friction constraints are not solved more than + if (iteration < infoGlobal.m_numIterations) + { + for (int j = 0; j < numConstraintPool; ++j) + { + int tmp = m_orderTmpConstraintPool[j]; + int swapi = b3RandInt2(j + 1); + m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; + m_orderTmpConstraintPool[swapi] = tmp; + } + + for (int j = 0; j < numFrictionPool; ++j) + { + int tmp = m_orderFrictionConstraintPool[j]; + int swapi = b3RandInt2(j + 1); + m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; + m_orderFrictionConstraintPool[swapi] = tmp; + } + } + } + } + + if (infoGlobal.m_solverMode & B3_SOLVER_SIMD) + { + ///solve all joint constraints, using SIMD, if available + for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) + { + b3SolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); + } + + if (iteration < infoGlobal.m_numIterations) + { + ///solve all contact constraints using SIMD, if available + if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int multiplier = (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1; + + for (int c = 0; c < numPoolConstraints; c++) + { + b3Scalar totalImpulse = 0; + + { + const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; + resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + totalImpulse = solveManifold.m_appliedImpulse; + } + bool applyFriction = true; + if (applyFriction) + { + { + b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]]; + + 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); + } + } + + 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; j < numPoolConstraints; j++) + { + const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + } + + if (!m_usePgs) + averageVelocities(); + + ///solve all friction constraints, using SIMD, if available + + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (j = 0; j < numFrictionPoolConstraints; j++) + { + b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + 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); + } + } + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (j = 0; j < numRollingFrictionPoolConstraints; j++) + { + b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; + b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; + if (totalImpulse > b3Scalar(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; j < m_tmpSolverNonContactConstraintPool.size(); j++) + { + b3SolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); + } + + if (iteration < infoGlobal.m_numIterations) + { + ///solve all contact constraints + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (int j = 0; j < numPoolConstraints; j++) + { + const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + } + ///solve all friction constraints + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (int j = 0; j < numFrictionPoolConstraints; j++) + { + b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + if (totalImpulse > b3Scalar(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; j < numRollingFrictionPoolConstraints; j++) + { + b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; + b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; + if (totalImpulse > b3Scalar(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; iteration++) + { + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + for (j = 0; j < numPoolConstraints; j++) + { + const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + + resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + } + } + } + } + else + { + for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) + { + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + for (j = 0; j < numPoolConstraints; j++) + { + const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + + resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + } + } + } + } + } +} + +b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) +{ + B3_PROFILE("solveGroupCacheFriendlyIterations"); + + { + ///this is a special step to resolve penetrations (just for contacts) + solveGroupCacheFriendlySplitImpulseIterations(constraints, numConstraints, infoGlobal); + + int maxIterations = m_maxOverrideNumSolverIterations > 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; i < m_tmpSolverBodyPool.size(); i++) + { + if (!m_tmpSolverBodyPool[i].m_invMass.isZero()) + { + int orgBodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; + m_deltaLinearVelocities[orgBodyIndex] += m_tmpSolverBodyPool[i].getDeltaLinearVelocity(); + m_deltaAngularVelocities[orgBodyIndex] += m_tmpSolverBodyPool[i].getDeltaAngularVelocity(); + } + } + + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) + { + int orgBodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; + + if (!m_tmpSolverBodyPool[i].m_invMass.isZero()) + { + b3Assert(m_bodyCount[orgBodyIndex] == m_bodyCountCheck[orgBodyIndex]); + + b3Scalar factor = 1.f / b3Scalar(m_bodyCount[orgBodyIndex]); + + m_tmpSolverBodyPool[i].m_deltaLinearVelocity = m_deltaLinearVelocities[orgBodyIndex] * factor; + m_tmpSolverBodyPool[i].m_deltaAngularVelocity = m_deltaAngularVelocities[orgBodyIndex] * factor; + } + } +} + +b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal) +{ + B3_PROFILE("solveGroupCacheFriendlyFinish"); + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int i, j; + + if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) + { + for (j = 0; j < numPoolConstraints; j++) + { + const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; + b3ContactPoint* pt = (b3ContactPoint*)solveManifold.m_originalContactPoint; + b3Assert(pt); + pt->m_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; j < numPoolConstraints; j++) + { + const b3SolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; + b3TypedConstraint* constr = (b3TypedConstraint*)solverConstr.m_originalContactPoint; + b3JointFeedback* fb = constr->getJointFeedback(); + 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; i < m_tmpSolverBodyPool.size(); i++) + { + int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; + //b3Assert(i==bodyIndex); + + b3RigidBodyData* body = &bodies[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 + { + 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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h new file mode 100644 index 000000000..5b616541d --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h @@ -0,0 +1,133 @@ +#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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp new file mode 100644 index 000000000..cfa7c7dd1 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp @@ -0,0 +1,190 @@ +/* +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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h new file mode 100644 index 000000000..14762a3e3 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h @@ -0,0 +1,153 @@ +/* +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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h new file mode 100644 index 000000000..196d0e579 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h @@ -0,0 +1,281 @@ +/* +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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h new file mode 100644 index 000000000..4927ae428 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h @@ -0,0 +1,73 @@ +/* +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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp new file mode 100644 index 000000000..885e277d8 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp @@ -0,0 +1,151 @@ +/* +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/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h new file mode 100644 index 000000000..f74aec4d3 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h @@ -0,0 +1,469 @@ +/* +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; + } +} + +// clang-format off +///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; + +}; + +// clang-format on + +/*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/Engine/lib/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp b/Engine/lib/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp new file mode 100644 index 000000000..f1080d9d5 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp @@ -0,0 +1,447 @@ +#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; i < this->getNumBodies(); 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 < m_maxNumBatches; bb++) + { + usedBodies.resize(0); + for (int ic = m_nConstraints - 1; ic >= 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; iter < m_nIterations; iter++) + { + b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0); + task.m_solveFriction = false; + task.run(0); + } + + for (int iter = 0; iter < m_nIterations; iter++) + { + b3SolveTask task(m_data->m_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; i < m_data->m_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/Engine/lib/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h b/Engine/lib/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h new file mode 100644 index 000000000..9c65419f2 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h @@ -0,0 +1,62 @@ +/* +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/Engine/lib/bullet/src/Bullet3Dynamics/premake4.lua b/Engine/lib/bullet/src/Bullet3Dynamics/premake4.lua new file mode 100644 index 000000000..e05b2cd19 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h b/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h new file mode 100644 index 000000000..cf2eed0e7 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h @@ -0,0 +1,31 @@ +#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/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h b/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h new file mode 100644 index 000000000..3e72f1c3f --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h @@ -0,0 +1,148 @@ + + +#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; i < src->m_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 (i < src->m_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/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3Inertia.h b/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3Inertia.h new file mode 100644 index 000000000..602a1335a --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3Inertia.h @@ -0,0 +1,14 @@ + + +#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/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h b/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h new file mode 100644 index 000000000..56d9118f9 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h @@ -0,0 +1,106 @@ + + +#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/Engine/lib/bullet/src/Bullet3Geometry/CMakeLists.txt b/Engine/lib/bullet/src/Bullet3Geometry/CMakeLists.txt new file mode 100644 index 000000000..820687270 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Geometry/b3AabbUtil.h b/Engine/lib/bullet/src/Bullet3Geometry/b3AabbUtil.h new file mode 100644 index 000000000..396a40145 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Geometry/b3AabbUtil.h @@ -0,0 +1,217 @@ +/* +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/Engine/lib/bullet/src/Bullet3Geometry/b3ConvexHullComputer.cpp b/Engine/lib/bullet/src/Bullet3Geometry/b3ConvexHullComputer.cpp new file mode 100644 index 000000000..b37652456 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Geometry/b3ConvexHullComputer.cpp @@ -0,0 +1,2745 @@ +/* +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/Engine/lib/bullet/src/Bullet3Geometry/b3ConvexHullComputer.h b/Engine/lib/bullet/src/Bullet3Geometry/b3ConvexHullComputer.h new file mode 100644 index 000000000..8852c5a52 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Geometry/b3ConvexHullComputer.h @@ -0,0 +1,99 @@ +/* +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/Engine/lib/bullet/src/Bullet3Geometry/b3GeometryUtil.cpp b/Engine/lib/bullet/src/Bullet3Geometry/b3GeometryUtil.cpp new file mode 100644 index 000000000..c4041003c --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Geometry/b3GeometryUtil.cpp @@ -0,0 +1,174 @@ +/* +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; i < numbrushes; i++) + { + const b3Vector3& N1 = planeEquations[i]; + b3Scalar dist = b3Scalar(N1.dot(point)) + b3Scalar(N1[3]) - margin; + if (dist > b3Scalar(0.)) + { + return false; + } + } + return true; +} + +bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray& vertices, b3Scalar margin) +{ + int numvertices = vertices.size(); + for (int i = 0; i < numvertices; i++) + { + const b3Vector3& N1 = vertices[i]; + b3Scalar dist = b3Scalar(planeNormal.dot(N1)) + b3Scalar(planeNormal[3]) - margin; + if (dist > b3Scalar(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 < numbrushes; i++) + { + const b3Vector3& N1 = planeEquations[i]; + if (planeEquation.dot(N1) > 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 < numvertices; i++) + { + const b3Vector3& N1 = vertices[i]; + + for (int j = i + 1; j < numvertices; j++) + { + const b3Vector3& N2 = vertices[j]; + + for (int k = j + 1; k < numvertices; k++) + { + const b3Vector3& N3 = vertices[k]; + + b3Vector3 planeEquation, edge0, edge1; + edge0 = N2 - N1; + edge1 = N3 - N1; + b3Scalar normalSign = b3Scalar(1.); + for (int ww = 0; ww < 2; ww++) + { + planeEquation = normalSign * edge0.cross(edge1); + if (planeEquation.length2() > 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 < numbrushes; i++) + { + const b3Vector3& N1 = planeEquations[i]; + + for (int j = i + 1; j < numbrushes; j++) + { + const b3Vector3& N2 = planeEquations[j]; + + for (int k = j + 1; k < numbrushes; k++) + { + const b3Vector3& N3 = planeEquations[k]; + + b3Vector3 n2n3; + n2n3 = N2.cross(N3); + b3Vector3 n3n1; + n3n1 = N3.cross(N1); + b3Vector3 n1n2; + n1n2 = N1.cross(N2); + + if ((n2n3.length2() > 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/Engine/lib/bullet/src/Bullet3Geometry/b3GeometryUtil.h b/Engine/lib/bullet/src/Bullet3Geometry/b3GeometryUtil.h new file mode 100644 index 000000000..967c8d67e --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Geometry/b3GeometryUtil.h @@ -0,0 +1,36 @@ +/* +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/Engine/lib/bullet/src/Bullet3Geometry/b3GrahamScan2dConvexHull.h b/Engine/lib/bullet/src/Bullet3Geometry/b3GrahamScan2dConvexHull.h new file mode 100644 index 000000000..8881c9a63 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Geometry/b3GrahamScan2dConvexHull.h @@ -0,0 +1,116 @@ +/* +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; i < originalPoints.size(); i++) + hull.push_back(originalPoints[0]); + return; + } + //step1 : find anchor point with smallest projection on axis0 and move it to first location + for (int i = 0; i < originalPoints.size(); i++) + { + // const b3Vector3& left = originalPoints[i]; + // const b3Vector3& right = originalPoints[0]; + b3Scalar projL = originalPoints[i].dot(axis0); + b3Scalar projR = originalPoints[0].dot(axis0); + if (projL < projR) + { + originalPoints.swap(0, i); + } + } + + //also precompute angles + originalPoints[0].m_angle = -1e30f; + for (int i = 1; i < originalPoints.size(); i++) + { + b3Vector3 xvec = axis0; + b3Vector3 ar = originalPoints[i] - originalPoints[0]; + originalPoints[i].m_angle = b3Cross(xvec, ar).dot(normalAxis) / ar.length(); + } + + //step 2: sort all points, based on 'angle' with this anchor + b3AngleCompareFunc comp(originalPoints[0]); + originalPoints.quickSortInternal(comp, 1, originalPoints.size() - 1); + + int i; + for (i = 0; i < 2; i++) + hull.push_back(originalPoints[i]); + + //step 3: keep all 'convex' points and discard concave points (using back tracking) + for (; i != originalPoints.size(); i++) + { + bool isConvex = false; + while (!isConvex && hull.size() > 1) + { + 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/Engine/lib/bullet/src/Bullet3Geometry/premake4.lua b/Engine/lib/bullet/src/Bullet3Geometry/premake4.lua new file mode 100644 index 000000000..cce93f7ee --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h new file mode 100644 index 000000000..b29699252 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h @@ -0,0 +1,42 @@ + +#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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp new file mode 100644 index 000000000..e714fadac --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp @@ -0,0 +1,338 @@ + +#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; i < m_gpuPairs.size(); i++) + { + printf("pair %d = %d,%d\n", i, pairsCpu[i].x, pairsCpu[i].y); + } + + printf("?!?\n"); + } + } + } + + //calculateOverlappingPairsHost(maxPairs); +} +void b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs) +{ + m_hostPairs.resize(0); + m_allAabbsGPU1.copyToHost(m_allAabbsCPU1); + for (int i = 0; i < m_allAabbsCPU1.size(); i++) + { + for (int j = i + 1; j < m_allAabbsCPU1.size(); j++) + { + if (b3TestAabbAgainstAabb2(m_allAabbsCPU1[i].m_minVec, m_allAabbsCPU1[i].m_maxVec, + m_allAabbsCPU1[j].m_minVec, m_allAabbsCPU1[j].m_maxVec)) + { + b3Int4 pair; + int a = m_allAabbsCPU1[j].m_minIndices[3]; + int b = m_allAabbsCPU1[i].m_minIndices[3]; + if (a <= b) + { + pair.x = a; + pair.y = b; //store the original index in the unsorted aabb array + } + else + { + pair.x = b; + pair.y = a; //store the original index in the unsorted aabb array + } + + if (m_hostPairs.size() < maxPairs) + { + m_hostPairs.push_back(pair); + } + } + } + } + + m_gpuPairs.copyFromHost(m_hostPairs); +} + +//call writeAabbsToGpu after done making all changes (createProxy etc) +void b3GpuGridBroadphase::writeAabbsToGpu() +{ + m_allAabbsGPU1.copyFromHost(m_allAabbsCPU1); + m_smallAabbsMappingGPU.copyFromHost(m_smallAabbsMappingCPU); + m_largeAabbsMappingGPU.copyFromHost(m_largeAabbsMappingCPU); +} + +cl_mem b3GpuGridBroadphase::getAabbBufferWS() +{ + return this->m_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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h new file mode 100644 index 000000000..b76cb43b6 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h @@ -0,0 +1,80 @@ +#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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp new file mode 100644 index 000000000..072192868 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp @@ -0,0 +1,557 @@ +/* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this 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); + } +} diff --git a/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.h b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.h new file mode 100644 index 000000000..b39077512 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp new file mode 100644 index 000000000..62ea7a32d --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp @@ -0,0 +1,76 @@ +/* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this 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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.h b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.h new file mode 100644 index 000000000..dda0eea7b --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp new file mode 100644 index 000000000..4126d03ed --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp @@ -0,0 +1,1298 @@ + +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; i < totalNumAabbs; i++) + { + m_sortedAxisCPU[axis][buf][i * 2].m_key = FloatFlip(m_allAabbsCPU[i].m_min[axis]) - 1; + m_sortedAxisCPU[axis][buf][i * 2].m_value = i * 2; + m_sortedAxisCPU[axis][buf][i * 2 + 1].m_key = FloatFlip(m_allAabbsCPU[i].m_max[axis]) + 1; + m_sortedAxisCPU[axis][buf][i * 2 + 1].m_value = i * 2 + 1; + } + } + } + } + + for (int axis = 0; axis < 3; axis++) + { + m_sorter->executeHost(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 < numEndPoints; i++) + { + int destIndex = m_sortedAxisCPU[axis][m_currentBuffer][i].m_value; + int newDest = destIndex / 2; + if (destIndex & 1) + { + m_objectMinMaxIndexCPU[axis][m_currentBuffer][newDest].y = i; + } + else + { + m_objectMinMaxIndexCPU[axis][m_currentBuffer][newDest].x = i; + } + } + } + } +} + +static bool b3PairCmp(const b3Int4& p, const b3Int4& q) +{ + return ((p.x < q.x) || ((p.x == q.x) && (p.y < q.y))); +} + +static bool operator==(const b3Int4& a, const b3Int4& b) +{ + return a.x == b.x && a.y == b.y; +}; + +static bool operator<(const b3Int4& a, const b3Int4& b) +{ + return a.x < b.x || (a.x == b.x && a.y < b.y); +}; + +static bool operator>(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 < preAabbs.size(); i++) + { + preAabbs[i] = m_allAabbsCPU[i]; + } + } + + if (m_currentBuffer < 0) + return; + { + B3_PROFILE("m_allAabbsGPU.copyToHost"); + m_allAabbsGPU.copyToHost(m_allAabbsCPU); + } + + b3AlignedObjectArray 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; i < m_allAabbsCPU.size(); i++) + { + //printf("aabb[%d] min=%f,%f,%f max=%f,%f,%f\n",i,m_allAabbsCPU[i].m_min[0],m_allAabbsCPU[i].m_min[1],m_allAabbsCPU[i].m_min[2], m_allAabbsCPU[i].m_max[0],m_allAabbsCPU[i].m_max[1],m_allAabbsCPU[i].m_max[2]); + } + + for (int axis = 0; axis < 3; axis++) + { + for (int buf = 0; buf < 2; buf++) + { + b3Assert(m_sortedAxisCPU[axis][buf].size() == m_allAabbsCPU.size() * 2); + } + } + } + + m_currentBuffer = 1 - m_currentBuffer; + + int totalNumAabbs = m_allAabbsCPU.size(); + + { + B3_PROFILE("assign m_sortedAxisCPU(FloatFlip)"); + for (int i = 0; i < totalNumAabbs; i++) + { + unsigned int keyMin[3]; + unsigned int keyMax[3]; + for (int axis = 0; axis < 3; axis++) + { + float vmin = m_allAabbsCPU[i].m_min[axis]; + float vmax = m_allAabbsCPU[i].m_max[axis]; + keyMin[axis] = FloatFlip(vmin); + keyMax[axis] = FloatFlip(vmax); + + m_sortedAxisCPU[axis][m_currentBuffer][i * 2].m_key = keyMin[axis] - 1; + m_sortedAxisCPU[axis][m_currentBuffer][i * 2].m_value = i * 2; + m_sortedAxisCPU[axis][m_currentBuffer][i * 2 + 1].m_key = keyMax[axis] + 1; + m_sortedAxisCPU[axis][m_currentBuffer][i * 2 + 1].m_value = i * 2 + 1; + } + //printf("aabb[%d] min=%u,%u,%u max %u,%u,%u\n", i,keyMin[0],keyMin[1],keyMin[2],keyMax[0],keyMax[1],keyMax[2]); + } + } + + { + B3_PROFILE("sort m_sortedAxisCPU"); + for (int axis = 0; axis < 3; axis++) + m_sorter->executeHost(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 < removedHostPairs.size(); i++) + { + b3Int4 removedPair = removedHostPairs[i]; + if ((removedPair.x != prevPair.x) || (removedPair.y != prevPair.y)) + { + int index1 = allPairs.findBinarySearch(removedPair); + + //#ifdef _DEBUG + + int index2 = allPairs.findLinearSearch(removedPair); + b3Assert(index1 == index2); + + //b3Assert(index1!=allPairs.size()); + if (index1 < allPairs.size()) + //#endif//_DEBUG + { + uniqueRemovedPairs++; + removedPositions.push_back(index1); + { + //printf("framepje(%d) remove pair(%d):%d,%d\n",framepje,i,removedPair.x,removedPair.y); + } + } + } + prevPair = removedPair; + } + + if (uniqueRemovedPairs) + { + for (int i = 0; i < removedPositions.size(); i++) + { + allPairs[removedPositions[i]].x = INT_MAX; + allPairs[removedPositions[i]].y = INT_MAX; + } + allPairs.quickSort(b3PairCmp); + allPairs.resize(allPairs.size() - uniqueRemovedPairs); + } + } + //if (uniqueRemovedPairs) + // printf("uniqueRemovedPairs=%d\n",uniqueRemovedPairs); + //printf("removedHostPairs.size = %d\n",removedHostPairs.size()); + + prevPair.x = -1; + prevPair.y = -1; + + int uniqueAddedPairs = 0; + b3AlignedObjectArray actualAddedPairs; + + { + B3_PROFILE("actual adding"); + for (int i = 0; i < addedHostPairs.size(); i++) + { + b3Int4 newPair = addedHostPairs[i]; + if ((newPair.x != prevPair.x) || (newPair.y != prevPair.y)) + { + //#ifdef _DEBUG + int index1 = allPairs.findBinarySearch(newPair); + + int index2 = allPairs.findLinearSearch(newPair); + b3Assert(index1 == index2); + + b3Assert(index1 == allPairs.size()); + if (index1 != allPairs.size()) + { + printf("??\n"); + } + + if (index1 == allPairs.size()) + //#endif //_DEBUG + { + uniqueAddedPairs++; + actualAddedPairs.push_back(newPair); + } + } + prevPair = newPair; + } + for (int i = 0; i < actualAddedPairs.size(); i++) + { + //printf("framepje (%d), new pair(%d):%d,%d\n",framepje,i,actualAddedPairs[i].x,actualAddedPairs[i].y); + allPairs.push_back(actualAddedPairs[i]); + } + } + + //if (uniqueAddedPairs) + // printf("uniqueAddedPairs=%d\n", uniqueAddedPairs); + + { + B3_PROFILE("m_overlappingPairs.copyFromHost"); + m_overlappingPairs.copyFromHost(allPairs); + } +} + +void b3GpuSapBroadphase::calculateOverlappingPairsHost(int maxPairs) +{ + //test + // if (m_currentBuffer>=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; i < numRigidBodies; i++) + { + b3SapAabb aabb = this->m_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 < numSmallAabbs; i++) + { + b3SapAabb smallAabbi = m_allAabbsCPU[m_smallAabbsMappingCPU[i]]; + //float reference = smallAabbi.m_max[axis]; + + for (int j = i + 1; j < numSmallAabbs; j++) + { + b3SapAabb smallAabbj = m_allAabbsCPU[m_smallAabbsMappingCPU[j]]; + + if (TestAabbAgainstAabb2((b3Vector3&)smallAabbi.m_min, (b3Vector3&)smallAabbi.m_max, + (b3Vector3&)smallAabbj.m_min, (b3Vector3&)smallAabbj.m_max)) + { + b3Int4 pair; + int a = smallAabbi.m_minIndices[3]; + int b = smallAabbj.m_minIndices[3]; + if (a <= b) + { + pair.x = a; //store the original index in the unsorted aabb array + pair.y = b; + } + else + { + pair.x = b; //store the original index in the unsorted aabb array + pair.y = a; + } + hostPairs.push_back(pair); + } + } + } + } + + { + int numSmallAabbs = m_smallAabbsMappingCPU.size(); + for (int i = 0; i < numSmallAabbs; i++) + { + b3SapAabb smallAabbi = m_allAabbsCPU[m_smallAabbsMappingCPU[i]]; + + //float reference = smallAabbi.m_max[axis]; + int numLargeAabbs = m_largeAabbsMappingCPU.size(); + + for (int j = 0; j < numLargeAabbs; j++) + { + b3SapAabb largeAabbj = m_allAabbsCPU[m_largeAabbsMappingCPU[j]]; + if (TestAabbAgainstAabb2((b3Vector3&)smallAabbi.m_min, (b3Vector3&)smallAabbi.m_max, + (b3Vector3&)largeAabbj.m_min, (b3Vector3&)largeAabbj.m_max)) + { + b3Int4 pair; + int a = largeAabbj.m_minIndices[3]; + int b = smallAabbi.m_minIndices[3]; + if (a <= b) + { + pair.x = a; + pair.y = b; //store the original index in the unsorted aabb array + } + else + { + pair.x = b; + pair.y = a; //store the original index in the unsorted aabb array + } + + hostPairs.push_back(pair); + } + } + } + } + + if (hostPairs.size() > 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;i maxPairs) + { + 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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h new file mode 100644 index 000000000..d17590b14 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h @@ -0,0 +1,143 @@ +#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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h new file mode 100644 index 000000000..60570f260 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h @@ -0,0 +1,13 @@ +#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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl new file mode 100644 index 000000000..ded4796d3 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h new file mode 100644 index 000000000..018541778 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h @@ -0,0 +1,198 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvh.cl b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvh.cl new file mode 100644 index 000000000..c375b9bf3 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h new file mode 100644 index 000000000..c02877dde --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h @@ -0,0 +1,728 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl new file mode 100644 index 000000000..93f77a643 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h new file mode 100644 index 000000000..d6999b94c --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h @@ -0,0 +1,341 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/CMakeLists.txt b/Engine/lib/bullet/src/Bullet3OpenCL/CMakeLists.txt new file mode 100644 index 000000000..1da58d4a9 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLInclude.h b/Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLInclude.h new file mode 100644 index 000000000..614653826 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLInclude.h @@ -0,0 +1,51 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp new file mode 100644 index 000000000..fe54ea5ec --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp @@ -0,0 +1,963 @@ +/* +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 (platformIndex < numPlatforms) + { + 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; + return platform; + } + + platform = platforms[platformIndex]; + + free(platforms); + } + + return platform; +} + +void b3OpenCLUtils::getPlatformInfo(cl_platform_id platform, b3OpenCLPlatformInfo* platformInfo) +{ + b3Assert(platform); + cl_int ciErrNum; + ciErrNum = clGetPlatformInfo(platform, CL_PLATFORM_VENDOR, B3_MAX_STRING_LENGTH, platformInfo->m_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 < num_devices; i++) + { + retContext = clCreateContext(cprops, 1, &devices[i], NULL, NULL, &ciErrNum); + if (ciErrNum == CL_SUCCESS) + break; + } + } + else + { + if (preferredDeviceIndex >= 0 && (unsigned int)preferredDeviceIndex < num_devices) + { + //create a context of the preferred device index + retContext = clCreateContext(cprops, 1, &devices[preferredDeviceIndex], NULL, NULL, &ciErrNum); + } + else + { + //create a context of all devices +#if defined(__APPLE__) + retContext = clCreateContext(cprops, num_devices, devices, MyFatalBreakAPPLE, NULL, &ciErrNum); +#else + b3Printf("numDevices=%d\n", num_devices); + + retContext = clCreateContext(cprops, num_devices, devices, NULL, NULL, &ciErrNum); +#endif + } + } + if (pErrNum != NULL) + { + *pErrNum = ciErrNum; + }; + + return retContext; +} + +cl_context b3OpenCLUtils_createContextFromType(cl_device_type deviceType, cl_int* pErrNum, void* pGLContext, void* pGLDC, int preferredDeviceIndex, int preferredPlatformIndex, cl_platform_id* retPlatformId) +{ +#ifdef B3_USE_CLEW + b3OpenCLUtils_clewInit(); +#endif + + cl_uint numPlatforms; + cl_context retContext = 0; + unsigned int i; + + cl_int ciErrNum = clGetPlatformIDs(0, NULL, &numPlatforms); + if (ciErrNum != CL_SUCCESS) + { + if (pErrNum != NULL) *pErrNum = ciErrNum; + return NULL; + } + if (numPlatforms > 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/Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.h b/Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.h new file mode 100644 index 000000000..6c82eed2a --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.h @@ -0,0 +1,190 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h new file mode 100644 index 000000000..27835bb74 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h @@ -0,0 +1,17 @@ +#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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.cpp new file mode 100644 index 000000000..4db717f8c --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.cpp @@ -0,0 +1,253 @@ + +#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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h new file mode 100644 index 000000000..a15fd0b2a --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h @@ -0,0 +1,62 @@ + +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp new file mode 100644 index 000000000..54a104c5c --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp @@ -0,0 +1,4408 @@ +/* +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 < nPoints; i++) + center += p[i]; + center /= (float)nPoints; + } + + // sample 4 directions + + float4 aVector = p[0] - center; + float4 u = cross3(nearNormal, aVector); + float4 v = cross3(nearNormal, u); + u = normalize3(u); + v = normalize3(v); + + //keep point with deepest penetration + float minW = FLT_MAX; + + int minIndex = -1; + + float4 maxDots; + maxDots.x = FLT_MIN; + maxDots.y = FLT_MIN; + maxDots.z = FLT_MIN; + maxDots.w = FLT_MIN; + + // idx, distance + for (int ie = 0; ie < nPoints; ie++) + { + if (p[ie].w < minW) + { + minW = p[ie].w; + minIndex = ie; + } + float f; + float4 r = p[ie] - center; + f = dot3F4(u, r); + if (f < maxDots.x) + { + maxDots.x = f; + contactIdx[0].x = ie; + } + + f = dot3F4(-u, r); + if (f < maxDots.y) + { + maxDots.y = f; + contactIdx[0].y = ie; + } + + f = dot3F4(v, r); + if (f < maxDots.z) + { + maxDots.z = f; + contactIdx[0].z = ie; + } + + f = dot3F4(-v, r); + if (f < maxDots.w) + { + maxDots.w = f; + contactIdx[0].w = ie; + } + } + + if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex) + { + //replace the first contact with minimum (todo: replace contact with least penetration) + contactIdx[0].x = minIndex; + } + + return 4; +} + +#define MAX_VERTS 1024 + +inline void project(const b3ConvexPolyhedronData& hull, const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray& 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 < numVerts; i++) + { + //b3Vector3 pt = trans * vertices[m_vertexOffset+i]; + //b3Scalar dp = pt.dot(dir); + //b3Vector3 vertex = vertices[hull.m_vertexOffset+i]; + b3Scalar dp = dot3F4((float4&)vertices[hull.m_vertexOffset + i], localDir); + //b3Assert(dp==dpL); + if (dp < min) min = dp; + if (dp > 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 < Min1 || Max1 < Min0) + return false; + + b3Scalar d0 = Max0 - Min1; + assert(d0 >= 0.0f); + b3Scalar d1 = Max1 - Min0; + assert(d1 >= 0.0f); + depth = d0 < d1 ? d0 : d1; + return true; +} + +inline bool IsAlmostZero(const b3Vector3& v) +{ + if (fabsf(v.x) > 1e-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; i < numFacesA; i++) + { + const float4& normal = (float4&)facesA[hullA.m_faceOffset + i].m_plane; + float4 faceANormalWS = b3QuatRotate(ornA, normal); + + if (dot3F4(deltaC2, faceANormalWS) < 0) + faceANormalWS *= -1.f; + + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar d; + if (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d)) + return false; + + if (d < dmin) + { + dmin = d; + sep = (b3Vector3&)faceANormalWS; + } + } + + int numFacesB = hullB.m_numFaces; + // Test normals from hullB + for (int i = 0; i < numFacesB; i++) + { + float4 normal = (float4&)facesB[hullB.m_faceOffset + i].m_plane; + float4 WorldNormal = b3QuatRotate(ornB, normal); + + if (dot3F4(deltaC2, WorldNormal) < 0) + { + WorldNormal *= -1.f; + } + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar d; + if (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d)) + return false; + + if (d < dmin) + { + dmin = d; + sep = (b3Vector3&)WorldNormal; + } + } + + int curEdgeEdge = 0; + // Test edges + for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++) + { + const float4& edge0 = (float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0]; + float4 edge0World = b3QuatRotate(ornA, (float4&)edge0); + + for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++) + { + const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1]; + float4 edge1World = b3QuatRotate(ornB, (float4&)edge1); + + float4 crossje = cross3(edge0World, edge1World); + + curEdgeEdge++; + if (!IsAlmostZero((b3Vector3&)crossje)) + { + crossje = normalize3(crossje); + if (dot3F4(deltaC2, crossje) < 0) + crossje *= -1.f; + +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar dist; + if (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist)) + return false; + + if (dist < dmin) + { + dmin = dist; + sep = (b3Vector3&)crossje; + } + } + } + } + + if ((dot3F4(-deltaC2, (float4&)sep)) > 0.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; e0 < hullA->m_numUniqueEdges; e0++) + { + const float4 edge0 = uniqueEdges[hullA->m_uniqueEdgesOffset + e0]; + float4 edge0World = b3QuatRotate(ornA, edge0); + + for (int e1 = 0; e1 < hullB->m_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 (Max0 < Min1 || Max1 < Min0) + result = false; + + float d0 = Max0 - Min1; + float d1 = Max1 - Min0; + dist = d0 < d1 ? d0 : d1; + result = true; + } + + if (dist < *dmin) + { + *dmin = dist; + *sep = crossje; + } + } + } + } + + if ((dot3F4(-DeltaC2, *sep)) > 0.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; face < hullA->m_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; e0 < numVerticesA; e0++) + { + const float4 a = verticesA[hullA->m_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 < numVertsIn; i++) + { + float depth = dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS; + if (depth <= minDist) + { + depth = minDist; + } + if (numContactsOut < contactCapacity) + { + if (depth <= maxDist) + { + float4 pointInWorld = pVtxIn[i]; + //resultOut.addContactPoint(separatingNormal,point,depth); + contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth); + //printf("depth=%f\n",depth); + } + } + else + { + b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity); + } + } + } + + return numContactsOut; +} + +static int clipHullAgainstHull(const float4& separatingNormal, + const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const float4& posA, const b3Quaternion& ornA, const float4& posB, const b3Quaternion& ornB, + float4* worldVertsB1, float4* worldVertsB2, int capacityWorldVerts, + const float minDist, float maxDist, + const b3AlignedObjectArray& 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; face < hullB.m_numFaces; face++) + { +#ifdef BT_DEBUG_SAT_FACE + if (once) + printf("face %d\n", face); + const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face]; + if (once) + { + for (int i = 0; i < faceB->m_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 < numVertices; e0++) + { + const float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]]; + worldVertsB1[numWorldVertsB1++] = transform(&b, &posB, &ornB); + } + } + + if (closestFaceB >= 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 < n; j++) v[0] += v[j]; +#define PARALLEL_DO(execution, n) \ + for (int ie = 0; ie < n; ie++) \ + { \ + execution; \ + } +#define REDUCE_MAX(v, n) \ + { \ + int i = 0; \ + for (int offset = 0; offset < n; offset++) v[i] = (v[i].y > v[i + offset].y) ? v[i] : v[i + offset]; \ + } +#define REDUCE_MIN(v, n) \ + { \ + int i = 0; \ + for (int offset = 0; offset < n; offset++) v[i] = (v[i].y < v[i + offset].y) ? v[i] : v[i + offset]; \ + } + +int extractManifold(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 = make_float4(0, 0, 0, 0); + { + for (int i = 0; i < nPoints; i++) + center += p[i]; + center /= (float)nPoints; + } + + // sample 4 directions + + float4 aVector = p[0] - center; + float4 u = cross3(nearNormal, aVector); + float4 v = cross3(nearNormal, u); + u = normalize3(u); + v = normalize3(v); + + //keep point with deepest penetration + float minW = FLT_MAX; + + int minIndex = -1; + + float4 maxDots; + maxDots.x = FLT_MIN; + maxDots.y = FLT_MIN; + maxDots.z = FLT_MIN; + maxDots.w = FLT_MIN; + + // idx, distance + for (int ie = 0; ie < nPoints; ie++) + { + if (p[ie].w < minW) + { + minW = p[ie].w; + minIndex = ie; + } + float f; + float4 r = p[ie] - center; + f = dot3F4(u, r); + if (f < maxDots.x) + { + maxDots.x = f; + contactIdx[0].x = ie; + } + + f = dot3F4(-u, r); + if (f < maxDots.y) + { + maxDots.y = f; + contactIdx[0].y = ie; + } + + f = dot3F4(v, r); + if (f < maxDots.z) + { + maxDots.z = f; + contactIdx[0].z = ie; + } + + f = dot3F4(-v, r); + if (f < maxDots.w) + { + maxDots.w = f; + contactIdx[0].w = ie; + } + } + + if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex) + { + //replace the first contact with minimum (todo: replace contact with least penetration) + contactIdx[0].x = minIndex; + } + + return 4; +} + +int clipHullHullSingle( + int bodyIndexA, int bodyIndexB, + const float4& posA, + const b3Quaternion& ornA, + const float4& 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); + + 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 (nContacts < maxContactCapacity) + { + contactIndex = nContacts; + globalContactOut->expand(); + 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; p < numPoints; p++) + { + contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]]; //check if it is actually on B + contact.m_worldNormalOnB = normalOnSurfaceB; + } + //printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints); + contact.m_worldNormalOnB.w = (b3Scalar)numPoints; + nContacts++; + } + else + { + b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity); + } + } + } + return contactIndex; +} + +void computeContactPlaneConvex(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) +{ + int shapeIndex = collidables[collidableIndexB].m_shapeIndex; + const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndex]; + + b3Vector3 posB = rigidBodies[bodyIndexB].m_pos; + b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; + b3Vector3 posA = rigidBodies[bodyIndexA].m_pos; + b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; + + // int numContactsOut = 0; + // int numWorldVertsB1= 0; + + b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane; + b3Vector3 planeNormal = b3MakeVector3(planeEq.x, planeEq.y, planeEq.z); + b3Vector3 planeNormalWorld = b3QuatRotate(ornA, planeNormal); + float planeConstant = planeEq.w; + b3Transform convexWorldTransform; + convexWorldTransform.setIdentity(); + convexWorldTransform.setOrigin(posB); + convexWorldTransform.setRotation(ornB); + b3Transform planeTransform; + planeTransform.setIdentity(); + planeTransform.setOrigin(posA); + planeTransform.setRotation(ornA); + + b3Transform planeInConvex; + planeInConvex = convexWorldTransform.inverse() * planeTransform; + b3Transform convexInPlane; + convexInPlane = planeTransform.inverse() * convexWorldTransform; + + b3Vector3 planeNormalInConvex = planeInConvex.getBasis() * -planeNormal; + float maxDot = -1e30; + int hitVertex = -1; + b3Vector3 hitVtx; + +#define MAX_PLANE_CONVEX_POINTS 64 + + b3Vector3 contactPoints[MAX_PLANE_CONVEX_POINTS]; + int numPoints = 0; + + b3Int4 contactIdx; + contactIdx.s[0] = 0; + contactIdx.s[1] = 1; + contactIdx.s[2] = 2; + contactIdx.s[3] = 3; + + for (int i = 0; i < hullB->m_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 (numPoints < MAX_PLANE_CONVEX_POINTS) + { + b3Vector3 vtxWorld = convexWorldTransform * vtx; + b3Vector3 vtxInPlane = planeTransform.inverse() * vtxWorld; + float dist = planeNormal.dot(vtxInPlane) - planeConstant; + if (dist < 0.f) + { + vtxWorld.w = dist; + contactPoints[numPoints] = vtxWorld; + numPoints++; + } + } + } + + int numReducedPoints = 0; + + numReducedPoints = numPoints; + + if (numPoints > 4) + { + 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; i < numReducedPoints; i++) + { + b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]]; + c->m_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 < numSubTreesA; p++) + { + b3BvhSubtreeInfo subtreeA = subTreesCPU[subTreesOffsetA + p]; + //bvhInfoCPU[bvhA].m_quantization + b3Vector3 treeAminLocal = MyUnQuantize(subtreeA.m_quantizedAabbMin, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin); + b3Vector3 treeAmaxLocal = MyUnQuantize(subtreeA.m_quantizedAabbMax, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin); + + b3Vector3 aabbAMinOut, aabbAMaxOut; + float margin = 0.f; + b3TransformAabb2(treeAminLocal, treeAmaxLocal, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut); + + for (int q = 0; q < numSubTreesB; q++) + { + b3BvhSubtreeInfo subtreeB = subTreesCPU[subTreesOffsetB + q]; + + b3Vector3 treeBminLocal = MyUnQuantize(subtreeB.m_quantizedAabbMin, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin); + b3Vector3 treeBmaxLocal = MyUnQuantize(subtreeB.m_quantizedAabbMax, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin); + + b3Vector3 aabbBMinOut, aabbBMaxOut; + float margin = 0.f; + b3TransformAabb2(treeBminLocal, treeBmaxLocal, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut); + + numAabbChecks = 0; + bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut); + if (aabbOverlap) + { + int startNodeIndexA = subtreeA.m_rootNodeIndex + bvhInfoCPU[bvhA].m_nodeOffset; + // int endNodeIndexA = startNodeIndexA+subtreeA.m_subtreeSize; + + int startNodeIndexB = subtreeB.m_rootNodeIndex + bvhInfoCPU[bvhB].m_nodeOffset; + // int endNodeIndexB = startNodeIndexB+subtreeB.m_subtreeSize; + + b3AlignedObjectArray 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 < maxNumCompoundPairsCapacity) + { + int childShapeIndexA = treeNodesCPU[node.x].getTriangleIndex(); + int childShapeIndexB = treeNodesCPU[node.y].getTriangleIndex(); + gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, childShapeIndexB); + } + } + } + } + } while (depth); + maxNumAabbChecks = b3Max(numAabbChecks, maxNumAabbChecks); + } + } + } + + return; + } + + if ((collidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) || (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)) + { + if (collidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + int numChildrenA = collidables[collidableIndexA].m_numChildShapes; + for (int c = 0; c < numChildrenA; c++) + { + int childShapeIndexA = collidables[collidableIndexA].m_shapeIndex + c; + int childColIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex; + + float4 posA = rigidBodies[bodyIndexA].m_pos; + b3Quat ornA = rigidBodies[bodyIndexA].m_quat; + float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition; + b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation; + float4 newPosA = b3QuatRotate(ornA, childPosA) + posA; + b3Quat newOrnA = b3QuatMul(ornA, childOrnA); + + b3Aabb aabbA = aabbsLocalSpace[childColIndexA]; + + b3Transform transA; + transA.setIdentity(); + transA.setOrigin(newPosA); + transA.setRotation(newOrnA); + b3Scalar margin = 0.0f; + + b3Vector3 aabbAMinOut, aabbAMaxOut; + + b3TransformAabb2((const b3Float4&)aabbA.m_min, (const b3Float4&)aabbA.m_max, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut); + + if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + int numChildrenB = collidables[collidableIndexB].m_numChildShapes; + for (int b = 0; b < numChildrenB; b++) + { + int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b; + int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; + b3Quat ornB = rigidBodies[bodyIndexB].m_quat; + float4 posB = rigidBodies[bodyIndexB].m_pos; + float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; + b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; + float4 newPosB = transform(&childPosB, &posB, &ornB); + b3Quat newOrnB = b3QuatMul(ornB, childOrnB); + + b3Aabb aabbB = aabbsLocalSpace[childColIndexB]; + + b3Transform transB; + transB.setIdentity(); + transB.setOrigin(newPosB); + transB.setRotation(newOrnB); + + b3Vector3 aabbBMinOut, aabbBMaxOut; + b3TransformAabb2((const b3Float4&)aabbB.m_min, (const b3Float4&)aabbB.m_max, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut); + + numAabbChecks++; + bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut); + if (aabbOverlap) + { + /* + int numFacesA = convexShapes[shapeIndexA].m_numFaces; + float dmin = FLT_MAX; + float4 posA = newPosA; + posA.w = 0.f; + float4 posB = newPosB; + posB.w = 0.f; + float4 c0local = convexShapes[shapeIndexA].m_localCenter; + b3Quat ornA = newOrnA; + float4 c0 = transform(&c0local, &posA, &ornA); + float4 c1local = convexShapes[shapeIndexB].m_localCenter; + b3Quat ornB =newOrnB; + float4 c1 = transform(&c1local,&posB,&ornB); + const float4 DeltaC2 = c0 - c1; + */ + { // + int compoundPairIdx = b3AtomicInc(numCompoundPairsOut); + if (compoundPairIdx < maxNumCompoundPairsCapacity) + { + gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, childShapeIndexB); + } + } // + } //fi(1) + } //for (int b=0 + } //if (collidables[collidableIndexB]. + else //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + if (1) + { + // int numFacesA = convexShapes[shapeIndexA].m_numFaces; + // float dmin = FLT_MAX; + float4 posA = newPosA; + posA.w = 0.f; + float4 posB = rigidBodies[bodyIndexB].m_pos; + posB.w = 0.f; + float4 c0local = convexShapes[shapeIndexA].m_localCenter; + b3Quat ornA = newOrnA; + float4 c0; + c0 = transform(&c0local, &posA, &ornA); + float4 c1local = convexShapes[shapeIndexB].m_localCenter; + b3Quat ornB = rigidBodies[bodyIndexB].m_quat; + float4 c1; + c1 = transform(&c1local, &posB, &ornB); + // const float4 DeltaC2 = c0 - c1; + + { + int compoundPairIdx = b3AtomicInc(numCompoundPairsOut); + if (compoundPairIdx < maxNumCompoundPairsCapacity) + { + gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, -1); + } //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 < numCompoundPairs) + { + int bodyIndexA = gpuCompoundPairs[i].x; + int bodyIndexB = gpuCompoundPairs[i].y; + + int childShapeIndexA = gpuCompoundPairs[i].z; + int childShapeIndexB = gpuCompoundPairs[i].w; + + int collidableIndexA = -1; + int collidableIndexB = -1; + + b3Quat ornA = rigidBodies[bodyIndexA].m_quat; + float4 posA = rigidBodies[bodyIndexA].m_pos; + + b3Quat ornB = rigidBodies[bodyIndexB].m_quat; + float4 posB = rigidBodies[bodyIndexB].m_pos; + + if (childShapeIndexA >= 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 < numCompoundPairs) + { + if (gpuHasCompoundSepNormalsOut[i]) + { + int bodyIndexA = gpuCompoundPairs[i].x; + int bodyIndexB = gpuCompoundPairs[i].y; + + int childShapeIndexA = gpuCompoundPairs[i].z; + int childShapeIndexB = gpuCompoundPairs[i].w; + + int collidableIndexA = -1; + int collidableIndexB = -1; + + b3Quat ornA = rigidBodies[bodyIndexA].m_quat; + float4 posA = rigidBodies[bodyIndexA].m_pos; + + b3Quat ornB = rigidBodies[bodyIndexB].m_quat; + float4 posB = rigidBodies[bodyIndexB].m_pos; + + if (childShapeIndexA >= 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; i < nReducedContacts; i++) + { + c->m_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; i < numCompoundPairsOut; i++) + { + processCompoundPairsKernel(&cpuCompoundPairsOut[0], rigidBodies, collidables, convexShapes, convexVertices, hostUniqueEdges, faces, convexIndices, 0, cpuChildShapes, + cpuCompoundSepNormalsOut, cpuHasCompoundSepNormalsOut, numCompoundPairsOut, i); + } + + for (int i = 0; i < numCompoundPairsOut; i++) + { + clipCompoundsHullHullKernel(&cpuCompoundPairsOut[0], rigidBodies, collidables, convexShapes, convexVertices, hostUniqueEdges, faces, convexIndices, cpuChildShapes, + cpuCompoundSepNormalsOut, cpuHasCompoundSepNormalsOut, globalContactsOut, &nGlobalContactsOut, numCompoundPairsOut, maxContactCapacity, i); + } + /* + int childColIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex; + + float4 posA = rigidBodies[bodyIndexA].m_pos; + b3Quat ornA = rigidBodies[bodyIndexA].m_quat; + float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition; + b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation; + float4 newPosA = b3QuatRotate(ornA,childPosA)+posA; + b3Quat newOrnA = b3QuatMul(ornA,childOrnA); + + int shapeIndexA = collidables[childColIndexA].m_shapeIndex; + + + 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; + + /* + + int numChildrenB = collidables[collidableIndexB].m_numChildShapes; + for (int c=0;cm_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 (numPoints < MAX_PLANE_CONVEX_POINTS) + { + b3Vector3 vtxWorld = convexWorldTransform * vtx; + b3Vector3 vtxInPlane = planeTransform.inverse() * vtxWorld; + float dist = planeNormal.dot(vtxInPlane) - planeConstant; + if (dist < 0.f) + { + vtxWorld.w = dist; + contactPoints[numPoints] = vtxWorld; + numPoints++; + } + } + } + + int numReducedPoints = 0; + + numReducedPoints = numPoints; + + if (numPoints > 4) + { + 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; i < numReducedPoints; i++) + { + b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]]; + c->m_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 (l2 < radius * radius) + { + dist = b3Sqrt(l2); + if (dist > minDist) + { + 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 < nPairs; i++) + { + int bodyIndexA = hostPairs[i].x; + int bodyIndexB = hostPairs[i].y; + int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx; + int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx; + + if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_SPHERE && + hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { + computeContactSphereConvex(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0], + &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity); + } + + if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + hostCollidables[collidableIndexB].m_shapeType == SHAPE_SPHERE) + { + computeContactSphereConvex(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0], + &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity); + //printf("convex-sphere\n"); + } + + if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + hostCollidables[collidableIndexB].m_shapeType == SHAPE_PLANE) + { + computeContactPlaneConvex(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0], + &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity); + // printf("convex-plane\n"); + } + + if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_PLANE && + hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { + computeContactPlaneConvex(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0], + &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity); + // printf("plane-convex\n"); + } + + if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS && + hostCollidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + computeContactCompoundCompound(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0], + &hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], hostAabbsWorldSpace, hostAabbsLocalSpace, hostVertices, hostUniqueEdges, hostIndices, hostFaces, &hostContacts[0], + nContacts, maxContactCapacity, treeNodesCPU, subTreesCPU, bvhInfoCPU); + // printf("convex-plane\n"); + } + + if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS && + hostCollidables[collidableIndexB].m_shapeType == SHAPE_PLANE) + { + computeContactPlaneCompound(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0], + &hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity); + // printf("convex-plane\n"); + } + + if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_PLANE && + hostCollidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + computeContactPlaneCompound(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0], + &hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity); + // printf("plane-convex\n"); + } + + if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { + //printf("hostPairs[i].z=%d\n",hostPairs[i].z); + int contactIndex = computeContactConvexConvex2(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, hostBodyBuf, hostCollidables, hostConvexData, hostVertices, hostUniqueEdges, hostIndices, hostFaces, hostContacts, nContacts, maxContactCapacity, oldHostContacts); + //int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts); + + if (contactIndex >= 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 < nPairs; i++) + { + int bodyIndexA = hostPairs[i].x; + int bodyIndexB = hostPairs[i].y; + int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx; + int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx; + + int shapeIndexA = hostCollidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = hostCollidables[collidableIndexB].m_shapeIndex; + + hostHasSepAxis[i] = 0; + + //once the broadphase avoids static-static pairs, we can remove this test + if ((hostBodyBuf[bodyIndexA].m_invMass == 0) && (hostBodyBuf[bodyIndexB].m_invMass == 0)) + { + continue; + } + + if ((hostCollidables[collidableIndexA].m_shapeType != SHAPE_CONVEX_HULL) || (hostCollidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL)) + { + continue; + } + + float dmin = FLT_MAX; + + b3ConvexPolyhedronData* convexShapeA = &hostConvexShapeData[shapeIndexA]; + b3ConvexPolyhedronData* convexShapeB = &hostConvexShapeData[shapeIndexB]; + b3Vector3 posA = hostBodyBuf[bodyIndexA].m_pos; + b3Vector3 posB = hostBodyBuf[bodyIndexB].m_pos; + b3Quaternion ornA = hostBodyBuf[bodyIndexA].m_quat; + b3Quaternion ornB = hostBodyBuf[bodyIndexB].m_quat; + + if (useGjk) + { + //first approximate the separating axis, to 'fail-proof' GJK+EPA or MPR + { + 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, false); + + if (hasEdgeEdge) + { + hostHasSepAxis[i] = 1; + hostSepAxis[i] = sepAxis; + hostSepAxis[i].w = dmin; + } + } + } + } + + if (hostHasSepAxis[i]) + { + int pairIndex = i; + + bool useMpr = true; + if (useMpr) + { + int res = 0; + float depth = 0.f; + b3Vector3 sepAxis2 = b3MakeVector3(1, 0, 0); + b3Vector3 resultPointOnBWorld = b3MakeVector3(0, 0, 0); + + float depthOut; + b3Vector3 dirOut; + b3Vector3 posOut; + + //res = b3MprPenetration(bodyIndexA,bodyIndexB,hostBodyBuf,hostConvexShapeData,hostCollidables,hostVertices,&mprConfig,&depthOut,&dirOut,&posOut); + res = b3MprPenetration(pairIndex, bodyIndexA, bodyIndexB, &hostBodyBuf[0], &hostConvexShapeData[0], &hostCollidables[0], &hostVertices[0], &hostSepAxis[0], &hostHasSepAxis[0], &depthOut, &dirOut, &posOut); + depth = depthOut; + sepAxis2 = b3MakeVector3(-dirOut.x, -dirOut.y, -dirOut.z); + resultPointOnBWorld = posOut; + //hostHasSepAxis[i] = 0; + + if (res == 0) + { + //add point? + //printf("depth = %f\n",depth); + //printf("normal = %f,%f,%f\n",dir.v[0],dir.v[1],dir.v[2]); + //qprintf("pos = %f,%f,%f\n",pos.v[0],pos.v[1],pos.v[2]); + + float dist = 0.f; + + const b3ConvexPolyhedronData& hullA = hostConvexShapeData[hostCollidables[hostBodyBuf[bodyIndexA].m_collidableIdx].m_shapeIndex]; + const b3ConvexPolyhedronData& hullB = hostConvexShapeData[hostCollidables[hostBodyBuf[bodyIndexB].m_collidableIdx].m_shapeIndex]; + + if (b3TestSepAxis(&hullA, &hullB, posA, ornA, posB, ornB, &sepAxis2, &hostVertices[0], &hostVertices[0], &dist)) + { + if (depth > 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 < nPairs; pairIndex++) + { + int bodyIndexA = hostPairs[pairIndex].x; + int bodyIndexB = hostPairs[pairIndex].y; + int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx; + int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx; + if (cpuChildShapes.size()) + { + findCompoundPairsKernel( + pairIndex, + bodyIndexA, + bodyIndexB, + collidableIndexA, + collidableIndexB, + &hostBodyBuf[0], + &hostCollidables[0], + &hostConvexData[0], + hostVertices, + hostAabbsWorldSpace, + hostAabbsLocalSpace, + &cpuChildShapes[0], + &cpuCompoundPairsOut[0], + &numCompoundPairs, + compoundPairCapacity, + treeNodesCPU, + subTreesCPU, + bvhInfoCPU); + } + } + + m_numCompoundPairsOut.copyFromHostPointer(&numCompoundPairs, 1, 0, true); + if (numCompoundPairs) + { + b3CompoundOverlappingPair* ptr = (b3CompoundOverlappingPair*)&cpuCompoundPairsOut[0]; + m_gpuCompoundPairs.copyFromHostPointer(ptr, numCompoundPairs, 0, true); + } + //cpuCompoundPairsOut + } + if (numCompoundPairs) + { + printf("numCompoundPairs=%d\n", numCompoundPairs); + } + + if (numCompoundPairs > 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 < nPairs; i++) + { + b3BvhTraversal(&hostPairs.at(0), + &hostBodyBuf.at(0), + &hostCollidables.at(0), + &hostAabbsWorldSpace.at(0), + &triangleConvexPairsOutHost.at(0), + &hostNumConcavePairsOut, + &subTreesCPU.at(0), + &treeNodesCPU.at(0), + &bvhInfoCPU.at(0), + nPairs, + maxTriConvexPairCapacity, + i); + } + numConcavePairs = hostNumConcavePairsOut; + + if (hostNumConcavePairsOut) + { + triangleConvexPairsOutHost.resize(hostNumConcavePairsOut); + triangleConvexPairsOut.copyFromHost(triangleConvexPairsOutHost); + } + // + + m_numConcavePairsOut.resize(0); + m_numConcavePairsOut.push_back(numConcavePairs); + } + + //printf("numConcavePairs=%d (max = %d\n",numConcavePairs,maxTriConvexPairCapacity); + + if (numConcavePairs > 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; + b3AlignedObjectArray concaveHasSeparatingNormalsCPU; + + 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 < numConcavePairs; i++) + { + b3FindConcaveSeparatingAxisKernel(&triangleConvexPairsOutHost.at(0), + &hostBodyBuf.at(0), + &hostCollidables.at(0), + &hostConvexData.at(0), &hostVertices.at(0), &hostUniqueEdges.at(0), + &hostFaces.at(0), &hostIndices.at(0), childShapePointerCPU, + &hostAabbsWorldSpace.at(0), + &concaveSepNormalsHost.at(0), + &clippingFacesOutCPU.at(0), + &worldVertsA1CPU.at(0), + &worldNormalsACPU.at(0), + &worldVertsB1CPU.at(0), + &concaveHasSeparatingNormalsCPU.at(0), + vertexFaceCapacity, + numConcavePairs, i); + }; + + m_concaveSepNormals.copyFromHost(concaveSepNormalsHost); + m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU); + clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU); + worldVertsA1GPU.copyFromHost(worldVertsA1CPU); + worldNormalsAGPU.copyFromHost(worldNormalsACPU); + worldVertsB1GPU.copyFromHost(worldVertsB1CPU); + } + // b3AlignedObjectArray 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); + + b3AlignedObjectArray concaveHasSeparatingNormalsCPU; + m_concaveHasSeparatingNormals.copyToHost(concaveHasSeparatingNormalsCPU); + + b3AlignedObjectArray concaveSepNormalsHost; + m_concaveSepNormals.copyToHost(concaveSepNormalsHost); + + b3AlignedObjectArray worldVertsB2CPU; + worldVertsB2CPU.resize(worldVertsB2GPU.size()); + + for (int i = 0; i < numConcavePairs; i++) + { + clipFacesAndFindContactsKernel(&concaveSepNormalsHost.at(0), + &concaveHasSeparatingNormalsCPU.at(0), + &clippingFacesOutCPU.at(0), + &worldVertsA1CPU.at(0), + &worldNormalsACPU.at(0), + &worldVertsB1CPU.at(0), + &worldVertsB2CPU.at(0), + vertexFaceCapacity, + i); + } + + clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU); + worldVertsB2GPU.copyFromHost(worldVertsB2CPU); + } + else + { + if (1) + { + B3_PROFILE("clipFacesAndFindContacts"); + //nContacts = m_totalContactsOut.at(0); + //int h = m_hasSeparatingNormals.at(0); + //int4 p = clippingFacesOutGPU.at(0); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL(m_concaveSepNormals.getBufferCL()), + b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()), + b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()), + b3BufferInfoCL(worldVertsA1GPU.getBufferCL()), + b3BufferInfoCL(worldNormalsAGPU.getBufferCL()), + b3BufferInfoCL(worldVertsB1GPU.getBufferCL()), + b3BufferInfoCL(worldVertsB2GPU.getBufferCL())}; + b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts, "m_clipFacesAndFindContacts"); + launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL)); + launcher.setConst(vertexFaceCapacity); + + launcher.setConst(numConcavePairs); + int debugMode = 0; + launcher.setConst(debugMode); + int num = numConcavePairs; + launcher.launch1D(num); + clFinish(m_queue); + //int bla = m_totalContactsOut.at(0); + } + } + //contactReduction + { + int newContactCapacity = nContacts + numConcavePairs; + contactOut->reserve(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); + + b3AlignedObjectArray concaveHasSeparatingNormalsCPU; + 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; i < numConcavePairs; i++) + { + b3NewContactReductionKernel(&triangleConvexPairsOutHost.at(0), + &hostBodyBuf.at(0), + &concaveSepNormalsHost.at(0), + &concaveHasSeparatingNormalsCPU.at(0), + &hostContacts.at(0), + &clippingFacesOutCPU.at(0), + &worldVertsB2CPU.at(0), + &nGlobalContactsOut, + vertexFaceCapacity, + newContactCapacity, + numConcavePairs, + i); + } + + nContacts = nGlobalContactsOut; + m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true); + // nContacts = m_totalContactsOut.at(0); + //contactOut->resize(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 < nPairs; i++) + { + int bodyIndexA = hostPairs[i].x; + int bodyIndexB = hostPairs[i].y; + + int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx; + int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx; + + int shapeIndexA = hostCollidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = hostCollidables[collidableIndexB].m_shapeIndex; + + if (hostHasSepNormals[i]) + { + b3FindClippingFaces(cpuSepNormals[i], + &hostConvexData[shapeIndexA], + &hostConvexData[shapeIndexB], + hostBodyBuf[bodyIndexA].m_pos, hostBodyBuf[bodyIndexA].m_quat, + hostBodyBuf[bodyIndexB].m_pos, hostBodyBuf[bodyIndexB].m_quat, + &worldVertsA1CPU.at(0), &worldNormalsACPU.at(0), + &worldVertsB1CPU.at(0), + vertexFaceCapacity, minDist, maxDist, + &hostVertices.at(0), &hostFaces.at(0), + &hostIndices.at(0), + &hostVertices.at(0), &hostFaces.at(0), + &hostIndices.at(0), &clippingFacesOutCPU.at(0), i); + } + } + + clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU); + worldVertsA1GPU.copyFromHost(worldVertsA1CPU); + worldNormalsAGPU.copyFromHost(worldNormalsACPU); + worldVertsB1GPU.copyFromHost(worldVertsB1CPU); + } + + ///clip face B against face A, reduce contacts and append them to a global contact array + if (1) + { + if (clipConvexFacesAndFindContactsCPU) + { + //b3AlignedObjectArray 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); + + b3AlignedObjectArray worldVertsA1CPU; + 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; i < nPairs; i++) + { + clipFacesAndFindContactsKernel( + &hostSepNormals.at(0), + &hostHasSepAxis.at(0), + &hostClippingFaces.at(0), + &worldVertsA1CPU.at(0), + &worldNormalsACPU.at(0), + &worldVertsB1CPU.at(0), + &worldVertsB2CPU.at(0), + + vertexFaceCapacity, + i); + } + + clippingFacesOutGPU.copyFromHost(hostClippingFaces); + worldVertsB2GPU.copyFromHost(worldVertsB2CPU); + } + else + { + B3_PROFILE("clipFacesAndFindContacts"); + //nContacts = m_totalContactsOut.at(0); + //int h = m_hasSeparatingNormals.at(0); + //int4 p = clippingFacesOutGPU.at(0); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL(m_sepNormals.getBufferCL()), + b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()), + b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()), + b3BufferInfoCL(worldVertsA1GPU.getBufferCL()), + b3BufferInfoCL(worldNormalsAGPU.getBufferCL()), + b3BufferInfoCL(worldVertsB1GPU.getBufferCL()), + b3BufferInfoCL(worldVertsB2GPU.getBufferCL())}; + + b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts, "m_clipFacesAndFindContacts"); + launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL)); + launcher.setConst(vertexFaceCapacity); + + launcher.setConst(nPairs); + int debugMode = 0; + launcher.setConst(debugMode); + int num = nPairs; + launcher.launch1D(num); + clFinish(m_queue); + } + + { + nContacts = m_totalContactsOut.at(0); + //printf("nContacts = %d\n",nContacts); + + int newContactCapacity = nContacts + nPairs; + contactOut->reserve(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; i < nPairs; i++) + { + b3NewContactReductionKernel(&hostPairs.at(0), + &hostBodyBuf.at(0), + &hostSepNormals.at(0), + &hostHasSepAxis.at(0), + &hostContactsOut.at(0), + &hostClippingFaces.at(0), + &worldVertsB2CPU.at(0), + &nGlobalContactsOut, + vertexFaceCapacity, + newContactCapacity, + nPairs, + i); + } + + nContacts = nGlobalContactsOut; + m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true); + hostContactsOut.resize(nContacts); + //printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts); + contactOut->copyFromHost(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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h new file mode 100644 index 000000000..53e8c4ed4 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h @@ -0,0 +1,106 @@ + +#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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h new file mode 100644 index 000000000..c4cf70007 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h @@ -0,0 +1,7 @@ +#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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp new file mode 100644 index 000000000..974b246f0 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp @@ -0,0 +1,1062 @@ +/* +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 (rl < GJK_MIN_DISTANCE) + { /* Touching or inside */ + m_status = eStatus::Inside; + break; + } + /* Append new vertice in -'v' direction */ + appendvertice(cs, -m_ray); + const b3Vector3& w = cs.c[cs.rank - 1]->w; + bool found = false; + for (unsigned int i = 0; i < 4; ++i) + { + if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS) + { + found = true; + break; + } + } + if (found) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + { /* Update lastw */ + lastw[clastw = (clastw + 1) & 3] = w; + } + /* Check for termination */ + const b3Scalar omega = b3Dot(m_ray, w) / rl; + alpha = b3Max(omega, alpha); + if (((rl - alpha) - (GJK_ACCURACY * rl)) <= 0) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + b3Scalar weights[4]; + unsigned int mask = 0; + switch (cs.rank) + { + case 2: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + weights, mask); + break; + case 3: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights, mask); + break; + case 4: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights, mask); + break; + } + if (sqdist >= 0) + { /* Valid */ + ns.rank = 0; + m_ray = b3MakeVector3(0, 0, 0); + m_current = next; + for (unsigned int i = 0, ni = cs.rank; i < ni; ++i) + { + if (mask & (1 << i)) + { + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w * weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if (mask == 15) m_status = eStatus::Inside; + } + else + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus::Failed; + } while (m_status == eStatus::Valid); + m_simplex = &m_simplices[m_current]; + switch (m_status) + { + case eStatus::Valid: + m_distance = m_ray.length(); + break; + case eStatus::Inside: + m_distance = 0; + break; + default: + { + } + } + return (m_status); + } + bool EncloseOrigin() + { + switch (m_simplex->rank) + { + case 1: + { + for (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 < mindist)) + { + mindist = subd; + m = static_cast(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + } + } + } + if (mindist < 0) + { + const b3Scalar d = b3Dot(a, n); + const b3Scalar s = b3Sqrt(l); + const b3Vector3 p = n * (d / l); + mindist = p.length2(); + m = 7; + w[0] = (b3Cross(dl[1], b - p)).length() / s; + w[1] = (b3Cross(dl[2], c - p)).length() / s; + w[2] = 1 - (w[0] + w[1]); + } + return (mindist); + } + return (-1); + } + static b3Scalar projectorigin(const b3Vector3& a, + const b3Vector3& b, + const b3Vector3& c, + const b3Vector3& d, + b3Scalar* w, unsigned int& m) + { + static const unsigned int imd3[] = {1, 2, 0}; + const b3Vector3* vt[] = {&a, &b, &c, &d}; + const b3Vector3 dl[] = {a - d, b - d, c - d}; + const b3Scalar vl = det(dl[0], dl[1], dl[2]); + const bool ng = (vl * b3Dot(a, b3Cross(b - c, a - b))) <= 0; + if (ng && (b3Fabs(vl) > 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 < mindist)) + { + mindist = subd; + m = static_cast((subm & 1 ? 1 << i : 0) + + (subm & 2 ? 1 << j : 0) + + (subm & 4 ? 8 : 0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + w[3] = subw[2]; + } + } + } + if (mindist < 0) + { + mindist = 0; + m = 15; + w[0] = det(c, b, d) / vl; + w[1] = det(a, c, d) / vl; + w[2] = det(b, a, d) / vl; + w[3] = 1 - (w[0] + w[1] + w[2]); + } + return (mindist); + } + return (-1); + } +}; + +// EPA +struct b3EPA +{ + /* Types */ + typedef b3GJK::sSV sSV; + struct sFace + { + b3Vector3 n; + b3Scalar d; + sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + unsigned char e[3]; + unsigned char pass; + }; + struct sList + { + sFace* root; + unsigned int count; + sList() : root(0), count(0) {} + }; + struct sHorizon + { + sFace* cf; + sFace* ff; + unsigned int nf; + sHorizon() : cf(0), ff(0), nf(0) {} + }; + struct eStatus + { + enum _ + { + Valid, + Touching, + Degenerated, + NonConvex, + InvalidHull, + OutOfFaces, + OutOfVertices, + AccuraryReached, + FallBack, + Failed + }; + }; + /* Fields */ + eStatus::_ m_status; + b3GJK::sSimplex m_result; + b3Vector3 m_normal; + b3Scalar m_depth; + sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + unsigned int m_nextsv; + sList m_hull; + sList m_stock; + /* Methods */ + b3EPA() + { + Initialize(); + } + + static inline void bind(sFace* fa, unsigned int ea, sFace* fb, unsigned int eb) + { + fa->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; i < EPA_MAX_FACES; ++i) + { + append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]); + } + } + eStatus::_ Evaluate(b3GJK& gjk, const b3Vector3& guess) + { + b3GJK::sSimplex& simplex = *gjk.m_simplex; + if ((simplex.rank > 1) && gjk.EncloseOrigin()) + { + /* Clean up */ + while (m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull, f); + append(m_stock, f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if (gjk.det(simplex.c[0]->w - simplex.c[3]->w, + simplex.c[1]->w - simplex.c[3]->w, + simplex.c[2]->w - simplex.c[3]->w) < 0) + { + 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 (; iterations < EPA_MAX_ITERATIONS; ++iterations) + { + if (m_nextsv < EPA_MAX_VERTICES) + { + sHorizon horizon; + sSV* w = &m_sv_store[m_nextsv++]; + bool valid = true; + best->pass = (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 (sqd < mind) + { + minf = f; + mind = sqd; + } + } + return (minf); + } + bool expand(unsigned int pass, sSV* w, sFace* f, unsigned int e, sHorizon& horizon) + { + static const unsigned int i1m3[] = {1, 2, 0}; + static const unsigned int i2m3[] = {2, 0, 1}; + if (f->pass != 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); +} + +} // namespace gjkepa2_impl2 + +// +// 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; i < gjk.m_simplex->rank; ++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; i < epa.m_result.rank; ++i) + { + w0 += shape.Support(epa.m_result.c[i]->d, 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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h new file mode 100644 index 000000000..7db32c630 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h @@ -0,0 +1,79 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp new file mode 100644 index 000000000..6f2c5251a --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp @@ -0,0 +1,358 @@ +/* +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 << MAX_NUM_PARTS_IN_BITS)); + b3Assert(triangleIndex < (1 << (31 - MAX_NUM_PARTS_IN_BITS))); + //negative indices are reserved for escapeIndex + b3Assert(triangleIndex >= 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_SubtreeHeaders.size(); i++) + { + b3BvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; + subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); + } + } + else + { + } +} + +void b3OptimizedBvh::refitPartial(b3StridingMeshInterface* meshInterface, const b3Vector3& aabbMin, const b3Vector3& aabbMax) +{ + //incrementally initialize quantization values + b3Assert(m_useQuantization); + + b3Assert(aabbMin.getX() > 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; i < this->m_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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h new file mode 100644 index 000000000..128655293 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h @@ -0,0 +1,56 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp new file mode 100644 index 000000000..9a448495f --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp @@ -0,0 +1,1254 @@ +/* +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; i < endIndex; i++) + { + mergeInternalNodeAabb(m_curNodeIndex, getAabbMin(i), getAabbMax(i)); + } + + m_curNodeIndex++; + + //internalNode->m_escapeIndex; + + int leftChildNodexIndex = m_curNodeIndex; + + //build left child tree + buildTree(startIndex, splitIndex); + + int rightChildNodexIndex = m_curNodeIndex; + //build right child tree + buildTree(splitIndex, endIndex); + +#ifdef DEBUG_TREE_BUILDING + gStackDepth--; +#endif //DEBUG_TREE_BUILDING + + int escapeIndex = m_curNodeIndex - curIndex; + + if (m_useQuantization) + { + //escapeIndex is the number of nodes of this subtree + const int sizeQuantizedNode = sizeof(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 < endIndex; i++) + { + b3Vector3 center = b3Scalar(0.5) * (getAabbMax(i) + getAabbMin(i)); + means += center; + } + means *= (b3Scalar(1.) / (b3Scalar)numIndices); + + splitValue = means[splitAxis]; + + //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. + for (i = startIndex; i < endIndex; i++) + { + b3Vector3 center = b3Scalar(0.5) * (getAabbMax(i) + getAabbMin(i)); + if (center[splitAxis] > splitValue) + { + //swap + swapLeafNodes(i, splitIndex); + splitIndex++; + } + } + + //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex + //otherwise the tree-building might fail due to stack-overflows in certain cases. + //unbalanced1 is unsafe: it can cause stack overflows + //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); + + //unbalanced2 should work too: always use center (perfect balanced trees) + //bool unbalanced2 = true; + + //this should be safe too: + int rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); + + if (unbalanced) + { + splitIndex = startIndex + (numIndices >> 1); + } + + bool unbal = (splitIndex == startIndex) || (splitIndex == (endIndex)); + (void)unbal; + 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; i < endIndex; i++) + { + b3Vector3 center = b3Scalar(0.5) * (getAabbMax(i) + getAabbMin(i)); + means += center; + } + means *= (b3Scalar(1.) / (b3Scalar)numIndices); + + for (i = startIndex; i < endIndex; i++) + { + b3Vector3 center = b3Scalar(0.5) * (getAabbMax(i) + getAabbMin(i)); + b3Vector3 diff2 = center - means; + diff2 = diff2 * diff2; + variance += diff2; + } + variance *= (b3Scalar(1.) / ((b3Scalar)numIndices - 1)); + + return variance.maxAxis(); +} + +void b3QuantizedBvh::reportAabbOverlappingNodex(b3NodeOverlapCallback* nodeCallback, const b3Vector3& aabbMin, const b3Vector3& aabbMax) const +{ + //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) + + if (m_useQuantization) + { + ///quantize query AABB + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + quantizeWithClamp(quantizedQueryAabbMin, aabbMin, 0); + quantizeWithClamp(quantizedQueryAabbMax, aabbMax, 1); + + switch (m_traversalMode) + { + case TRAVERSAL_STACKLESS: + walkStacklessQuantizedTree(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax, 0, m_curNodeIndex); + break; + case TRAVERSAL_STACKLESS_CACHE_FRIENDLY: + walkStacklessQuantizedTreeCacheFriendly(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax); + break; + case TRAVERSAL_RECURSIVE: + { + const b3QuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0]; + walkRecursiveQuantizedTreeAgainstQueryAabb(rootNode, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax); + } + break; + default: + //unsupported + b3Assert(0); + } + } + else + { + walkStacklessTree(nodeCallback, aabbMin, aabbMax); + } +} + +static int b3s_maxIterations = 0; + +void b3QuantizedBvh::walkStacklessTree(b3NodeOverlapCallback* nodeCallback, const b3Vector3& aabbMin, const b3Vector3& aabbMax) 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; + + while (curIndex < m_curNodeIndex) + { + //catch bugs in tree data + b3Assert(walkIterations < m_curNodeIndex); + + walkIterations++; + aabbOverlap = b3TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode->m_aabbMinOrg, rootNode->m_aabbMaxOrg); + isLeafNode = rootNode->m_escapeIndex == -1; + + //PCK: unsigned instead of bool + if (isLeafNode && (aabbOverlap != 0)) + { + nodeCallback->processNode(rootNode->m_subPart, rootNode->m_triangleIndex); + } + + //PCK: unsigned instead of bool + if ((aabbOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } + else + { + escapeIndex = rootNode->m_escapeIndex; + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (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; i < this->m_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; i < numElem; i++, memPtr++) + { + m_contiguousNodes[i].m_aabbMaxOrg.deSerializeFloat(memPtr->m_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; i < numElem; i++, memPtr++) + { + m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_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; i < numElem; i++, memPtr++) + { + m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_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; i < numElem; i++, memPtr++) + { + m_contiguousNodes[i].m_aabbMaxOrg.deSerializeDouble(memPtr->m_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; i < numElem; i++, memPtr++) + { + m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_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; i < numElem; i++, memPtr++) + { + m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h new file mode 100644 index 000000000..48b41abca --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h @@ -0,0 +1,511 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.cpp new file mode 100644 index 000000000..6b0c941f2 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.cpp @@ -0,0 +1,207 @@ +/* +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; part < graphicssubparts; part++) + { + getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part); + numtotalphysicsverts += numtriangles * 3; //upper bound + + ///unlike that developers want to pass in double-precision meshes in single-precision Bullet build + ///so disable this feature by default + ///see patch http://code.google.com/p/bullet/issues/detail?id=213 + + switch (type) + { + case PHY_FLOAT: + { + float* graphicsbase; + + switch (gfxindextype) + { + case PHY_INTEGER: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + case PHY_SHORT: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + case PHY_UCHAR: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(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; gfxindex < numtriangles; gfxindex++) + { + unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((b3Scalar)graphicsbase[0] * meshScaling.getX(), (b3Scalar)graphicsbase[1] * meshScaling.getY(), (b3Scalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((b3Scalar)graphicsbase[0] * meshScaling.getX(), (b3Scalar)graphicsbase[1] * meshScaling.getY(), (b3Scalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((b3Scalar)graphicsbase[0] * meshScaling.getX(), (b3Scalar)graphicsbase[1] * meshScaling.getY(), (b3Scalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + case PHY_SHORT: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((b3Scalar)graphicsbase[0] * meshScaling.getX(), (b3Scalar)graphicsbase[1] * meshScaling.getY(), (b3Scalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((b3Scalar)graphicsbase[0] * meshScaling.getX(), (b3Scalar)graphicsbase[1] * meshScaling.getY(), (b3Scalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((b3Scalar)graphicsbase[0] * meshScaling.getX(), (b3Scalar)graphicsbase[1] * meshScaling.getY(), (b3Scalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + case PHY_UCHAR: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((b3Scalar)graphicsbase[0] * meshScaling.getX(), (b3Scalar)graphicsbase[1] * meshScaling.getY(), (b3Scalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((b3Scalar)graphicsbase[0] * meshScaling.getX(), (b3Scalar)graphicsbase[1] * meshScaling.getY(), (b3Scalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((b3Scalar)graphicsbase[0] * meshScaling.getX(), (b3Scalar)graphicsbase[1] * meshScaling.getY(), (b3Scalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.h new file mode 100644 index 000000000..087b30f3e --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.h @@ -0,0 +1,158 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h new file mode 100644 index 000000000..9ca1e2294 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h @@ -0,0 +1,34 @@ + +#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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.cpp new file mode 100644 index 000000000..3908c6de8 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.cpp @@ -0,0 +1,24 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.h new file mode 100644 index 000000000..a0fd3e7ac --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.h @@ -0,0 +1,37 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.cpp new file mode 100644 index 000000000..73faadbdd --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.cpp @@ -0,0 +1,90 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h new file mode 100644 index 000000000..b6ceb8df1 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h @@ -0,0 +1,128 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h new file mode 100644 index 000000000..5cc4b5a62 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h @@ -0,0 +1,10 @@ +#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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp new file mode 100644 index 000000000..dae61d458 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp @@ -0,0 +1,574 @@ + +/* +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 < numverts; i++) + { + b3Scalar curLen2 = m_simplexVectorW[i].length2(); + if (maxV < curLen2) + maxV = curLen2; + } + return maxV; +} + +//return the current simplex +int b3VoronoiSimplexSolver::getSimplex(b3Vector3* pBuf, b3Vector3* qBuf, b3Vector3* yBuf) const +{ + int i; + for (i = 0; i < numVertices(); i++) + { + yBuf[i] = m_simplexVectorW[i]; + pBuf[i] = m_simplexPointsP[i]; + qBuf[i] = m_simplexPointsQ[i]; + } + return numVertices(); +} + +bool b3VoronoiSimplexSolver::inSimplex(const b3Vector3& w) +{ + bool found = false; + int i, numverts = numVertices(); + //b3Scalar maxV = b3Scalar(0.); + + //w is in the current (reduced) simplex + for (i = 0; i < numverts; i++) + { +#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD + if (m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold) +#else + if (m_simplexVectorW[i] == w) +#endif + found = true; + } + + //check in case lastW is already removed + if (w == m_lastW) + return true; + + return found; +} + +void b3VoronoiSimplexSolver::backup_closest(b3Vector3& v) +{ + v = m_cachedV; +} + +bool b3VoronoiSimplexSolver::emptySimplex() const +{ + return (numVertices() == 0); +} + +void b3VoronoiSimplexSolver::compute_points(b3Vector3& p1, b3Vector3& p2) +{ + updateClosestVectorAndPoints(); + p1 = m_cachedP1; + p2 = m_cachedP2; +} + +bool b3VoronoiSimplexSolver::closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, b3SubSimplexClosestResult& result) +{ + result.m_usedVertices.reset(); + + // Check if P in vertex region outside A + b3Vector3 ab = b - a; + b3Vector3 ac = c - a; + b3Vector3 ap = p - a; + b3Scalar d1 = ab.dot(ap); + b3Scalar d2 = ac.dot(ap); + if (d1 <= b3Scalar(0.0) && d2 <= b3Scalar(0.0)) + { + result.m_closestPointOnSimplex = a; + result.m_usedVertices.usedVertexA = true; + result.setBarycentricCoordinates(1, 0, 0); + return true; // a; // barycentric coordinates (1,0,0) + } + + // Check if P in vertex region outside B + b3Vector3 bp = p - b; + b3Scalar d3 = ab.dot(bp); + b3Scalar d4 = ac.dot(bp); + if (d3 >= 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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h new file mode 100644 index 000000000..b40b16997 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h @@ -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. +*/ + +#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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl new file mode 100644 index 000000000..faa413441 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h new file mode 100644 index 000000000..b2e0a2dd4 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h @@ -0,0 +1,1288 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl b/Engine/lib/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl new file mode 100644 index 000000000..a6565fd6f --- /dev/null +++ b/Engine/lib/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 < nSrc - 1; i++) + b3Assert(src[i].m_key <= src[i + 1].m_key); + + b3SortData minData, zeroData, maxData; + minData.m_key = -1; + minData.m_value = -1; + zeroData.m_key = 0; + zeroData.m_value = 0; + maxData.m_key = nDst; + maxData.m_value = nDst; + + if (option == BOUND_LOWER) + { + for (int i = 0; i < nSrc; i++) + { + b3SortData& iData = (i == 0) ? minData : src[i - 1]; + b3SortData& jData = (i == nSrc) ? maxData : src[i]; + + if (iData.m_key != jData.m_key) + { + int k = jData.m_key; + { + dst[k] = i; + } + } + } + } + else if (option == BOUND_UPPER) + { + for (int i = 1; i < nSrc + 1; i++) + { + b3SortData& iData = src[i - 1]; + b3SortData& jData = (i == nSrc) ? maxData : src[i]; + + if (iData.m_key != jData.m_key) + { + int k = iData.m_key; + { + dst[k] = i; + } + } + } + } + else if (option == COUNT) + { + b3AlignedObjectArray lower; + lower.resize(nDst); + b3AlignedObjectArray upper; + upper.resize(nDst); + + for (int i = 0; i < nDst; i++) + { + lower[i] = upper[i] = 0; + } + + executeHost(src, nSrc, lower, nDst, BOUND_LOWER); + executeHost(src, nSrc, upper, nDst, BOUND_UPPER); + + for (int i = 0; i < nDst; i++) + { + dst[i] = upper[i] - lower[i]; + } + } + else + { + b3Assert(0); + } +} diff --git a/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.h b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.h new file mode 100644 index 000000000..0d633e3d2 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.h @@ -0,0 +1,64 @@ +/* +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 B3_BOUNDSEARCH_H +#define B3_BOUNDSEARCH_H + +#pragma once + +/*#include +#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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BufferInfoCL.h b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BufferInfoCL.h new file mode 100644 index 000000000..35fc467b2 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BufferInfoCL.h @@ -0,0 +1,18 @@ + +#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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.cpp new file mode 100644 index 000000000..bd25bb210 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.cpp @@ -0,0 +1,119 @@ +#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 < n; i++) + { + src[i + offset] = value; + } +} + +void b3FillCL::executeHost(b3AlignedObjectArray& src, const int value, int n, int offset) +{ + for (int i = 0; i < n; i++) + { + src[i + offset] = value; + } +} + +void b3FillCL::execute(b3OpenCLArray& 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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.h b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.h new file mode 100644 index 000000000..c92c3e511 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.h @@ -0,0 +1,52 @@ +#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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp new file mode 100644 index 000000000..c97d02eb4 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp @@ -0,0 +1,296 @@ +#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 < m_arrays.size(); i++) + { + delete (m_arrays[i]); + } + + m_arrays.clear(); + if (gDebugLauncherCL) + { + static int counter = 0; + printf("[%d] Finished launching OpenCL kernel %s\n", counter++, m_name); + } +} + +void b3LauncherCL::setBuffer(cl_mem clBuffer) +{ + if (m_enableSerialization) + { + b3KernelArgData kernelArg; + kernelArg.m_argIndex = m_idx; + kernelArg.m_isBuffer = 1; + kernelArg.m_clBuffer = clBuffer; + + cl_mem_info param_name = CL_MEM_SIZE; + size_t param_value; + size_t sizeInBytes = sizeof(size_t); + size_t actualSizeInBytes; + cl_int err; + err = clGetMemObjectInfo(kernelArg.m_clBuffer, + param_name, + sizeInBytes, + ¶m_value, + &actualSizeInBytes); + + b3Assert(err == CL_SUCCESS); + kernelArg.m_argSizeInBytes = param_value; + + m_kernelArguments.push_back(kernelArg); + m_serializationSizeInBytes += sizeof(b3KernelArgData); + m_serializationSizeInBytes += param_value; + } + cl_int status = clSetKernelArg(m_kernel, m_idx++, sizeof(cl_mem), &clBuffer); + b3Assert(status == CL_SUCCESS); +} + +void b3LauncherCL::setBuffers(b3BufferInfoCL* buffInfo, int n) +{ + for (int i = 0; i < n; i++) + { + if (m_enableSerialization) + { + b3KernelArgData kernelArg; + kernelArg.m_argIndex = m_idx; + kernelArg.m_isBuffer = 1; + kernelArg.m_clBuffer = buffInfo[i].m_clBuffer; + + cl_mem_info param_name = CL_MEM_SIZE; + size_t param_value; + size_t sizeInBytes = sizeof(size_t); + size_t actualSizeInBytes; + cl_int err; + err = clGetMemObjectInfo(kernelArg.m_clBuffer, + param_name, + sizeInBytes, + ¶m_value, + &actualSizeInBytes); + + b3Assert(err == CL_SUCCESS); + kernelArg.m_argSizeInBytes = param_value; + + m_kernelArguments.push_back(kernelArg); + m_serializationSizeInBytes += sizeof(b3KernelArgData); + m_serializationSizeInBytes += param_value; + } + cl_int status = clSetKernelArg(m_kernel, m_idx++, sizeof(cl_mem), &buffInfo[i].m_clBuffer); + b3Assert(status == CL_SUCCESS); + } +} + +struct b3KernelArgDataUnaligned +{ + 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]; + }; +}; +#include + +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; i < numArguments; i++) + { + b3KernelArgDataUnaligned* arg = (b3KernelArgDataUnaligned*)&buf[index]; + + index += sizeof(b3KernelArgData); + if (arg->m_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; ii < numArguments; ii++) + { + b3KernelArgData* argGold = (b3KernelArgData*)&goldBuffer[index]; + + if (m_kernelArguments[ii].m_argSizeInBytes != argGold->m_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; j < m_kernelArguments[j].m_argSizeInBytes; j++) + { + memBuf[j] = 0xaa; + } + + cl_int status = 0; + status = clEnqueueReadBuffer(m_commandQueue, m_kernelArguments[ii].m_clBuffer, CL_TRUE, 0, m_kernelArguments[ii].m_argSizeInBytes, + memBuf, 0, 0, 0); + b3Assert(status == CL_SUCCESS); + clFinish(m_commandQueue); + + for (int b = 0; b < m_kernelArguments[ii].m_argSizeInBytes; b++) + { + int expected = goldBuf[b]; + int found = memBuf[b]; + if (expected != found) + { + printf("failed validation: argument %d OpenCL data at byte position %d expected: %d, found %d\n", + ii, b, expected, found); + return -4; + } + } + + index += argGold->m_argSizeInBytes; + } + else + { + //compare content + for (int b = 0; b < m_kernelArguments[ii].m_argSizeInBytes; b++) + { + int expected = argGold->m_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 < destBufferCapacity; i++) + destBuffer[i] = 0xec; + + assert(destBufferCapacity >= 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; i < this->m_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 < buffSize + 1; i++) + { + unsigned char* ptr = (unsigned char*)&buf[i]; + *ptr = 0xff; + } + // int actualWrite = serializeArguments(buf,buffSize); + + // unsigned char* cptr = (unsigned char*)&buf[buffSize]; + // printf("buf[buffSize] = %d\n",*cptr); + + assert(buf[buffSize] == 0xff); //check for buffer overrun + int* ptr = (int*)&buf[buffSize]; + + *ptr = num; + + FILE* f = fopen(fileName, "wb"); + fwrite(buf, buffSize + sizeof(int), 1, f); + fclose(f); + + delete[] buf; +} diff --git a/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h new file mode 100644 index 000000000..18e9c1db2 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h @@ -0,0 +1,128 @@ + +#ifndef B3_LAUNCHER_CL_H +#define B3_LAUNCHER_CL_H + +#include "b3BufferInfoCL.h" +#include "Bullet3Common/b3MinMax.h" +#include "b3OpenCLArray.h" +#include + +#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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h new file mode 100644 index 000000000..e837cceb6 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h @@ -0,0 +1,300 @@ +#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 < capacity()); + T elem; + copyToHostPointer(&elem, 1, n, true); + return elem; + } + + B3_FORCE_INLINE T at(size_t n) const + { + b3Assert(n >= 0); + b3Assert(n < size()); + T elem; + copyToHostPointer(&elem, 1, n, true); + return elem; + } + + B3_FORCE_INLINE bool resize(size_t newsize, bool copyOldContents = true) + { + bool result = true; + size_t curSize = size(); + + if (newsize < curSize) + { + //leave the OpenCL memory for now + } + else + { + if (newsize > size()) + { + result = reserve(newsize, copyOldContents); + } + + //leave new data uninitialized (init in debug mode?) + //for (size_t i=curSize;i 0); + 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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.cpp new file mode 100644 index 000000000..822b51163 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.cpp @@ -0,0 +1,120 @@ +#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 < sizeof(T) * 8; i++) + n = n | (n >> 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 < n; i++) + { + dst[i] = s; + s += src[i]; + } + } + /*else + { + 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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.cpp new file mode 100644 index 000000000..1cac97c98 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.cpp @@ -0,0 +1,120 @@ +#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 < sizeof(T) * 8; i++) + n = n | (n >> 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 < n; i++) + { + dst[i] = s; + s += src[i]; + } + } + /*else + { + 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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp new file mode 100644 index 000000000..e86af6583 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp @@ -0,0 +1,646 @@ + +#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 << BITS_PER_PASS); + + int tables[NUM_TABLES]; + int counter[NUM_TABLES]; + + b3SortData* src = &inout[0]; + b3AlignedObjectArray workbuffer; + workbuffer.resize(inout.size()); + b3SortData* dst = &workbuffer[0]; + + int count = 0; + for (int startBit = 0; startBit < sortBits; startBit += BITS_PER_PASS) + { + for (int i = 0; i < NUM_TABLES; i++) + { + tables[i] = 0; + } + + for (int i = 0; i < n; i++) + { + int tableIdx = (src[i].m_key >> startBit) & (NUM_TABLES - 1); + tables[tableIdx]++; + } +//#define TEST +#ifdef TEST + printf("histogram size=%d\n", NUM_TABLES); + for (int i = 0; i < NUM_TABLES; i++) + { + if (tables[i] != 0) + { + printf("tables[%d]=%d]\n", i, tables[i]); + } + } +#endif //TEST \ + // prefix scan + int sum = 0; + for (int i = 0; i < NUM_TABLES; i++) + { + int iData = tables[i]; + tables[i] = sum; + sum += iData; + counter[i] = 0; + } + + // distribute + for (int i = 0; i < n; i++) + { + int tableIdx = (src[i].m_key >> 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 < test2.size(); i++) + { + printf("test2[%d].m_key=%d\n", i, test2[i].m_key); + printf("test2[%d].m_value=%d\n", i, test2[i].m_value); + } +#endif //DEBUG_RADIXSORT2 + + b3OpenCLArray* 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; i < workingSize; i++) + { + m_workBuffer4->copyFromHostPointer(&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; ib < sortBits; ib += 4) + { +#ifdef DEBUG_RADIXSORT2 + keyValuesInOut.copyToHost(test2); + printf("numElem = %d\n", test2.size()); + for (int i = 0; i < test2.size(); i++) + { + if (test2[i].m_key != test2[i].m_value) + { + printf("test2[%d].m_key=%d\n", i, test2[i].m_key); + printf("test2[%d].m_value=%d\n", i, test2[i].m_value); + } + } +#endif //DEBUG_RADIXSORT2 + + cdata.m_startBit = ib; + + if (src->size()) + { + 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; i < testHist.size(); i++) + { + if (testHist[i] != 0) + printf("testHist[%d]=%d\n", i, testHist[i]); + } +#endif //DEBUG_RADIXSORT + +//fast prefix scan is not working properly on Mac OSX yet +#ifdef __APPLE__ + bool fastScan = false; +#else + bool fastScan = !m_deviceCPU; //only use fast scan on GPU +#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); + } + +#ifdef DEBUG_RADIXSORT + destHisto->copyToHost(testHist); + printf("ib = %d, testHist size = %d, non zero elements:\n", ib, testHist.size()); + for (int i = 0; i < testHist.size(); i++) + { + if (testHist[i] != 0) + printf("testHist[%d]=%d\n", i, testHist[i]); + } + + for (int i = 0; i < testHist.size(); i += NUM_WGS) + { + printf("testHist[%d]=%d\n", i / NUM_WGS, testHist[i]); + } + +#endif //DEBUG_RADIXSORT + +#define USE_GPU +#ifdef USE_GPU + + 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_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 < NUM_TABLES; i++) + { + tables[i] = testHist[i * NUM_WGS]; + } + + // distribute + for (int i = 0; i < n; i++) + { + int tableIdx = (srcHost[i].m_key >> 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 < NUM_TABLES; i++) + { + tables[i] = testHist[i * NUM_WGS]; + } + + // distribute + for (int i = 0; i < n; i++) + { + int tableIdx = (srcHost[i].m_key >> 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 < NUM_WGS; wgIdx++) + { + int counter[NUM_TABLES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + int nBlocks = (n) / blockSize - nBlocksPerWG * wgIdx; + + for (int iblock = 0; iblock < b3Min(cdata.m_nBlocksPerWG, nBlocks); iblock++) + { + for (int lIdx = 0; lIdx < 64; lIdx++) + { + int addr = iblock * blockSize + blockSize * cdata.m_nBlocksPerWG * wgIdx + ELEMENTS_PER_WORK_ITEM * lIdx; + + // MY_HISTOGRAM( localKeys.x ) ++ is much expensive than atomic add as it requires read and write while atomics can just add on AMD + // Using registers didn't perform well. It seems like use localKeys to address requires a lot of alu ops + // AMD: AtomInc performs better while NV prefers ++ + for (int j = 0; j < ELEMENTS_PER_WORK_ITEM; j++) + { + if (addr + j < n) + { + // printf ("addr+j=%d\n", addr+j); + + int i = addr + j; + + int tableIdx = (srcHost[i].m_key >> 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; i < testHist.size(); i++) + { + if (testHist[i] != 0) + printf("testHist[%d]=%d\n", i, testHist[i]); + } +#endif //DEBUG_RADIXSORT + b3Swap(src, dst); + b3Swap(srcHisto, destHisto); + +#ifdef DEBUG_RADIXSORT2 + keyValuesInOut.copyToHost(test2); + printf("numElem = %d\n", test2.size()); + for (int i = 0; i < test2.size(); i++) + { + if (test2[i].m_key != test2[i].m_value) + { + printf("test2[%d].m_key=%d\n", i, test2[i].m_key); + printf("test2[%d].m_value=%d\n", i, test2[i].m_value); + } + } +#endif //DEBUG_RADIXSORT2 + + count++; + } + + if (count & 1) + { + b3Assert(0); //need to copy from workbuffer to keyValuesInOut + } + + if (m_workBuffer4->size()) + { + 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 < test2.size(); i++) + { + printf("test2[%d].m_key=%d\n", i, test2[i].m_key); + printf("test2[%d].m_value=%d\n", i, test2[i].m_value); + } +#endif +} + +void b3RadixSort32CL::execute(b3OpenCLArray& 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; ib < sortBits; ib += 4) + { + cdata.m_startBit = ib; + + if (src->size()) + { + 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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h new file mode 100644 index 000000000..69caf182d --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h @@ -0,0 +1,84 @@ + +#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 << BITS_PER_PASS), + // if you change this, change nPerWI in kernel as well + NUM_WGS = 20 * 6, // cypress + // NUM_WGS = 24*6, // cayman + // NUM_WGS = 32*4, // nv + }; + +private: +public: + b3RadixSort32CL(cl_context ctx, cl_device_id device, cl_command_queue queue, int initialCapacity = 0); + + virtual ~b3RadixSort32CL(); + + void execute(b3OpenCLArray& 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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernels.cl b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernels.cl new file mode 100644 index 000000000..f3b4a1e8a --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernelsCL.h b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernelsCL.h new file mode 100644 index 000000000..1758dd41e --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernelsCL.h @@ -0,0 +1,86 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernels.cl b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernels.cl new file mode 100644 index 000000000..2eee5752e --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernelsCL.h b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernelsCL.h new file mode 100644 index 000000000..33c927946 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernelsCL.h @@ -0,0 +1,131 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernels.cl b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernels.cl new file mode 100644 index 000000000..71c31075d --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernelsCL.h b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernelsCL.h new file mode 100644 index 000000000..983e65227 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernelsCL.h @@ -0,0 +1,90 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanFloat4Kernels.cl b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanFloat4Kernels.cl new file mode 100644 index 000000000..c9da79854 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/RadixSort32KernelsCL.h b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/RadixSort32KernelsCL.h new file mode 100644 index 000000000..fb4bdda30 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/RadixSort32KernelsCL.h @@ -0,0 +1,909 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp new file mode 100644 index 000000000..6571f3054 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp @@ -0,0 +1,374 @@ + +#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 < poly.m_numFaces; i++) + { + const b3GpuFace& face = faces[poly.m_faceOffset + i]; + float fromPlaneDist = b3Dot(rayFromLocal, face.m_plane) + face.m_plane.w; + float toPlaneDist = b3Dot(rayToLocal, face.m_plane) + face.m_plane.w; + if (fromPlaneDist < 0.f) + { + if (toPlaneDist >= 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; r < rays.size(); r++) + { + b3Vector3 rayFrom = rays[r].m_from; + b3Vector3 rayTo = rays[r].m_to; + float hitFraction = hitResults[r].m_hitFraction; + + int hitBodyIndex = -1; + b3Vector3 hitNormal; + + for (int b = 0; b < numBodies; b++) + { + const b3Vector3& pos = bodies[b].m_pos; + //const b3Quaternion& orn = bodies[b].m_quat; + + switch (collidables[bodies[b].m_collidableIdx].m_shapeType) + { + case SHAPE_SPHERE: + { + b3Scalar radius = collidables[bodies[b].m_collidableIdx].m_radius; + if (sphere_intersect(pos, radius, rayFrom, rayTo, hitFraction)) + { + hitBodyIndex = b; + b3Vector3 hitPoint; + hitPoint.setInterpolate3(rays[r].m_from, rays[r].m_to, hitFraction); + hitNormal = (hitPoint - bodies[b].m_pos).normalize(); + } + } + case SHAPE_CONVEX_HULL: + { + b3Transform convexWorldTransform; + convexWorldTransform.setIdentity(); + convexWorldTransform.setOrigin(bodies[b].m_pos); + convexWorldTransform.setRotation(bodies[b].m_quat); + b3Transform convexWorld2Local = convexWorldTransform.inverse(); + + b3Vector3 rayFromLocal = convexWorld2Local(rayFrom); + b3Vector3 rayToLocal = convexWorld2Local(rayTo); + + int shapeIndex = collidables[bodies[b].m_collidableIdx].m_shapeIndex; + const b3ConvexPolyhedronData& poly = narrowphaseData->m_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*>(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/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h b/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h new file mode 100644 index 000000000..f1f6ffd40 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h @@ -0,0 +1,28 @@ +#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/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl b/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl new file mode 100644 index 000000000..e72d96876 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h b/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h new file mode 100644 index 000000000..94f6a8eb9 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h @@ -0,0 +1,380 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h new file mode 100644 index 000000000..89c0142ab --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h @@ -0,0 +1,17 @@ + +#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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp new file mode 100644 index 000000000..a271090af --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp @@ -0,0 +1,134 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h new file mode 100644 index 000000000..1f163ba7d --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h @@ -0,0 +1,128 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp new file mode 100644 index 000000000..089fb1f6a --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp @@ -0,0 +1,1305 @@ + +#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; i < src->m_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 (i < src->m_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 < numBodies; i++) + bodyCount[i] = 0; + + b3AlignedObjectArray contactConstraintOffsets; + contactConstraintOffsets.resize(numManifolds); + + for (int i = 0; i < numManifolds; i++) + { + int pa = manifoldPtr[i].m_bodyAPtrAndSignBit; + int pb = manifoldPtr[i].m_bodyBPtrAndSignBit; + + bool isFixedA = (pa < 0) || (pa == solverInfo.m_fixedBodyIndex); + bool isFixedB = (pb < 0) || (pb == solverInfo.m_fixedBodyIndex); + + int bodyIndexA = manifoldPtr[i].getBodyA(); + int bodyIndexB = manifoldPtr[i].getBodyB(); + + if (!isFixedA) + { + contactConstraintOffsets[i].x = bodyCount[bodyIndexA]; + bodyCount[bodyIndexA]++; + } + if (!isFixedB) + { + contactConstraintOffsets[i].y = bodyCount[bodyIndexB]; + bodyCount[bodyIndexB]++; + } + } + + b3AlignedObjectArray 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 < numManifolds; i++) + { + ContactToConstraintKernel(&manifoldPtr[0], bodies, inertias, &contactConstraints[0], numManifolds, + solverInfo.m_deltaTime, + solverInfo.m_positionDrift, + solverInfo.m_positionConstraintCoeff, + i, bodyCount); + } + int maxIter = solverInfo.m_numIterations; + + b3AlignedObjectArray deltaLinearVelocities; + b3AlignedObjectArray deltaAngularVelocities; + deltaLinearVelocities.resize(totalNumSplitBodies); + deltaAngularVelocities.resize(totalNumSplitBodies); + for (unsigned int i = 0; i < totalNumSplitBodies; i++) + { + deltaLinearVelocities[i].setZero(); + deltaAngularVelocities[i].setZero(); + } + + for (int iter = 0; iter < maxIter; iter++) + { + int i = 0; + for (i = 0; i < numManifolds; i++) + { + //float frictionCoeff = contactConstraints[i].getFrictionCoeff(); + int aIdx = (int)contactConstraints[i].m_bodyA; + int bIdx = (int)contactConstraints[i].m_bodyB; + b3RigidBodyData& bodyA = bodies[aIdx]; + b3RigidBodyData& bodyB = bodies[bIdx]; + + b3Vector3 zero = b3MakeVector3(0, 0, 0); + + b3Vector3* dlvAPtr = &zero; + b3Vector3* davAPtr = &zero; + b3Vector3* dlvBPtr = &zero; + b3Vector3* davBPtr = &zero; + + if (bodyA.m_invMass) + { + int bodyOffsetA = offsetSplitBodies[aIdx]; + int constraintOffsetA = contactConstraintOffsets[i].x; + int splitIndexA = bodyOffsetA + constraintOffsetA; + dlvAPtr = &deltaLinearVelocities[splitIndexA]; + davAPtr = &deltaAngularVelocities[splitIndexA]; + } + + if (bodyB.m_invMass) + { + int bodyOffsetB = offsetSplitBodies[bIdx]; + int constraintOffsetB = contactConstraintOffsets[i].y; + int splitIndexB = bodyOffsetB + constraintOffsetB; + dlvBPtr = &deltaLinearVelocities[splitIndexB]; + davBPtr = &deltaAngularVelocities[splitIndexB]; + } + + { + float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX}; + float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f}; + + solveContact(contactConstraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, inertias[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, inertias[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt, *dlvAPtr, *davAPtr, *dlvBPtr, *davBPtr); + } + } + + //easy + for (int i = 0; i < numBodies; i++) + { + if (bodies[i].m_invMass) + { + int bodyOffset = offsetSplitBodies[i]; + int count = bodyCount[i]; + float factor = 1.f / float(count); + b3Vector3 averageLinVel; + averageLinVel.setZero(); + b3Vector3 averageAngVel; + averageAngVel.setZero(); + for (int j = 0; j < count; j++) + { + averageLinVel += deltaLinearVelocities[bodyOffset + j] * factor; + averageAngVel += deltaAngularVelocities[bodyOffset + j] * factor; + } + for (int j = 0; j < count; j++) + { + deltaLinearVelocities[bodyOffset + j] = averageLinVel; + deltaAngularVelocities[bodyOffset + j] = averageAngVel; + } + } + } + } + for (int iter = 0; iter < maxIter; iter++) + { + //int i=0; + + //solve friction + + for (int i = 0; i < numManifolds; i++) + { + 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 += contactConstraints[i].m_appliedRambdaDt[j]; + } + float frictionCoeff = contactConstraints[i].getFrictionCoeff(); + int aIdx = (int)contactConstraints[i].m_bodyA; + int bIdx = (int)contactConstraints[i].m_bodyB; + b3RigidBodyData& bodyA = bodies[aIdx]; + b3RigidBodyData& bodyB = bodies[bIdx]; + + b3Vector3 zero = b3MakeVector3(0, 0, 0); + + b3Vector3* dlvAPtr = &zero; + b3Vector3* davAPtr = &zero; + b3Vector3* dlvBPtr = &zero; + b3Vector3* davBPtr = &zero; + + if (bodyA.m_invMass) + { + int bodyOffsetA = offsetSplitBodies[aIdx]; + int constraintOffsetA = contactConstraintOffsets[i].x; + int splitIndexA = bodyOffsetA + constraintOffsetA; + dlvAPtr = &deltaLinearVelocities[splitIndexA]; + davAPtr = &deltaAngularVelocities[splitIndexA]; + } + + if (bodyB.m_invMass) + { + int bodyOffsetB = offsetSplitBodies[bIdx]; + int constraintOffsetB = contactConstraintOffsets[i].y; + int splitIndexB = bodyOffsetB + constraintOffsetB; + dlvBPtr = &deltaLinearVelocities[splitIndexB]; + davBPtr = &deltaAngularVelocities[splitIndexB]; + } + + for (int j = 0; j < 4; j++) + { + maxRambdaDt[j] = frictionCoeff * sum; + minRambdaDt[j] = -maxRambdaDt[j]; + } + + solveFriction(contactConstraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, inertias[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, inertias[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt, *dlvAPtr, *davAPtr, *dlvBPtr, *davBPtr); + } + + //easy + for (int i = 0; i < numBodies; i++) + { + if (bodies[i].m_invMass) + { + int bodyOffset = offsetSplitBodies[i]; + int count = bodyCount[i]; + float factor = 1.f / float(count); + b3Vector3 averageLinVel; + averageLinVel.setZero(); + b3Vector3 averageAngVel; + averageAngVel.setZero(); + for (int j = 0; j < count; j++) + { + averageLinVel += deltaLinearVelocities[bodyOffset + j] * factor; + averageAngVel += deltaAngularVelocities[bodyOffset + j] * factor; + } + for (int j = 0; j < count; j++) + { + deltaLinearVelocities[bodyOffset + j] = averageLinVel; + deltaAngularVelocities[bodyOffset + j] = averageAngVel; + } + } + } + } + + //easy + for (int i = 0; i < numBodies; i++) + { + if (bodies[i].m_invMass) + { + int bodyOffset = offsetSplitBodies[i]; + int count = bodyCount[i]; + if (count) + { + bodies[i].m_linVel += deltaLinearVelocities[bodyOffset]; + bodies[i].m_angVel += deltaAngularVelocities[bodyOffset]; + } + } + } +} + +void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const struct b3Config& config, int static0Index) +// +// +//void b3GpuJacobiContactSolver::solveGroup(b3OpenCLArray* 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; iter < maxIter; iter++) + { + { + B3_PROFILE("m_solveContactKernel"); + b3LauncherCL launcher(m_queue, m_data->m_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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h new file mode 100644 index 000000000..8281aee05 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h @@ -0,0 +1,56 @@ + +#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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp new file mode 100644 index 000000000..2e4f6c157 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp @@ -0,0 +1,1013 @@ +#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; i < m_data->m_bvhData.size(); i++) + { + delete m_data->m_bvhData[i]; + } + for (int i = 0; i < m_data->m_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 (curSize < m_data->m_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; i < convexPtr->m_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; i < convexPtr->m_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; p < numIndices; p++) + { + m_data->m_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; i < convexPtr->m_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; i < numVertices; i++) + { + float* vertex = (float*)&vts[i * strideInBytes]; + verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2])); + } + + b3ConvexUtility* utilPtr = new b3ConvexUtility(); + bool merge = true; + if (numVertices) + { + utilPtr->initializePolyhedralFeatures(&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; i < utilPtr->m_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; i < utilPtr->m_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_data->m_config.m_maxCompoundChildShapes); + for (int i = 0; i < childShapes->size(); 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; i < childShapes->size(); 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; i < numNodes; i++) + { + b3QuantizedBvhNode node; + b3Vector3 aabbMin, aabbMax; + aabbMin = (b3Vector3&)childLocalAabbs[i].m_min; + aabbMax = (b3Vector3&)childLocalAabbs[i].m_max; + + bvh->quantize(&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; i < numNewNodes - 1; i++) + { + if (bvh->getQuantizedNodeArray()[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; i < numNewSubtrees; i++) + { + m_data->m_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]); + } + int numNewTreeNodes = bvh->getQuantizedNodeArray().size(); + + for (int i = 0; i < numNewTreeNodes; i++) + { + m_data->m_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; i < vertices->size(); 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; i < numNewSubtrees; i++) + { + m_data->m_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]); + } + int numNewTreeNodes = bvh->getQuantizedNodeArray().size(); + + for (int i = 0; i < numNewTreeNodes; i++) + { + m_data->m_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; i < convex.m_numFaces; i++) + { + if (i % 256 == 0) + { + //printf("i=%d out of %d", i,convex.m_numFaces); + } + b3Vector3 vert0(vertices->at(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; p < numIndices; p++) + { + int vi = indices->at(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; i < vertices->size(); 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 && bodyIndex < m_data->m_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 && bodyIndex < m_data->m_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 && bodyIndex < m_data->m_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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h new file mode 100644 index 000000000..21a68de34 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h @@ -0,0 +1,101 @@ +#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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h new file mode 100644 index 000000000..716a5ea0f --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h @@ -0,0 +1,89 @@ + +#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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp new file mode 100644 index 000000000..bd9d6bb04 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp @@ -0,0 +1,1068 @@ + +/* +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; i < numBodies; i++) + { + b3RigidBodyData& body = m_gpuData->m_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; i < numConstraints; i++) + { + unsigned int& info1 = m_tmpConstraintSizesPool[i]; + // unsigned int info1; + if (m_gpuData->m_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; i < numConstraints; i++) + { + const int& info1 = m_tmpConstraintSizesPool[i]; + + if (info1) + { + int constraintIndex = batchConstraints[i].m_originalConstraintIndex; + int constraintRowOffset = m_gpuData->m_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; j < info1; j++) + { + memset(¤tConstraintRow[j], 0, sizeof(b3GpuSolverConstraint)); + currentConstraintRow[j].m_angularComponentA.setValue(0, 0, 0); + currentConstraintRow[j].m_angularComponentB.setValue(0, 0, 0); + currentConstraintRow[j].m_appliedImpulse = 0.f; + currentConstraintRow[j].m_appliedPushImpulse = 0.f; + currentConstraintRow[j].m_cfm = 0.f; + currentConstraintRow[j].m_contactNormal.setValue(0, 0, 0); + currentConstraintRow[j].m_friction = 0.f; + currentConstraintRow[j].m_frictionIndex = 0; + currentConstraintRow[j].m_jacDiagABInv = 0.f; + currentConstraintRow[j].m_lowerLimit = 0.f; + currentConstraintRow[j].m_upperLimit = 0.f; + + currentConstraintRow[j].m_originalContactPoint = 0; + currentConstraintRow[j].m_overrideNumSolverIterations = 0; + currentConstraintRow[j].m_relpos1CrossNormal.setValue(0, 0, 0); + currentConstraintRow[j].m_relpos2CrossNormal.setValue(0, 0, 0); + currentConstraintRow[j].m_rhs = 0.f; + currentConstraintRow[j].m_rhsPenetration = 0.f; + currentConstraintRow[j].m_solverBodyIdA = 0; + currentConstraintRow[j].m_solverBodyIdB = 0; + + currentConstraintRow[j].m_lowerLimit = -B3_INFINITY; + currentConstraintRow[j].m_upperLimit = B3_INFINITY; + currentConstraintRow[j].m_appliedImpulse = 0.f; + currentConstraintRow[j].m_appliedPushImpulse = 0.f; + currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; + currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; + currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; + } + + 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); + + 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 < info1; j++) + { + b3GpuSolverConstraint& solverConstraint = currentConstraintRow[j]; + + if (solverConstraint.m_upperLimit >= 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; i < numManifolds; i++) + { + b3Contact4& manifold = manifoldPtr[i]; + convertContact(bodies, inertias, &manifold, infoGlobal); + } + } +#endif //B3_SUPPORT_CONTACT_CONSTRAINTS + } + + // b3ContactSolverInfo info = infoGlobal; + + // int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); + // int numConstraintPool = m_tmpSolverContactConstraintPool.size(); + // int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); + + return 0.f; +} + +///a straight copy from GPU/OpenCL kernel, for debugging +__inline void internalApplyImpulse(b3GpuSolverBody* body, const b3Vector3& linearComponent, const b3Vector3& angularComponent, float impulseMagnitude) +{ + body->m_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; bb < numBatches; bb++) + { + int numConstraintsInBatch = m_gpuData->m_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; b < numConstraintsInBatch; b++) + { + const b3BatchConstraint& c = batchConstraints[batchOffset + b]; + /*printf("-----------\n"); + printf("bb=%d\n",bb); + printf("c.batchId = %d\n", c.m_batchId); + */ + b3Assert(c.m_batchId == bb); + b3GpuGenericConstraint* constraint = &m_gpuData->m_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; jj < numConstraintRows; jj++) + { + // + b3GpuSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[constraintOffset + jj]; + //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); + resolveSingleConstraintRowGeneric2(&m_tmpSolverBodyPool[constraint.m_solverBodyIdA], &m_tmpSolverBodyPool[constraint.m_solverBodyIdB], &constraint); + } + } + } + } //useGpu + batchOffset += numConstraintsInBatch; + constraintOffset += numConstraintsInBatch; + } + } //for (int iteration... + + if (!useGpuSolveJointConstraintRows) + { + { + B3_PROFILE("copy from host"); + m_gpuData->m_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; j < numJoints; j++) + { + b3GpuSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; + resolveSingleConstraintRowGeneric2(&m_tmpSolverBodyPool[constraint.m_solverBodyIdA], &m_tmpSolverBodyPool[constraint.m_solverBodyIdB], &constraint); + } + + if (!m_usePgs) + { + averageVelocities(); + } + } + } + } + clFinish(m_gpuData->m_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 (maxNumConstraints < numConstraints) + { + maxNumConstraints = numConstraints; + //printf("maxNumConstraints = %d\n",maxNumConstraints ); + } + + int numUsedArray = numBodies / 32 + 1; + bodyUsed.resize(numUsedArray); + + for (int q = 0; q < numUsedArray; q++) + bodyUsed[q] = 0; + + int curBodyUsed = 0; + + int numIter = 0; + +#if defined(_DEBUG) + for (int i = 0; i < numConstraints; i++) + cs[i].m_batchId = -1; +#endif + + int numValidConstraints = 0; + // int unprocessedConstraintIndex = 0; + + int batchIdx = 0; + + { + B3_PROFILE("cpu batch innerloop"); + + while (numValidConstraints < numConstraints) + { + numIter++; + int nCurrentBatch = 0; + // clear flag + for (int i = 0; i < curBodyUsed; i++) + bodyUsed[curUsed[i] / 32] = 0; + + curBodyUsed = 0; + + for (int i = numValidConstraints; i < numConstraints; i++) + { + int idx = i; + b3Assert(idx < numConstraints); + // check if it can go + int bodyAS = cs[idx].m_bodyAPtrAndSignBit; + int bodyBS = cs[idx].m_bodyBPtrAndSignBit; + int bodyA = abs(bodyAS); + int bodyB = abs(bodyBS); + bool aIsStatic = (bodyAS < 0) || bodyAS == staticIdx; + bool bIsStatic = (bodyBS < 0) || bodyBS == staticIdx; + int aUnavailable = 0; + int bUnavailable = 0; + if (!aIsStatic) + { + aUnavailable = bodyUsed[bodyA / 32] & (1 << (bodyA & 31)); + } + if (!aUnavailable) + if (!bIsStatic) + { + bUnavailable = bodyUsed[bodyB / 32] & (1 << (bodyB & 31)); + } + + if (aUnavailable == 0 && bUnavailable == 0) // ok + { + if (!aIsStatic) + { + bodyUsed[bodyA / 32] |= (1 << (bodyA & 31)); + curUsed[curBodyUsed++] = bodyA; + } + if (!bIsStatic) + { + bodyUsed[bodyB / 32] |= (1 << (bodyB & 31)); + curUsed[curBodyUsed++] = bodyB; + } + + cs[idx].m_batchId = batchIdx; + + if (i != numValidConstraints) + { + b3Swap(cs[i], cs[numValidConstraints]); + numSwaps++; + } + + numValidConstraints++; + { + nCurrentBatch++; + if (nCurrentBatch == simdWidth) + { + nCurrentBatch = 0; + for (int i = 0; i < curBodyUsed; i++) + bodyUsed[curUsed[i] / 32] = 0; + curBodyUsed = 0; + } + } + } + } + m_gpuData->m_batchSizes.push_back(nCurrentBatch); + batchIdx++; + } + } + +#if defined(_DEBUG) + // debugPrintf( "nBatches: %d\n", batchIdx ); + for (int i = 0; i < numConstraints; i++) + { + b3Assert(cs[i].m_batchId != -1); + } +#endif + + if (maxSwaps < numSwaps) + { + maxSwaps = numSwaps; + //printf("maxSwaps = %d\n", maxSwaps); + } + + return batchIdx; +} + +/// b3PgsJacobiSolver Sequentially applies impulses +b3Scalar b3GpuPgsConstraintSolver::solveGroup(b3OpenCLArray* 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; cid < numConstraints; cid++) + { + int originalConstraintIndex = batchConstraints[cid].m_originalConstraintIndex; + int constraintRowOffset = m_gpuData->m_cpuConstraintRowOffsets[originalConstraintIndex]; + int numRows = m_gpuData->m_cpuConstraintInfo1[originalConstraintIndex]; + if (numRows) + { + // printf("cid=%d, breakingThreshold =%f\n",cid,breakingThreshold); + for (int i = 0; i < numRows; i++) + { + int rowIndex = constraintRowOffset + i; + int orgConstraintIndex = m_gpuData->m_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; i < m_tmpSolverBodyPool.size(); i++) + { + int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; + //printf("bodyIndex=%d\n",bodyIndex); + b3Assert(i == bodyIndex); + + b3RigidBodyData* body = &m_gpuData->m_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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h new file mode 100644 index 000000000..00bc544f0 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h @@ -0,0 +1,76 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp new file mode 100644 index 000000000..e3d235a4f --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp @@ -0,0 +1,1529 @@ + +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; iter < numIterations; iter++) + { + for (int cellId = 0; cellId < numBatches; cellId++) + { + int offset = 0; + for (int ii = 0; ii < B3_MAX_NUM_BATCHES; ii++) + { + int numInBatch = batchSizes->at(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; iter < numIterations; iter++) + { + for (int cellId = 0; cellId < numBatches; cellId++) + { + int offset = 0; + for (int ii = 0; ii < B3_MAX_NUM_BATCHES; ii++) + { + int numInBatch = batchSizes->at(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; iter < numIterations; iter++) + { + for (int ib = 0; ib < B3_SOLVER_N_BATCHES; ib++) + { +#ifdef DEBUG_ME + memset(debugInfo, 0, sizeof(SolverDebugInfo) * numWorkItems); + gpuDebugInfo.write(debugInfo, numWorkItems); +#endif + + cdata.z = ib; + + b3LauncherCL launcher(m_data->m_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; i < numWorkItems; i++) + { + if (debugInfo[i].m_valInt2 > 0) + { + 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; iter < numIterations; iter++) + { + for (int ib = 0; ib < B3_SOLVER_N_BATCHES; ib++) + { + cdata.z = ib; + + 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 //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_key < b.m_key); +} + +static bool b3ContactCmp(const b3Contact4& p, const b3Contact4& q) +{ + return ((p.m_bodyAPtrAndSignBit < q.m_bodyAPtrAndSignBit) || + ((p.m_bodyAPtrAndSignBit == q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit < q.m_bodyBPtrAndSignBit)) || + ((p.m_bodyAPtrAndSignBit == q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit == q.m_bodyBPtrAndSignBit) && p.m_childIndexA < q.m_childIndexA) || + ((p.m_bodyAPtrAndSignBit == q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit == q.m_bodyBPtrAndSignBit) && p.m_childIndexA < q.m_childIndexA) || + ((p.m_bodyAPtrAndSignBit == q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit == q.m_bodyBPtrAndSignBit) && p.m_childIndexA == q.m_childIndexA && p.m_childIndexB < q.m_childIndexB)); +} + +#define USE_SPATIAL_BATCHING 1 +#define USE_4x4_GRID 1 + +#ifndef USE_SPATIAL_BATCHING +static const int gridTable4x4[] = + { + 0, 1, 17, 16, + 1, 2, 18, 19, + 17, 18, 32, 3, + 16, 19, 3, 34}; +static 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 + +}; + +#endif + +void SetSortDataCPU(b3Contact4* gContact, b3RigidBodyData* gBodies, b3SortData* gSortDataOut, int nContacts, float scale, const b3Int4& nSplit, int staticIdx) +{ + for (int gIdx = 0; gIdx < nContacts; gIdx++) + { + 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); + +#if USE_SPATIAL_BATCHING + int idx = (aStatic) ? bIdx : aIdx; + b3Vector3 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 + bool bStatic = (bPtrAndSignBit < 0) || (bPtrAndSignBit == staticIdx); + +#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; + } + } +} + +void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const b3Config& config, int static0Index) +{ + B3_PROFILE("solveContacts"); + m_data->m_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; i < cpuConstraints.size(); i++) + { + cpuConstraints[i].m_batchIdx = i; + } + } + m_data->m_pBufContactOutGPU->copyFromHost(cpuConstraints); + if (m_debugOutput == 100) + { + for (int i = 0; i < cpuConstraints.size(); i++) + { + printf("c[%d].m_bodyA = %d, m_bodyB = %d, batchId = %d\n", i, cpuConstraints[i].m_bodyAPtrAndSignBit, cpuConstraints[i].m_bodyBPtrAndSignBit, cpuConstraints[i].m_batchIdx); + } + } + + m_debugOutput++; + } + } + + int nContactOut = m_data->m_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; i < nContacts; i++) + { + int srcIdx = sortDataHost[i].y; + outContacts[i] = inContacts[srcIdx]; + } + m_data->m_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; i < nContacts; i++) + { + m_data->m_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; i < B3_SOLVER_N_CELLS; i++) + { + int n = (nNativeHost)[i]; + int offset = (offsetsNativeHost)[i]; + if (n) + { + numNonzeroGrid++; + int simdWidth = numBodies + 1; //-1;//64;//-1;//32; + int numBatches = sortConstraintByBatch3(&cpuContacts[0] + offset, n, simdWidth, csCfg.m_staticIdx, numBodies, &m_data->m_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 < n; i++) + cs[i].getBatchIdx() = -1; +#endif + for (int i = 0; i < n; i++) + idxSrc[i] = i; + nIdxSrc = n; + + int batchIdx = 0; + + { + B3_PROFILE("cpu batch innerloop"); + while (nIdxSrc) + { + numIter++; + nIdxDst = 0; + int nCurrentBatch = 0; + + // clear flag + for (int i = 0; i < N_FLG / 32; i++) flg[i] = 0; + + for (int i = 0; i < nIdxSrc; i++) + { + int idx = idxSrc[i]; + + b3Assert(idx < n); + // check if it can go + int bodyAS = cs[idx].m_bodyAPtrAndSignBit; + int bodyBS = cs[idx].m_bodyBPtrAndSignBit; + + int bodyA = abs(bodyAS); + int bodyB = abs(bodyBS); + + int aIdx = bodyA & FLG_MASK; + int bIdx = bodyB & FLG_MASK; + + unsigned int aUnavailable = flg[aIdx / 32] & (1 << (aIdx & 31)); + unsigned int bUnavailable = flg[bIdx / 32] & (1 << (bIdx & 31)); + + bool aIsStatic = (bodyAS < 0) || bodyAS == staticIdx; + bool bIsStatic = (bodyBS < 0) || bodyBS == staticIdx; + + //use inv_mass! + aUnavailable = !aIsStatic ? aUnavailable : 0; // + bUnavailable = !bIsStatic ? bUnavailable : 0; + + if (aUnavailable == 0 && bUnavailable == 0) // ok + { + if (!aIsStatic) + flg[aIdx / 32] |= (1 << (aIdx & 31)); + if (!bIsStatic) + flg[bIdx / 32] |= (1 << (bIdx & 31)); + + cs[idx].getBatchIdx() = batchIdx; + sortData[idx].m_key = batchIdx; + sortData[idx].m_value = idx; + + { + nCurrentBatch++; + if (nCurrentBatch == simdWidth) + { + nCurrentBatch = 0; + for (int i = 0; i < N_FLG / 32; i++) flg[i] = 0; + } + } + } + else + { + idxDst[nIdxDst++] = idx; + } + } + b3Swap(idxSrc, idxDst); + b3Swap(nIdxSrc, nIdxDst); + batchIdx++; + } + } + { + B3_PROFILE("quickSort"); + sortData.quickSort(sortfnc); + } + + { + B3_PROFILE("reorder"); + // reorder + + memcpy(&old[0], cs, sizeof(b3Contact4) * n); + for (int i = 0; i < n; i++) + { + int idx = sortData[i].m_value; + cs[i] = old[idx]; + } + } + +#if defined(_DEBUG) + // debugPrintf( "nBatches: %d\n", batchIdx ); + for (int i = 0; i < n; i++) + { + b3Assert(cs[i].getBatchIdx() != -1); + } +#endif + return batchIdx; +} + +b3AlignedObjectArray 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; i < numConstraints; i++) + cs[i].getBatchIdx() = -1; +#endif + for (int i = 0; i < numConstraints; i++) + idxSrc[i] = i; + + int numValidConstraints = 0; + // int unprocessedConstraintIndex = 0; + + int batchIdx = 0; + + { + B3_PROFILE("cpu batch innerloop"); + + while (numValidConstraints < numConstraints) + { + numIter++; + int nCurrentBatch = 0; + // clear flag + for (int i = 0; i < curBodyUsed; i++) + bodyUsed2[i] = 0; + curBodyUsed = 0; + + for (int i = numValidConstraints; i < numConstraints; i++) + { + int idx = idxSrc[i]; + b3Assert(idx < numConstraints); + // check if it can go + int bodyAS = cs[idx].m_bodyAPtrAndSignBit; + int bodyBS = cs[idx].m_bodyBPtrAndSignBit; + int bodyA = abs(bodyAS); + int bodyB = abs(bodyBS); + bool aIsStatic = (bodyAS < 0) || bodyAS == staticIdx; + bool bIsStatic = (bodyBS < 0) || bodyBS == staticIdx; + int aUnavailable = 0; + int bUnavailable = 0; + if (!aIsStatic) + { + for (int j = 0; j < curBodyUsed; j++) + { + if (bodyA == bodyUsed2[j]) + { + aUnavailable = 1; + break; + } + } + } + if (!aUnavailable) + if (!bIsStatic) + { + for (int j = 0; j < curBodyUsed; j++) + { + if (bodyB == bodyUsed2[j]) + { + bUnavailable = 1; + break; + } + } + } + + if (aUnavailable == 0 && bUnavailable == 0) // ok + { + if (!aIsStatic) + { + bodyUsed2[curBodyUsed++] = bodyA; + } + if (!bIsStatic) + { + bodyUsed2[curBodyUsed++] = bodyB; + } + + cs[idx].getBatchIdx() = batchIdx; + m_data->m_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; i < curBodyUsed; i++) + bodyUsed2[i] = 0; + + curBodyUsed = 0; + } + } + } + } + + batchIdx++; + } + } + { + B3_PROFILE("quickSort"); + //m_data->m_sortData.quickSort(sortfnc); + } + + { + B3_PROFILE("reorder"); + // reorder + + memcpy(&m_data->m_old[0], cs, sizeof(b3Contact4) * numConstraints); + + for (int i = 0; i < numConstraints; i++) + { + b3Assert(m_data->m_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 < numConstraints; i++) + { + b3Assert(cs[i].getBatchIdx() != -1); + } +#endif + + return batchIdx; +} + +b3AlignedObjectArray 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 (maxNumConstraints < numConstraints) + { + maxNumConstraints = numConstraints; + //printf("maxNumConstraints = %d\n",maxNumConstraints ); + } + + int numUsedArray = numBodies / 32 + 1; + bodyUsed.resize(numUsedArray); + + for (int q = 0; q < numUsedArray; q++) + bodyUsed[q] = 0; + + int curBodyUsed = 0; + + int numIter = 0; + + m_data->m_sortData.resize(0); + m_data->m_idxBuffer.resize(0); + m_data->m_old.resize(0); + +#if defined(_DEBUG) + for (int i = 0; i < numConstraints; i++) + cs[i].getBatchIdx() = -1; +#endif + + int numValidConstraints = 0; + // int unprocessedConstraintIndex = 0; + + int batchIdx = 0; + + { + B3_PROFILE("cpu batch innerloop"); + + while (numValidConstraints < numConstraints) + { + numIter++; + int nCurrentBatch = 0; + batchSizes[batchIdx] = 0; + + // clear flag + for (int i = 0; i < curBodyUsed; i++) + bodyUsed[curUsed[i] / 32] = 0; + + curBodyUsed = 0; + + for (int i = numValidConstraints; i < numConstraints; i++) + { + int idx = i; + b3Assert(idx < numConstraints); + // check if it can go + int bodyAS = cs[idx].m_bodyAPtrAndSignBit; + int bodyBS = cs[idx].m_bodyBPtrAndSignBit; + int bodyA = abs(bodyAS); + int bodyB = abs(bodyBS); + bool aIsStatic = (bodyAS < 0) || bodyAS == staticIdx; + bool bIsStatic = (bodyBS < 0) || bodyBS == staticIdx; + int aUnavailable = 0; + int bUnavailable = 0; + if (!aIsStatic) + { + aUnavailable = bodyUsed[bodyA / 32] & (1 << (bodyA & 31)); + } + if (!aUnavailable) + if (!bIsStatic) + { + bUnavailable = bodyUsed[bodyB / 32] & (1 << (bodyB & 31)); + } + + if (aUnavailable == 0 && bUnavailable == 0) // ok + { + if (!aIsStatic) + { + bodyUsed[bodyA / 32] |= (1 << (bodyA & 31)); + curUsed[curBodyUsed++] = bodyA; + } + if (!bIsStatic) + { + bodyUsed[bodyB / 32] |= (1 << (bodyB & 31)); + curUsed[curBodyUsed++] = bodyB; + } + + cs[idx].getBatchIdx() = batchIdx; + + if (i != numValidConstraints) + { + b3Swap(cs[i], cs[numValidConstraints]); + numSwaps++; + } + + numValidConstraints++; + { + nCurrentBatch++; + if (nCurrentBatch == simdWidth) + { + batchSizes[batchIdx] += simdWidth; + nCurrentBatch = 0; + for (int i = 0; i < curBodyUsed; i++) + bodyUsed[curUsed[i] / 32] = 0; + curBodyUsed = 0; + } + } + } + } + + if (batchIdx >= 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 < numConstraints; i++) + { + b3Assert(cs[i].getBatchIdx() != -1); + } +#endif + + batchSizes[batchIdx] = 0; + + if (maxSwaps < numSwaps) + { + maxSwaps = numSwaps; + //printf("maxSwaps = %d\n", maxSwaps); + } + + return batchIdx; +} diff --git a/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h new file mode 100644 index 000000000..6ab7502af --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h @@ -0,0 +1,37 @@ + +#ifndef B3_GPU_BATCHING_PGS_SOLVER_H +#define B3_GPU_BATCHING_PGS_SOLVER_H + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "b3GpuConstraint4.h" + +class b3GpuPgsContactSolver +{ +protected: + int m_debugOutput; + + struct b3GpuBatchingPgsSolverInternalData* m_data; + + void batchContacts(b3OpenCLArray* 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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp new file mode 100644 index 000000000..fef33ad1c --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -0,0 +1,677 @@ +/* +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; i < m_data->m_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; i < m_data->m_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; i < hostPairs.size(); i++) + { + hostPairs[i].z = 0xffffffff; + } + + gpuPairs.copyFromHost(hostPairs); + } + } + + m_data->m_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; i < numContacts; i++) + { + totalPoints += contacts->getNPoints(); + } + 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; nodeID < numBodies; nodeID++) + { + integrateSingleTransform(bodies, nodeID, timeStep, angularDamp, m_data->m_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; i < numBodies; i++) + { + b3ComputeWorldAabb(i, m_data->m_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; i < numBodies; i++) + { + b3ComputeWorldAabb(i, m_data->m_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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h new file mode 100644 index 000000000..0e5c6fec1 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h @@ -0,0 +1,70 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h new file mode 100644 index 000000000..e0a26fda1 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h @@ -0,0 +1,68 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h new file mode 100644 index 000000000..db815d9b3 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h @@ -0,0 +1,210 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h new file mode 100644 index 000000000..7d9eea243 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h @@ -0,0 +1,73 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.cpp b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.cpp new file mode 100644 index 000000000..ccf67da1a --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.cpp @@ -0,0 +1,1128 @@ +/* +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; ii < B3_MAX_NUM_BATCHES; ii++) + { + int numInBatch = m_batchSizes->at(m_cellIndex * B3_MAX_NUM_BATCHES + ii); + if (!numInBatch) + break; + + for (int jj = 0; jj < numInBatch; jj++) + { + int i = m_start + offset + jj; + int batchId = m_constraints[i].m_batchIdx; + b3Assert(batchId == ii); + float frictionCoeff = m_constraints[i].getFrictionCoeff(); + int aIdx = (int)m_constraints[i].m_bodyA; + int bIdx = (int)m_constraints[i].m_bodyB; + // int localBatch = m_constraints[i].m_batchIdx; + b3RigidBodyData& bodyA = m_bodies[aIdx]; + b3RigidBodyData& bodyB = m_bodies[bIdx]; + + if (!m_solveFriction) + { + float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX}; + float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f}; + + solveContact(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 < m_nIterations; iter++) + { + for (int cellBatch = 0; cellBatch < B3_SOLVER_N_BATCHES; cellBatch++) + { + int nSplitX = B3_SOLVER_N_SPLIT_X; + int nSplitY = B3_SOLVER_N_SPLIT_Y; + int numWorkgroups = B3_SOLVER_N_CELLS / B3_SOLVER_N_BATCHES; + //printf("cell Batch %d\n",cellBatch); + b3AlignedObjectArray usedBodies[B3_SOLVER_N_CELLS]; + for (int i = 0; i < B3_SOLVER_N_CELLS; i++) + { + usedBodies[i].resize(0); + } + + //for (int wgIdx=numWorkgroups-1;wgIdx>=0;wgIdx--) + for (int wgIdx = 0; wgIdx < numWorkgroups; wgIdx++) + { + int zIdx = (wgIdx / ((nSplitX * nSplitY) / 4)) * 2 + ((cellBatch & 4) >> 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 < m_nIterations; iter++) + { + for (int cellBatch = 0; cellBatch < B3_SOLVER_N_BATCHES; cellBatch++) + { + 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 wgIdx = 0; wgIdx < numWorkgroups; wgIdx++) + { + int zIdx = (wgIdx / ((nSplitX * nSplitY) / 4)) * 2 + ((cellBatch & 4) >> 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; iter < m_nIterations; iter++) + { + SolveTask task(bodyNative, shapeNative, constraintNative, 0, n, maxNumBatches, 0, 0, 0, 0); + task.m_solveFriction = false; + task.run(0); + } + + for (int iter = 0; iter < m_nIterations; iter++) + { + SolveTask task(bodyNative, shapeNative, constraintNative, 0, n, maxNumBatches, 0, 0, 0, 0); + task.m_solveFriction = true; + task.run(0); + } + } + + bodyBuf->copyFromHost(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 < numWorkgroups; wgIdx++) + { + printf("wgIdx = %d ", wgIdx); + + int zIdx = (wgIdx / ((nSplitX * nSplitY)) / 2) * 2 + ((cellBatch & 4) >> 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 < end; c++) + { + b3GpuConstraint4& constraint = cpuConstraints[c]; + //printf("constraint (%d,%d)\n", constraint.m_bodyA,constraint.m_bodyB); + if (usedBodies.findLinearSearch(constraint.m_bodyA) < usedBodies.size()) + { + printf("error?\n"); + } + if (usedBodies.findLinearSearch(constraint.m_bodyB) < usedBodies.size()) + { + printf("error?\n"); + } + } + + for (int c = start; c < end; c++) + { + b3GpuConstraint4& constraint = cpuConstraints[c]; + usedBodies.push_back(constraint.m_bodyA); + usedBodies.push_back(constraint.m_bodyB); + } + } +} + +static bool verify = false; + +void b3Solver::solveContactConstraint(const b3OpenCLArray* 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; iter < m_nIterations; iter++) + { + for (int ib = 0; ib < B3_SOLVER_N_BATCHES; ib++) + { + if (verify) + { + checkConstraintBatch(bodyBuf, shapeBuf, constraint, m_numConstraints, m_offsets, ib); + } + +#ifdef DEBUG_ME + memset(debugInfo, 0, sizeof(SolverDebugInfo) * numWorkItems); + gpuDebugInfo.write(debugInfo, numWorkItems); +#endif + + cdata.z = ib; + + b3LauncherCL launcher(m_queue, m_solveContactKernel, "m_solveContactKernel"); +#if 1 + + b3BufferInfoCL bInfo[] = { + + b3BufferInfoCL(bodyBuf->getBufferCL()), + 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; i < numWorkItems; i++) + { + if (debugInfo[i].m_valInt2 > 0) + { + 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; iter < m_nIterations; iter++) + { + for (int ib = 0; ib < B3_SOLVER_N_BATCHES; ib++) + { + cdata.z = ib; + + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL(bodyBuf->getBufferCL()), + 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; gIdx < nContacts; gIdx++) + { + int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit); + int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit); + + b3Float4 posA = gBodies[aIdx].m_pos; + b3Float4 linVelA = gBodies[aIdx].m_linVel; + b3Float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + b3Mat3x3 invInertiaA = gShapes[aIdx].m_initInvInertia; + + b3Float4 posB = gBodies[bIdx].m_pos; + b3Float4 linVelB = gBodies[bIdx].m_linVel; + b3Float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + b3Mat3x3 invInertiaB = gShapes[bIdx].m_initInvInertia; + + b3ContactConstraint4_t cs; + + setConstraint4(posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB, + &gContact[gIdx], cdata.m_dt, cdata.m_positionDrift, cdata.m_positionConstraintCoeff, + &cs); + + cs.m_batchIdx = gContact[gIdx].m_batchIdx; + + gConstraintOut[gIdx] = (b3GpuConstraint4&)cs; + } + + contactCOut->copyFromHost(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; i < numWorkItems; i++) + { + if (debugInfo[i].m_valInt1 > 0) + { + 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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.h new file mode 100644 index 000000000..ee63531d7 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.h @@ -0,0 +1,110 @@ +/* +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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl new file mode 100644 index 000000000..3b891b863 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl new file mode 100644 index 000000000..7f5dabe27 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h new file mode 100644 index 000000000..c94b55851 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h @@ -0,0 +1,720 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl new file mode 100644 index 000000000..5c4d62e4e --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl new file mode 100644 index 000000000..3dc48d435 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h new file mode 100644 index 000000000..1e6e3579b --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h @@ -0,0 +1,600 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl new file mode 100644 index 000000000..a21a08c3b --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h new file mode 100644 index 000000000..f4d98d994 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h @@ -0,0 +1,908 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl new file mode 100644 index 000000000..ba8ba735d --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h new file mode 100644 index 000000000..bb949b202 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h @@ -0,0 +1,482 @@ +//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/Engine/lib/bullet/src/Bullet3OpenCL/premake4.lua b/Engine/lib/bullet/src/Bullet3OpenCL/premake4.lua new file mode 100644 index 000000000..ee35fdb52 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/CMakeLists.txt b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/CMakeLists.txt new file mode 100644 index 000000000..125576634 --- /dev/null +++ b/Engine/lib/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/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h new file mode 100644 index 000000000..eaa27dfe8 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h @@ -0,0 +1,987 @@ +/* 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; +}; + +} // namespace Bullet3SerializeBullet2 +#endif //__BULLET2_H__ \ No newline at end of file diff --git a/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp new file mode 100644 index 000000000..d2a716367 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp @@ -0,0 +1,400 @@ +/* +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/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.h b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.h new file mode 100644 index 000000000..ede1d378a --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.h @@ -0,0 +1,74 @@ +/* +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); +}; +}; // namespace bParse + +#endif //B3_BULLET_FILE_H diff --git a/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.cpp b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.cpp new file mode 100644 index 000000000..ff75ff8cc --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.cpp @@ -0,0 +1,69 @@ +/* +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/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.h b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.h new file mode 100644 index 000000000..c9d0f37d9 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.h @@ -0,0 +1,84 @@ +/* +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)); +} // namespace bParse + +#endif //__BCHUNK_H__ diff --git a/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Common.h b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Common.h new file mode 100644 index 000000000..5884fad4d --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Common.h @@ -0,0 +1,40 @@ +/* +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; +} // namespace bParse + +#endif //__BCOMMON_H__ diff --git a/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp new file mode 100644 index 000000000..09c8f2385 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp @@ -0,0 +1,616 @@ +/* +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; j < eleLen; j++, curStruct += 2) + { + if (curStruct[0] == type) + { + //char *name = m_Names[curStruct[1]].m_name; + //if (name[0] != '*') + if (m_Names[curStruct[1]].m_isPointer) + { + mCMPFlags[i] = FDF_STRUCT_NEQU; + initRecurseCmpFlags(i); + } + } + } + } + } +} + +// ----------------------------------------------------- // +void bDNA::initCmpFlags(bDNA *memDNA) +{ + // compare the file to memory + // this ptr should be the file data + + assert(!(m_Names.size() == 0)); // && "SDNA empty!"); + mCMPFlags.resize(mStructs.size(), FDF_NONE); + + int i; + for (i = 0; i < (int)mStructs.size(); i++) + { + short *oldStruct = mStructs[i]; + + int oldLookup = getReverseType(oldStruct[0]); + if (oldLookup == -1) + { + mCMPFlags[i] = FDF_NONE; + continue; + } + //char* typeName = mTypes[oldStruct[0]]; + +//#define SLOW_FORWARD_COMPATIBLE 1 +#ifdef SLOW_FORWARD_COMPATIBLE + char *typeName = mTypes[oldLookup]; + int newLookup = memDNA->getReverseType(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; j < elementLength; j++, curStruct += 2, oldStruct += 2) + { + // type the same + //const char* typeFileDNA = mTypes[oldStruct[0]]; + //const char* typeMemDNA = mTypes[curStruct[0]]; + if (strcmp(mTypes[oldStruct[0]], memDNA->mTypes[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 < dataLen; i++) + { + bNameInfo info; + info.m_name = cp; + info.m_isPointer = (info.m_name[0] == '*') || (info.m_name[1] == '*'); + name_is_array(info.m_name, &info.m_dim0, &info.m_dim1); + m_Names.push_back(info); + while (*cp) cp++; + cp++; + } + + cp = b3AlignPointer(cp, 4); + + /* + TYPE (4 bytes) + 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 < dataLen; i++) + { + mTypes.push_back(cp); + while (*cp) cp++; + cp++; + } + + cp = b3AlignPointer(cp, 4); + + /* + TLEN (4 bytes) + (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 < dataLen; i++, shtPtr++) + { + if (swap) + shtPtr[0] = ChunkUtils::swapShort(shtPtr[0]); + mTlens.push_back(shtPtr[0]); + } + + if (dataLen & 1) shtPtr++; + + /* + STRC (4 bytes) + 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; i < dataLen; i++) + { + mStructs.push_back(shtPtr); + if (swap) + { + shtPtr[0] = ChunkUtils::swapShort(shtPtr[0]); + shtPtr[1] = ChunkUtils::swapShort(shtPtr[1]); + + int len = shtPtr[1]; + shtPtr += 2; + + for (int a = 0; a < len; a++, shtPtr += 2) + { + shtPtr[0] = ChunkUtils::swapShort(shtPtr[0]); + shtPtr[1] = ChunkUtils::swapShort(shtPtr[1]); + } + } + else + shtPtr += (2 * shtPtr[1]) + 2; + } + + // build reverse lookups + for (i = 0; i < (int)mStructs.size(); i++) + { + short *strc = mStructs.at(i); + if (!mPtrLen && strcmp(mTypes[strc[0]], "ListBase") == 0) + { + mPtrLen = mTlens[strc[0]] / 2; + } + + mStructReverse.insert(strc[0], i); + mTypeLookup.insert(b3HashString(mTypes[strc[0]]), i); + } +} + +// ----------------------------------------------------- // +int bDNA::getArraySize(char *string) +{ + int ret = 1; + int len = strlen(string); + + char *next = 0; + for (int i = 0; i < len; i++) + { + char c = string[i]; + + if (c == '[') + next = &string[i + 1]; + else if (c == ']') + if (next) + ret *= atoi(next); + } + + // print (string << ' ' << ret); + return ret; +} + +void bDNA::dumpTypeDefinitions() +{ + int i; + + int numTypes = mTypes.size(); + + for (i = 0; i < numTypes; i++) + { + } + + for (i = 0; i < (int)mStructs.size(); i++) + { + int totalBytes = 0; + short *oldStruct = mStructs[i]; + + int oldLookup = getReverseType(oldStruct[0]); + if (oldLookup == -1) + { + mCMPFlags[i] = FDF_NONE; + continue; + } + + short *newStruct = mStructs[oldLookup]; + char *typeName = mTypes[newStruct[0]]; + printf("%3d: %s ", i, typeName); + + //char *name = mNames[oldStruct[1]]; + int len = oldStruct[1]; + printf(" (%d fields) ", len); + oldStruct += 2; + + printf("{"); + int j; + for (j = 0; j < len; ++j, oldStruct += 2) + { + const char *name = m_Names[oldStruct[1]].m_name; + printf("%s %s", mTypes[oldStruct[0]], name); + int elemNumBytes = 0; + int arrayDimensions = getArraySizeNew(oldStruct[1]); + + if (m_Names[oldStruct[1]].m_isPointer) + { + elemNumBytes = VOID_IS_8 ? 8 : 4; + } + else + { + elemNumBytes = getLength(oldStruct[0]); + } + printf(" /* %d bytes */", elemNumBytes * arrayDimensions); + + if (j == len - 1) + { + printf(";}"); + } + else + { + printf("; "); + } + totalBytes += elemNumBytes * arrayDimensions; + } + printf("\ntotalBytes=%d\n\n", totalBytes); + } + +#if 0 + /* dump out display of types and their sizes */ + 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/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.h b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.h new file mode 100644 index 000000000..ca6004d96 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.h @@ -0,0 +1,101 @@ +/* +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; +}; +} // namespace bParse + +#endif //__BDNA_H__ diff --git a/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Defines.h b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Defines.h new file mode 100644 index 000000000..0524c94db --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Defines.h @@ -0,0 +1,149 @@ +/* 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/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp new file mode 100644 index 000000000..145de62db --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp @@ -0,0 +1,1653 @@ +/* +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 < MAX_STRLEN); + slen = b3Min(slen, MAX_STRLEN); + for (int i = 0; i < slen; i++) + { + if (memName[i] == ']' || memName[i] == '[') + { + buffer[i] = 0; //'_'; + } + else + { + buffer[i] = memName[i]; + } + } + buffer[slen] = 0; + return buffer; +} + +// ----------------------------------------------------- // +bFile::bFile(const char *filename, const char headerString[7]) + : mOwnsBuffer(true), + mFileBuffer(0), + mFileLen(0), + mVersion(0), + mDataStart(0), + mFileDNA(0), + mMemoryDNA(0), + mFlags(FD_INVALID) +{ + for (int i = 0; i < 7; i++) + { + m_headerString[i] = headerString[i]; + } + + FILE *fp = fopen(filename, "rb"); + if (fp) + { + fseek(fp, 0L, SEEK_END); + mFileLen = ftell(fp); + fseek(fp, 0L, SEEK_SET); + + mFileBuffer = (char *)malloc(mFileLen + 1); + int bytesRead; + bytesRead = fread(mFileBuffer, mFileLen, 1, fp); + + fclose(fp); + + // + parseHeader(); + } +} + +// ----------------------------------------------------- // +bFile::bFile(char *memoryBuffer, int len, const char headerString[7]) + : mOwnsBuffer(false), + mFileBuffer(0), + mFileLen(0), + mVersion(0), + mDataStart(0), + mFileDNA(0), + mMemoryDNA(0), + mFlags(FD_INVALID) +{ + for (int i = 0; i < 7; i++) + { + m_headerString[i] = headerString[i]; + } + mFileBuffer = memoryBuffer; + mFileLen = len; + + parseHeader(); +} + +// ----------------------------------------------------- // +bFile::~bFile() +{ + if (mOwnsBuffer && mFileBuffer) + { + free(mFileBuffer); + mFileBuffer = 0; + } + + delete mMemoryDNA; + delete mFileDNA; +} + +// ----------------------------------------------------- // +void bFile::parseHeader() +{ + if (!mFileLen || !mFileBuffer) + return; + + char *blenderBuf = mFileBuffer; + char header[B3_SIZEOFBLENDERHEADER + 1]; + memcpy(header, blenderBuf, B3_SIZEOFBLENDERHEADER); + header[B3_SIZEOFBLENDERHEADER] = '\0'; + + if (strncmp(header, m_headerString, 6) != 0) + { + memcpy(header, m_headerString, B3_SIZEOFBLENDERHEADER); + return; + } + + if (header[6] == 'd') + { + mFlags |= FD_DOUBLE_PRECISION; + } + + char *ver = header + 9; + mVersion = atoi(ver); + if (mVersion <= 241) + { + //printf("Warning, %d not fully tested : <= 242\n", mVersion); + } + + int littleEndian = 1; + littleEndian = ((char *)&littleEndian)[0]; + + // swap ptr sizes... + if (header[7] == '-') + { + mFlags |= FD_FILE_64; + if (!VOID_IS_8) + mFlags |= FD_BITS_VARIES; + } + else if (VOID_IS_8) + mFlags |= FD_BITS_VARIES; + + // swap endian... + if (header[8] == 'V') + { + if (littleEndian == 1) + mFlags |= FD_ENDIAN_SWAP; + } + else if (littleEndian == 0) + mFlags |= FD_ENDIAN_SWAP; + + mFlags |= FD_OK; +} + +// ----------------------------------------------------- // +bool bFile::ok() +{ + return (mFlags & FD_OK) != 0; +} + +// ----------------------------------------------------- // +void bFile::parseInternal(int verboseMode, char *memDna, int memDnaLength) +{ + if ((mFlags & FD_OK) == 0) + return; + + char *blenderData = mFileBuffer; + bChunkInd dna; + dna.oldPtr = 0; + + char *tempBuffer = blenderData; + for (int i = 0; i < mFileLen; i++) + { + // looking for the data's starting position + // and the start of SDNA decls + + if (!mDataStart && strncmp(tempBuffer, "REND", 4) == 0) + mDataStart = i; + + if (strncmp(tempBuffer, "DNA1", 4) == 0) + { + // read the DNA1 block and extract SDNA + if (getNextBlock(&dna, tempBuffer, mFlags) > 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; i < mFileDNA->getNumNames(); 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; i < dataChunk.nr; i++) + { + swapStruct(dataChunk.dna_nr, data, ignoreEndianFlag); + data += len; + } +} + +void bFile::swapLen(char *dataPtr) +{ + const bool VOID_IS_8 = ((sizeof(void *) == 8)); + if (VOID_IS_8) + { + if (mFlags & FD_BITS_VARIES) + { + 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); + } + 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 < dataLen; i++) + { + while (*cp) cp++; + cp++; + } + + cp = b3AlignPointer(cp, 4); + + /* + TYPE (4 bytes) + 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 < dataLen; i++) + { + while (*cp) cp++; + cp++; + } + + cp = b3AlignPointer(cp, 4); + + /* + TLEN (4 bytes) + (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 < dataLen; i++, shtPtr++) + { + //??????if (swap) + shtPtr[0] = ChunkUtils::swapShort(shtPtr[0]); + } + + if (dataLen & 1) + shtPtr++; + + /* + STRC (4 bytes) + 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 < dataLen; i++) + { + //if (swap) + { + int len = shtPtr[1]; + + shtPtr[0] = ChunkUtils::swapShort(shtPtr[0]); + shtPtr[1] = ChunkUtils::swapShort(shtPtr[1]); + + shtPtr += 2; + + for (int a = 0; a < len; a++, shtPtr += 2) + { + shtPtr[0] = ChunkUtils::swapShort(shtPtr[0]); + shtPtr[1] = ChunkUtils::swapShort(shtPtr[1]); + } + } + // else + // shtPtr+= (2*shtPtr[1])+2; + } +} + +void bFile::writeFile(const char *fileName) +{ + FILE *f = fopen(fileName, "wb"); + fwrite(mFileBuffer, 1, mFileLen, f); + fclose(f); +} + +void bFile::preSwap() +{ + //const bool brokenDNA = (mFlags&FD_BROKEN_DNA)!=0; + //FD_ENDIAN_SWAP + //byte 8 determines the endianness of the file, little (v) versus big (V) + int littleEndian = 1; + littleEndian = ((char *)&littleEndian)[0]; + + if (mFileBuffer[8] == 'V') + { + mFileBuffer[8] = 'v'; + } + else + { + mFileBuffer[8] = 'V'; + } + + mDataStart = 12; + + char *dataPtr = mFileBuffer + mDataStart; + + bChunkInd dataChunk; + dataChunk.code = 0; + bool ignoreEndianFlag = true; + + //we always want to swap here + + int seek = getNextBlock(&dataChunk, dataPtr, mFlags); + //dataPtr += ChunkUtils::getOffset(mFlags); + char *dataPtrHead = 0; + + while (1) + { + // one behind + if (dataChunk.code == B3_SDNA || dataChunk.code == B3_DNA1 || dataChunk.code == B3_TYPE || dataChunk.code == B3_TLEN || dataChunk.code == B3_STRC) + { + swapDNA(dataPtr); + break; + } + else + { + //if (dataChunk.code == DNA1) break; + dataPtrHead = dataPtr + ChunkUtils::getOffset(mFlags); + + swapLen(dataPtr); + if (dataChunk.dna_nr >= 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; i < dataChunk.nr; i++) + { + dest[i] = src[i]; + if (mFlags & FD_ENDIAN_SWAP) + { + B3_SWITCH_SHORT(dest[i]); + } + } + addDataBlock(dataAlloc); + return dataAlloc; + } + } + + ///don't try to convert Link block data, just memcpy it. Other data can be converted. + if (strcmp("Link", oldType) != 0) + { + 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]); + 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; block < dataChunk.nr; block++) + { + bool fixupPointers = true; + parseStruct(cur, old, dataChunk.dna_nr, reverseOld, fixupPointers); + mLibPointers.insert(old, (bStructHandle *)cur); + + cur += curLen; + old += oldLen; + } + return dataAlloc; + } + } + else + { + //printf("Link found\n"); + } + } + else + { +//#define DEBUG_EQUAL_STRUCTS +#ifdef DEBUG_EQUAL_STRUCTS + short *oldStruct; + char *oldType; + oldStruct = mFileDNA->getStruct(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; ele < elementLength; ele++, memoryStruct += 2) + { + memType = mMemoryDNA->getType(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; i < arrayLen; i++) + { + parseStruct(tmpCpc, tmpCpo, old_nr, new_nr, fixupPointers); + tmpCpc += size / arrayLen; + tmpCpo += fpLen / arrayLen; + } + } + cpc += size; + cpo += fpLen; + } + else + cpc += size; + } + else + { + getMatchingFileDNA(fileStruct, memName, memType, cpc, dtPtr, fixupPointers); + cpc += size; + } + } +} + +// ----------------------------------------------------- // +static void getElement(int arrayLen, const char *cur, const char *old, char *oldPtr, char *curData) +{ +#define b3GetEle(value, current, type, cast, size, ptr) \ + if (strcmp(current, type) == 0) \ + { \ + value = (*(cast *)ptr); \ + ptr += size; \ + } + +#define b3SetEle(value, current, type, cast, size, ptr) \ + if (strcmp(current, type) == 0) \ + { \ + (*(cast *)ptr) = (cast)value; \ + ptr += size; \ + } + double value = 0.0; + + for (int i = 0; i < arrayLen; i++) + { + b3GetEle(value, old, "char", char, sizeof(char), oldPtr); + b3SetEle(value, cur, "char", char, sizeof(char), curData); + b3GetEle(value, old, "short", short, sizeof(short), oldPtr); + b3SetEle(value, cur, "short", short, sizeof(short), curData); + b3GetEle(value, old, "ushort", unsigned short, sizeof(unsigned short), oldPtr); + b3SetEle(value, cur, "ushort", unsigned short, sizeof(unsigned short), curData); + b3GetEle(value, old, "int", int, sizeof(int), oldPtr); + b3SetEle(value, cur, "int", int, sizeof(int), curData); + b3GetEle(value, old, "long", int, sizeof(int), oldPtr); + b3SetEle(value, cur, "long", int, sizeof(int), curData); + b3GetEle(value, old, "float", float, sizeof(float), oldPtr); + b3SetEle(value, cur, "float", float, sizeof(float), curData); + b3GetEle(value, old, "double", double, sizeof(double), oldPtr); + b3SetEle(value, cur, "double", double, sizeof(double), curData); + } +} + +// ----------------------------------------------------- // +void bFile::swapData(char *data, short type, int arraySize, bool ignoreEndianFlag) +{ + if (ignoreEndianFlag || (mFlags & FD_ENDIAN_SWAP)) + { + if (type == 2 || type == 3) + { + short *sp = (short *)data; + for (int i = 0; i < arraySize; i++) + { + sp[0] = ChunkUtils::swapShort(sp[0]); + sp++; + } + } + if (type > 3 && type < 8) + { + char c; + char *cp = data; + for (int i = 0; i < arraySize; i++) + { + c = cp[0]; + cp[0] = cp[3]; + cp[3] = c; + c = cp[1]; + cp[1] = cp[2]; + cp[2] = c; + cp += 4; + } + } + } +} + +void bFile::safeSwapPtr(char *dst, const char *src) +{ + int ptrFile = mFileDNA->getPointerSize(); + 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; i < len; i++, dna_addr += 2) + { + const char *type = mFileDNA->getType(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; a < arrayLen; a++) + { + safeSwapPtr(cpc, cpo); + m_pointerFixupArray.push_back(cpc); + cpc += ptrMem; + cpo += ptrFile; + } + } + else + { + if (name[1] == '*') + m_pointerPtrFixupArray.push_back(strcData); + else + m_pointerFixupArray.push_back(strcData); + } + } + else + { + // printf("skipped %s %s : %x\n",type.c_str(),name.c_str(),strcData); + } + } + + else if (strcmp(type, lookupType) == 0) + memcpy(strcData, data, eleLen); + else + getElement(arrayLen, lookupType, type, data, strcData); + + // -- + return; + } + data += eleLen; + } +} + +// ----------------------------------------------------- // +char *bFile::getFileElement(short *firstStruct, char *lookupName, char *lookupType, char *data, short **foundPos) +{ + short *old = firstStruct; //mFileDNA->getStruct(old_nr); + int elementLength = old[1]; + old += 2; + + for (int i = 0; i < elementLength; i++, old += 2) + { + char *type = mFileDNA->getType(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; i < elementLen; i++, strc += 2) + { + char *type = mFileDNA->getType(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; i < arrayLen; i++) + { + swapStruct(old_nr, tmpBuf, ignoreEndianFlag); + tmpBuf += size / arrayLen; + } + } + } + else + { + //int arrayLenOld = mFileDNA->getArraySize(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; i < m_pointerPtrFixupArray.size(); i++) + { + char *cur = m_pointerPtrFixupArray.at(i); + void **ptrptr = (void **)cur; + + bChunkInd *block = m_chunkPtrPtrMap.find(*ptrptr); + if (block) + { + int ptrMem = mMemoryDNA->getPointerSize(); + 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; block < dataChunk.nr; block++) + { + resolvePointersStructRecursive(cur, dataChunk.dna_nr, verboseMode, 1); + cur += oldLen; + } +} + +int bFile::resolvePointersStructRecursive(char *strcPtr, int dna_nr, int verboseMode, int recursion) +{ + bParse::bDNA *fileDna = mFileDNA ? mFileDNA : mMemoryDNA; + + char *memType; + char *memName; + short firstStructType = fileDna->getStruct(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; ele < elementLength; ele++, oldStruct += 2) + { + memType = fileDna->getType(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 < arrayLen; a++) + { + if (verboseMode & FD_VERBOSE_EXPORT_XML) + { + for (int i = 0; i < recursion; i++) + { + printf(" "); + } + //skip the * + printf("<%s type=\"pointer\"> ", &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 < recursion; i++) + { + printf(" "); + } + printf("<%s type=\"pointer\"> ", &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; i < recursion; i++) + { + printf(" "); + } + + if (arrayLen > 1) + { + printf("<%s type=\"%s\" count=%d>\n", cleanName, memType, arrayLen); + } + else + { + printf("<%s type=\"%s\">\n", cleanName, memType); + } + } + + for (int i = 0; i < arrayLen; i++) + { + byteOffset += resolvePointersStructRecursive(elemPtr + byteOffset, revType, verboseMode, recursion + 1); + } + if (verboseMode & FD_VERBOSE_EXPORT_XML) + { + for (int i = 0; i < recursion; i++) + { + printf(" "); + } + printf("\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 < recursion; i++) + printf(" "); + if (arrayLen == 1) + printf("<%s type=\"%s\">", cleanName, memType); + else + printf("<%s type=\"%s\" count=%d>", cleanName, memType, arrayLen); + for (i = 0; i < arrayLen; i++) + printf(" %d ", dbPtr[i]); + printf("\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 < recursion; i++) + printf(" "); + char cleanName[MAX_STRLEN]; + getCleanName(memName, cleanName); + + if (arrayLen == 1) + { + printf("<%s type=\"%s\">", memName, memType); + } + else + { + printf("<%s type=\"%s\" count=%d>", cleanName, memType, arrayLen); + } + for (i = 0; i < arrayLen; i++) + printf(" %f ", dbPtr[i]); + printf("\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; i < m_chunks.size(); i++) + { + const bChunkInd &dataChunk = m_chunks.at(i); + + if (!mFileDNA || fileDna->flagEqual(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; i < m_chunks.size(); i++) + { + bChunkInd &dataChunk = m_chunks[i]; + dataChunk.oldPtr = findLibPointer(dataChunk.oldPtr); + } +} +void bFile::dumpChunks(bParse::bDNA *dna) +{ + int i; + + for (i = 0; i < m_chunks.size(); i++) + { + bChunkInd &dataChunk = m_chunks[i]; + char *codeptr = (char *)&dataChunk.code; + char codestr[5] = {codeptr[0], codeptr[1], codeptr[2], codeptr[3], 0}; + + short *newStruct = dna->getStruct(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; i < m_chunks.size(); i++) + { + bChunkInd &dataChunk = m_chunks.at(i); + + // Ouch! need to rebuild the struct + short *oldStruct, *curStruct; + char *oldType, *newType; + int oldLen, curLen, reverseOld; + + oldStruct = fileDna->getStruct(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/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.h b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.h new file mode 100644 index 000000000..bda229cfb --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.h @@ -0,0 +1,158 @@ +/* +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); +}; +} // namespace bParse + +#endif //__BFILE_H__ diff --git a/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.cpp b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.cpp new file mode 100644 index 000000000..ea4a8e200 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.cpp @@ -0,0 +1,18062 @@ +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/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.h b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.h new file mode 100644 index 000000000..d9e153e23 --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.h @@ -0,0 +1,601 @@ +/* +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 < dataLen; i++) + { + while (*cp) cp++; + cp++; + } + cp = b3AlignPointer(cp, 4); + + /* + TYPE (4 bytes) + 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 < dataLen; i++) + { + mTypes.push_back(cp); + while (*cp) cp++; + cp++; + } + + cp = b3AlignPointer(cp, 4); + + /* + TLEN (4 bytes) + (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 < dataLen; i++, shtPtr++) + { + if (!littleEndian) + shtPtr[0] = b3SwapEndian(shtPtr[0]); + mTlens.push_back(shtPtr[0]); + } + + if (dataLen & 1) shtPtr++; + + /* + STRC (4 bytes) + 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; i < dataLen; i++) + { + mStructs.push_back(shtPtr); + + if (!littleEndian) + { + shtPtr[0] = b3SwapEndian(shtPtr[0]); + shtPtr[1] = b3SwapEndian(shtPtr[1]); + + int len = shtPtr[1]; + shtPtr += 2; + + for (int a = 0; a < len; a++, shtPtr += 2) + { + shtPtr[0] = b3SwapEndian(shtPtr[0]); + shtPtr[1] = b3SwapEndian(shtPtr[1]); + } + } + else + { + shtPtr += (2 * shtPtr[1]) + 2; + } + } + + // build reverse lookups + for (i = 0; i < (int)mStructs.size(); i++) + { + short* strc = mStructs.at(i); + mStructReverse.insert(strc[0], i); + mTypeLookup.insert(b3HashString(mTypes[strc[0]]), i); + } + } + +public: + b3DefaultSerializer(int totalSize = 0) + : m_totalSize(totalSize), + m_currentSize(0), + m_dna(0), + m_dnaLength(0), + m_serializationFlags(0) + { + m_buffer = m_totalSize ? (unsigned char*)b3AlignedAlloc(totalSize, 16) : 0; + + const bool VOID_IS_8 = ((sizeof(void*) == 8)); + +#ifdef B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + if (VOID_IS_8) + { +#if _WIN64 + initDNA((const char*)b3s_bulletDNAstr64, b3s_bulletDNAlen64); +#else + b3Assert(0); +#endif + } + else + { +#ifndef _WIN64 + initDNA((const char*)b3s_bulletDNAstr, b3s_bulletDNAlen); +#else + b3Assert(0); +#endif + } + +#else //B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + if (VOID_IS_8) + { + initDNA((const char*)b3s_bulletDNAstr64, b3s_bulletDNAlen64); + } + else + { + initDNA((const char*)b3s_bulletDNAstr, b3s_bulletDNAlen); + } +#endif //B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + } + + virtual ~b3DefaultSerializer() + { + if (m_buffer) + b3AlignedFree(m_buffer); + if (m_dna) + b3AlignedFree(m_dna); + } + + void writeHeader(unsigned char* buffer) const + { +#ifdef B3_USE_DOUBLE_PRECISION + memcpy(buffer, "BULLETd", 7); +#else + memcpy(buffer, "BULLETf", 7); +#endif //B3_USE_DOUBLE_PRECISION + + int littleEndian = 1; + littleEndian = ((char*)&littleEndian)[0]; + + if (sizeof(void*) == 8) + { + buffer[7] = '-'; + } + else + { + buffer[7] = '_'; + } + + if (littleEndian) + { + buffer[8] = 'v'; + } + else + { + buffer[8] = 'V'; + } + + buffer[9] = '2'; + buffer[10] = '8'; + buffer[11] = '1'; + } + + virtual void startSerialization() + { + m_uniqueIdGenerator = 1; + if (m_totalSize) + { + unsigned char* buffer = internalAlloc(B3_HEADER_LENGTH); + writeHeader(buffer); + } + } + + virtual void finishSerialization() + { + writeDNA(); + + //if we didn't pre-allocate a buffer, we need to create a contiguous buffer now + int mysize = 0; + if (!m_totalSize) + { + if (m_buffer) + b3AlignedFree(m_buffer); + + m_currentSize += B3_HEADER_LENGTH; + m_buffer = (unsigned char*)b3AlignedAlloc(m_currentSize, 16); + + unsigned char* currentPtr = m_buffer; + writeHeader(m_buffer); + currentPtr += B3_HEADER_LENGTH; + mysize += B3_HEADER_LENGTH; + for (int i = 0; i < m_chunkPtrs.size(); i++) + { + int curLength = sizeof(b3Chunk) + m_chunkPtrs[i]->m_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_currentSize < m_totalSize); + } + else + { + ptr = (unsigned char*)b3AlignedAlloc(size, 16); + m_currentSize += int(size); + } + return ptr; + } + + virtual b3Chunk* allocate(size_t size, int numElements) + { + unsigned char* ptr = internalAlloc(int(size) * numElements + sizeof(b3Chunk)); + + unsigned char* data = ptr + sizeof(b3Chunk); + + b3Chunk* chunk = (b3Chunk*)ptr; + chunk->m_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 < len; i++) + { + destinationName[i] = name[i]; + } + destinationName[len] = 0; + finalizeChunk(chunk, "char", B3_ARRAY_CODE, (void*)name); + } + } + } + + virtual int getSerializationFlags() const + { + return m_serializationFlags; + } + + virtual void setSerializationFlags(int flags) + { + m_serializationFlags = flags; + } +}; + +#endif //B3_SERIALIZER_H diff --git a/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/premake4.lua b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/premake4.lua new file mode 100644 index 000000000..b9ee301be --- /dev/null +++ b/Engine/lib/bullet/src/Bullet3Serialize/Bullet2FileLoader/premake4.lua @@ -0,0 +1,16 @@ + project "Bullet2FileLoader" + + kind "StaticLib" + + includedirs { + "../../../src" + } + + if os.is("Linux") then + buildoptions{"-fPIC"} + end + + files { + "**.cpp", + "**.h" + } \ No newline at end of file diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp index 77763305b..ec6fe9f4d 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp @@ -2,7 +2,6 @@ //Bullet Continuous Collision Detection and Physics Library //Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - // // btAxisSweep3 // @@ -19,18 +18,15 @@ // 3. This notice may not be removed or altered from any source distribution. #include "btAxisSweep3.h" - -btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator) -:btAxisSweep3Internal(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator) +btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator) + : btAxisSweep3Internal(worldAabbMin, worldAabbMax, 0xfffe, 0xffff, maxHandles, pairCache, disableRaycastAccelerator) { // 1 handle is reserved as sentinel btAssert(maxHandles > 1 && maxHandles < 32767); - } - -bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache , bool disableRaycastAccelerator) -:btAxisSweep3Internal(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache,disableRaycastAccelerator) +bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator) + : btAxisSweep3Internal(worldAabbMin, worldAabbMax, 0xfffffffe, 0x7fffffff, maxHandles, pairCache, disableRaycastAccelerator) { // 1 handle is reserved as sentinel btAssert(maxHandles > 1 && maxHandles < 2147483647); diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index a3648df1a..1e42f25f3 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -33,9 +33,7 @@ class btAxisSweep3 : public btAxisSweep3Internal { public: - - btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); - + 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. @@ -44,9 +42,7 @@ public: class bt32BitAxisSweep3 : public btAxisSweep3Internal { public: - - bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); - + bt32BitAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); }; #endif diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h index 2c4d41bc0..2ee35528f 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h @@ -36,172 +36,158 @@ template class btAxisSweep3Internal : public btBroadphaseInterface { protected: - - BP_FP_INT_TYPE m_bpHandleMask; - BP_FP_INT_TYPE m_handleSentinel; + BP_FP_INT_TYPE m_bpHandleMask; + BP_FP_INT_TYPE m_handleSentinel; public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); + BT_DECLARE_ALIGNED_ALLOCATOR(); class Edge { public: - BP_FP_INT_TYPE m_pos; // low bit is min/max + BP_FP_INT_TYPE m_pos; // low bit is min/max BP_FP_INT_TYPE m_handle; - BP_FP_INT_TYPE IsMax() const {return static_cast(m_pos & 1);} + BP_FP_INT_TYPE IsMax() const { return static_cast(m_pos & 1); } }; public: - class Handle : public btBroadphaseProxy + class Handle : public btBroadphaseProxy { public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - + BT_DECLARE_ALIGNED_ALLOCATOR(); + // indexes into the edge arrays - BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 -// BP_FP_INT_TYPE m_uniqueId; - btBroadphaseProxy* m_dbvtProxy;//for faster raycast + BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 + // BP_FP_INT_TYPE m_uniqueId; + btBroadphaseProxy* m_dbvtProxy; //for faster raycast //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject - - SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;} - SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];} - }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry - + SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) { m_minEdges[0] = next; } + SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const { return m_minEdges[0]; } + }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry + protected: - btVector3 m_worldAabbMin; // overall system bounds - btVector3 m_worldAabbMax; // overall system bounds + btVector3 m_worldAabbMin; // overall system bounds + btVector3 m_worldAabbMax; // overall system bounds - btVector3 m_quantize; // scaling factor for quantization + btVector3 m_quantize; // scaling factor for quantization - BP_FP_INT_TYPE m_numHandles; // number of active handles - BP_FP_INT_TYPE m_maxHandles; // max number of handles - Handle* m_pHandles; // handles pool - - BP_FP_INT_TYPE m_firstFreeHandle; // free handles list + BP_FP_INT_TYPE m_numHandles; // number of active handles + BP_FP_INT_TYPE m_maxHandles; // max number of handles + Handle* m_pHandles; // handles pool - Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries) + BP_FP_INT_TYPE m_firstFreeHandle; // free handles list + + Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries) void* m_pEdgesRawPtr[3]; btOverlappingPairCache* m_pairCache; ///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. btOverlappingPairCallback* m_userPairCallback; - - bool m_ownsPairCache; - int m_invalidPair; + bool m_ownsPairCache; + + int m_invalidPair; ///additional dynamic aabb structure, used to accelerate ray cast queries. ///can be disabled using a optional argument in the constructor - btDbvtBroadphase* m_raycastAccelerator; - btOverlappingPairCache* m_nullPairCache; - + btDbvtBroadphase* m_raycastAccelerator; + btOverlappingPairCache* m_nullPairCache; // allocation/deallocation BP_FP_INT_TYPE allocHandle(); void freeHandle(BP_FP_INT_TYPE handle); - - bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1); + bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB, int axis0, int axis1); #ifdef DEBUG_BROADPHASE - void debugPrintAxis(int axis,bool checkCardinality=true); -#endif //DEBUG_BROADPHASE + void debugPrintAxis(int axis, bool checkCardinality = true); +#endif //DEBUG_BROADPHASE //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); - - - void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); - void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); - void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); - void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); + void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps); + void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps); + void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps); + void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps); public: + btAxisSweep3Internal(const btVector3& worldAabbMin, const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); - btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false); - - virtual ~btAxisSweep3Internal(); + virtual ~btAxisSweep3Internal(); BP_FP_INT_TYPE getNumHandles() const { return m_numHandles; } - virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - - 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;} + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); + + 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; } virtual void resetPool(btDispatcher* dispatcher); - void processAllOverlappingPairs(btOverlapCallback* callback); + void processAllOverlappingPairs(btOverlapCallback* callback); //Broadphase Interface - 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; - - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); - virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); + 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; + + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); - void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const; ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result - void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - - bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + void unQuantize(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const; - btOverlappingPairCache* getOverlappingPairCache() + bool testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); + + btOverlappingPairCache* getOverlappingPairCache() { return m_pairCache; } - const btOverlappingPairCache* getOverlappingPairCache() const + const btOverlappingPairCache* getOverlappingPairCache() const { return m_pairCache; } - void setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback) + void setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback) { m_userPairCallback = pairCallback; } - const btOverlappingPairCallback* getOverlappingPairUserCallback() const + const btOverlappingPairCallback* getOverlappingPairUserCallback() const { return m_userPairCallback; } ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame ///will add some transform later - virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const + virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const { aabbMin = m_worldAabbMin; aabbMax = m_worldAabbMax; } - virtual void printStats() + virtual void printStats() { -/* printf("btAxisSweep3.h\n"); + /* printf("btAxisSweep3.h\n"); printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(), m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ()); */ - } - }; //////////////////////////////////////////////////////////////////// - - - #ifdef DEBUG_BROADPHASE #include @@ -209,75 +195,73 @@ template void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinality) { int numEdges = m_pHandles[0].m_maxEdges[axis]; - printf("SAP Axis %d, numEdges=%d\n",axis,numEdges); + printf("SAP Axis %d, numEdges=%d\n", axis, numEdges); int i; - for (i=0;im_handle); - int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis]; + int handleIndex = pEdge->IsMax() ? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis]; char beginOrEnd; - beginOrEnd=pEdge->IsMax()?'E':'B'; - printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex); + beginOrEnd = pEdge->IsMax() ? 'E' : 'B'; + printf(" [%c,h=%d,p=%x,i=%d]\n", beginOrEnd, pEdge->m_handle, pEdge->m_pos, handleIndex); } if (checkCardinality) - btAssert(numEdges == m_numHandles*2+1); + btAssert(numEdges == m_numHandles * 2 + 1); } -#endif //DEBUG_BROADPHASE +#endif //DEBUG_BROADPHASE template -btBroadphaseProxy* btAxisSweep3Internal::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher) +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); - - Handle* handle = getHandle(handleId); - - if (m_raycastAccelerator) - { - btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher); - handle->m_dbvtProxy = rayProxy; - } - return handle; + (void)shapeType; + 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); + handle->m_dbvtProxy = rayProxy; + } + return handle; } - - template -void btAxisSweep3Internal::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +void btAxisSweep3Internal::destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher) { Handle* handle = static_cast(proxy); if (m_raycastAccelerator) - m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher); + m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy, dispatcher); removeHandle(static_cast(handle->m_uniqueId), dispatcher); } template -void btAxisSweep3Internal::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) +void btAxisSweep3Internal::setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher) { Handle* handle = static_cast(proxy); handle->m_aabbMin = aabbMin; handle->m_aabbMax = aabbMax; - updateHandle(static_cast(handle->m_uniqueId), aabbMin, aabbMax,dispatcher); + updateHandle(static_cast(handle->m_uniqueId), aabbMin, aabbMax, dispatcher); if (m_raycastAccelerator) - m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher); - + m_raycastAccelerator->setAabb(handle->m_dbvtProxy, aabbMin, aabbMax, dispatcher); } template -void btAxisSweep3Internal::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) +void btAxisSweep3Internal::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax) { if (m_raycastAccelerator) { - m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax); - } else + m_raycastAccelerator->rayTest(rayFrom, rayTo, rayCallback, aabbMin, aabbMax); + } + else { //choose axis? BP_FP_INT_TYPE axis = 0; //for each proxy - for (BP_FP_INT_TYPE i=1;i::rayTest(const btVector3& rayFrom,cons } template -void btAxisSweep3Internal::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) +void btAxisSweep3Internal::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) { if (m_raycastAccelerator) { - m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback); - } else + m_raycastAccelerator->aabbTest(aabbMin, aabbMax, callback); + } + else { //choose axis? BP_FP_INT_TYPE axis = 0; //for each proxy - for (BP_FP_INT_TYPE i=1;im_aabbMin,handle->m_aabbMax)) + if (TestAabbAgainstAabb2(aabbMin, aabbMax, handle->m_aabbMin, handle->m_aabbMax)) { callback.process(handle); } @@ -312,66 +297,60 @@ void btAxisSweep3Internal::aabbTest(const btVector3& aabbMin, co } } - - template -void btAxisSweep3Internal::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +void btAxisSweep3Internal::getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const { Handle* pHandle = static_cast(proxy); aabbMin = pHandle->m_aabbMin; aabbMax = pHandle->m_aabbMax; } - template -void btAxisSweep3Internal::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +void btAxisSweep3Internal::unQuantize(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const { Handle* pHandle = static_cast(proxy); unsigned short vecInMin[3]; unsigned short vecInMax[3]; - vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ; - vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ; - vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ; - vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ; - vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ; - vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ; - - aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ())); + vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos; + vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos + 1; + vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos; + vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos + 1; + vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos; + vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos + 1; + + aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()), (btScalar)(vecInMin[1]) / (m_quantize.getY()), (btScalar)(vecInMin[2]) / (m_quantize.getZ())); aabbMin += m_worldAabbMin; - - aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ())); + + aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()), (btScalar)(vecInMax[1]) / (m_quantize.getY()), (btScalar)(vecInMax[2]) / (m_quantize.getZ())); aabbMax += m_worldAabbMin; } - - - template -btAxisSweep3Internal::btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator) -:m_bpHandleMask(handleMask), -m_handleSentinel(handleSentinel), -m_pairCache(pairCache), -m_userPairCallback(0), -m_ownsPairCache(false), -m_invalidPair(0), -m_raycastAccelerator(0) +btAxisSweep3Internal::btAxisSweep3Internal(const btVector3& worldAabbMin, const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator) + : m_bpHandleMask(handleMask), + m_handleSentinel(handleSentinel), + m_pairCache(pairCache), + m_userPairCallback(0), + m_ownsPairCache(false), + m_invalidPair(0), + m_raycastAccelerator(0) { - BP_FP_INT_TYPE maxHandles = static_cast(userMaxHandles+1);//need to add one sentinel handle + BP_FP_INT_TYPE maxHandles = static_cast(userMaxHandles + 1); //need to add one sentinel handle if (!m_pairCache) { - void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16); - m_pairCache = new(ptr) btHashedOverlappingPairCache(); + void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16); + m_pairCache = new (ptr) btHashedOverlappingPairCache(); m_ownsPairCache = true; } if (!disableRaycastAccelerator) { - m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache(); - m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache); - m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs + m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache), 16)) btNullPairCache(); + m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase), 16)) btDbvtBroadphase(m_nullPairCache); //m_pairCache); + m_raycastAccelerator->m_deferedcollide = true; //don't add/remove pairs } //btAssert(bounds.HasVolume()); @@ -382,13 +361,13 @@ m_raycastAccelerator(0) btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; - BP_FP_INT_TYPE maxInt = m_handleSentinel; + BP_FP_INT_TYPE maxInt = m_handleSentinel; - m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize; + m_quantize = btVector3(btScalar(maxInt), btScalar(maxInt), btScalar(maxInt)) / aabbSize; // allocate handles buffer, using btAlignedAlloc, and put all handles on free list m_pHandles = new Handle[maxHandles]; - + m_maxHandles = maxHandles; m_numHandles = 0; @@ -404,14 +383,14 @@ m_raycastAccelerator(0) // allocate edge buffers for (int i = 0; i < 3; i++) { - m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16); - m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2]; + m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge) * maxHandles * 2, 16); + m_pEdges[i] = new (m_pEdgesRawPtr[i]) Edge[maxHandles * 2]; } } //removed overlap management // make boundary sentinels - + m_pHandles[0].m_clientObject = 0; for (int axis = 0; axis < 3; axis++) @@ -425,10 +404,8 @@ m_raycastAccelerator(0) m_pEdges[axis][1].m_handle = 0; #ifdef DEBUG_BROADPHASE debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - +#endif //DEBUG_BROADPHASE } - } template @@ -439,14 +416,14 @@ btAxisSweep3Internal::~btAxisSweep3Internal() m_nullPairCache->~btOverlappingPairCache(); btAlignedFree(m_nullPairCache); m_raycastAccelerator->~btDbvtBroadphase(); - btAlignedFree (m_raycastAccelerator); + btAlignedFree(m_raycastAccelerator); } for (int i = 2; i >= 0; i--) { btAlignedFree(m_pEdgesRawPtr[i]); } - delete [] m_pHandles; + delete[] m_pHandles; if (m_ownsPairCache) { @@ -470,13 +447,12 @@ void btAxisSweep3Internal::quantize(BP_FP_INT_TYPE* out, const b out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax); #else btVector3 v = (point - m_worldAabbMin) * m_quantize; - out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax); - out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax); - out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax); -#endif //OLD_CLAMPING_METHOD + out[0] = (v[0] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[0] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0] & m_bpHandleMask) | isMax); + out[1] = (v[1] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[1] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1] & m_bpHandleMask) | isMax); + out[2] = (v[2] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[2] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2] & m_bpHandleMask) | isMax); +#endif //OLD_CLAMPING_METHOD } - template BP_FP_INT_TYPE btAxisSweep3Internal::allocHandle() { @@ -500,9 +476,8 @@ void btAxisSweep3Internal::freeHandle(BP_FP_INT_TYPE handle) m_numHandles--; } - template -BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher) +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]; @@ -511,10 +486,9 @@ BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& // allocate a handle BP_FP_INT_TYPE handle = allocHandle(); - Handle* pHandle = getHandle(handle); - + pHandle->m_uniqueId = static_cast(handle); //pHandle->m_pOverlaps = 0; pHandle->m_clientObject = pOwner; @@ -524,11 +498,9 @@ BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& // compute current limit of edge arrays BP_FP_INT_TYPE limit = static_cast(m_numHandles * 2); - // insert new edges just inside the max boundary edge for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++) { - m_pHandles[0].m_maxEdges[axis] += 2; m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1]; @@ -544,22 +516,19 @@ BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& } // now sort the new edges to their correct position - sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false); - sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false); - sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false); - sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false); - sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true); - sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true); - + sortMinDown(0, pHandle->m_minEdges[0], dispatcher, false); + sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher, false); + sortMinDown(1, pHandle->m_minEdges[1], dispatcher, false); + sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher, false); + sortMinDown(2, pHandle->m_minEdges[2], dispatcher, true); + sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher, true); return handle; } - template -void btAxisSweep3Internal::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher) +void btAxisSweep3Internal::removeHandle(BP_FP_INT_TYPE handle, btDispatcher* dispatcher) { - Handle* pHandle = getHandle(handle); //explicitly remove the pairs containing the proxy @@ -567,50 +536,43 @@ void btAxisSweep3Internal::removeHandle(BP_FP_INT_TYPE handle,bt ///@todo: compare performance if (!m_pairCache->hasDeferredRemoval()) { - m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher); + m_pairCache->removeOverlappingPairsContainingProxy(pHandle, dispatcher); } // compute current limit of edge arrays int limit = static_cast(m_numHandles * 2); - + int axis; - for (axis = 0;axis<3;axis++) + for (axis = 0; axis < 3; axis++) { m_pHandles[0].m_maxEdges[axis] -= 2; } // remove the edges by sorting them up to the end of the list - for ( axis = 0; axis < 3; axis++) + for (axis = 0; axis < 3; axis++) { Edge* pEdges = m_pEdges[axis]; BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis]; pEdges[max].m_pos = m_handleSentinel; - sortMaxUp(axis,max,dispatcher,false); - + sortMaxUp(axis, max, dispatcher, false); BP_FP_INT_TYPE i = pHandle->m_minEdges[axis]; pEdges[i].m_pos = m_handleSentinel; + sortMinUp(axis, i, dispatcher, false); - sortMinUp(axis,i,dispatcher,false); + pEdges[limit - 1].m_handle = 0; + pEdges[limit - 1].m_pos = m_handleSentinel; - pEdges[limit-1].m_handle = 0; - pEdges[limit-1].m_pos = m_handleSentinel; - #ifdef DEBUG_BROADPHASE - debugPrintAxis(axis,false); -#endif //DEBUG_BROADPHASE - - + debugPrintAxis(axis, false); +#endif //DEBUG_BROADPHASE } - // free the handle freeHandle(handle); - - } template @@ -625,20 +587,16 @@ void btAxisSweep3Internal::resetPool(btDispatcher* /*dispatcher* m_pHandles[m_maxHandles - 1].SetNextFree(0); } } -} +} - -extern int gOverlappingPairs; //#include template -void btAxisSweep3Internal::calculateOverlappingPairs(btDispatcher* dispatcher) +void btAxisSweep3Internal::calculateOverlappingPairs(btDispatcher* dispatcher) { - if (m_pairCache->hasDeferredRemoval()) { - - btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); + btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); //perform a sort, to find duplicates and to sort 'invalid' pairs to the end overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); @@ -646,18 +604,15 @@ void btAxisSweep3Internal::calculateOverlappingPairs(btDispatche overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); m_invalidPair = 0; - int i; btBroadphasePair previousPair; previousPair.m_pProxy0 = 0; previousPair.m_pProxy1 = 0; previousPair.m_algorithm = 0; - - - for (i=0;i::calculateOverlappingPairs(btDispatche if (!isDuplicate) { ///important to use an AABB test that is consistent with the broadphase - bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); + bool hasOverlap = testAabbOverlap(pair.m_pProxy0, pair.m_pProxy1); if (hasOverlap) { - needsRemoval = false;//callback->processOverlap(pair); - } else + needsRemoval = false; //callback->processOverlap(pair); + } + else { needsRemoval = true; } - } else + } + else { //remove duplicate needsRemoval = true; //should have no algorithm btAssert(!pair.m_algorithm); } - + if (needsRemoval) { - m_pairCache->cleanOverlappingPair(pair,dispatcher); + m_pairCache->cleanOverlappingPair(pair, dispatcher); - // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - // m_overlappingPairArray.pop_back(); + // 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 +///if you don't like to skip the invalid pairs in the array, execute following code: +#define CLEAN_INVALID_PAIRS 1 +#ifdef CLEAN_INVALID_PAIRS //perform a sort, to sort 'invalid' pairs to the end overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); m_invalidPair = 0; - #endif//CLEAN_INVALID_PAIRS - +#endif //CLEAN_INVALID_PAIRS + //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); } - } - template -bool btAxisSweep3Internal::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +bool btAxisSweep3Internal::testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { const Handle* pHandleA = static_cast(proxy0); const Handle* pHandleB = static_cast(proxy1); - + //optimization 1: check the array index (memory address), instead of the m_pos for (int axis = 0; axis < 3; axis++) - { - if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || - pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) - { - return false; - } - } + { + if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || + pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) + { + return false; + } + } return true; } template -bool btAxisSweep3Internal::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1) +bool btAxisSweep3Internal::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB, int axis0, int axis1) { //optimization 1: check the array index (memory address), instead of the m_pos - if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] || + if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] || pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] || pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] || - pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1]) - { - return false; - } + pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1]) + { + return false; + } return true; } template -void btAxisSweep3Internal::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) +void btAxisSweep3Internal::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher) { -// btAssert(bounds.IsFinite()); + // btAssert(bounds.IsFinite()); //btAssert(bounds.HasVolume()); Handle* pHandle = getHandle(handle); @@ -778,34 +731,28 @@ void btAxisSweep3Internal::updateHandle(BP_FP_INT_TYPE handle, c // expand (only adds overlaps) if (dmin < 0) - sortMinDown(axis, emin,dispatcher,true); + sortMinDown(axis, emin, dispatcher, true); if (dmax > 0) - sortMaxUp(axis, emax,dispatcher,true); + sortMaxUp(axis, emax, dispatcher, true); // shrink (only removes overlaps) if (dmin > 0) - sortMinUp(axis, emin,dispatcher,true); + sortMinUp(axis, emin, dispatcher, true); if (dmax < 0) - sortMaxDown(axis, emax,dispatcher,true); + sortMaxDown(axis, emax, dispatcher, true); #ifdef DEBUG_BROADPHASE - debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE } - - } - - - // sorting a min edge downwards can only ever *add* overlaps template void btAxisSweep3Internal::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps) { - Edge* pEdge = m_pEdges[axis] + edge; Edge* pPrev = pEdge - 1; Handle* pHandleEdge = getHandle(pEdge->m_handle); @@ -817,16 +764,15 @@ void btAxisSweep3Internal::sortMinDown(int axis, BP_FP_INT_TYPE if (pPrev->IsMax()) { // if previous edge is a maximum check the bounds and add an overlap if necessary - const int axis1 = (1 << axis) & 3; - const int axis2 = (1 << axis1) & 3; - if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2)) + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev, axis1, axis2)) { - m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev); + m_pairCache->addOverlappingPair(pHandleEdge, pHandlePrev); if (m_userPairCallback) - m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev); + m_userPairCallback->addOverlappingPair(pHandleEdge, pHandlePrev); //AddOverlap(pEdge->m_handle, pPrev->m_handle); - } // update edge reference in other handle @@ -849,8 +795,7 @@ void btAxisSweep3Internal::sortMinDown(int axis, BP_FP_INT_TYPE #ifdef DEBUG_BROADPHASE debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - +#endif //DEBUG_BROADPHASE } // sorting a min edge upwards can only ever *remove* overlaps @@ -869,25 +814,21 @@ void btAxisSweep3Internal::sortMinUp(int axis, BP_FP_INT_TYPE ed { Handle* handle0 = getHandle(pEdge->m_handle); Handle* handle1 = getHandle(pNext->m_handle); - const int axis1 = (1 << axis) & 3; - const int axis2 = (1 << axis1) & 3; - + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + // if next edge is maximum remove any overlap between the two handles - if (updateOverlaps + if (updateOverlaps #ifdef USE_OVERLAP_TEST_ON_REMOVES - && testOverlap2D(handle0,handle1,axis1,axis2) -#endif //USE_OVERLAP_TEST_ON_REMOVES - ) + && testOverlap2D(handle0, handle1, axis1, axis2) +#endif //USE_OVERLAP_TEST_ON_REMOVES + ) { - - - m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); + m_pairCache->removeOverlappingPair(handle0, handle1, dispatcher); if (m_userPairCallback) - m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); - + m_userPairCallback->removeOverlappingPair(handle0, handle1, dispatcher); } - // update edge reference in other handle pHandleNext->m_maxEdges[axis]--; } @@ -905,15 +846,12 @@ void btAxisSweep3Internal::sortMinUp(int axis, BP_FP_INT_TYPE ed pEdge++; pNext++; } - - } // sorting a max edge downwards can only ever *remove* overlaps template void btAxisSweep3Internal::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps) { - Edge* pEdge = m_pEdges[axis] + edge; Edge* pPrev = pEdge - 1; Handle* pHandleEdge = getHandle(pEdge->m_handle); @@ -927,28 +865,25 @@ void btAxisSweep3Internal::sortMaxDown(int axis, BP_FP_INT_TYPE // if previous edge was a minimum remove any overlap between the two handles Handle* handle0 = getHandle(pEdge->m_handle); Handle* handle1 = getHandle(pPrev->m_handle); - const int axis1 = (1 << axis) & 3; - const int axis2 = (1 << axis1) & 3; + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; - if (updateOverlaps + if (updateOverlaps #ifdef USE_OVERLAP_TEST_ON_REMOVES - && testOverlap2D(handle0,handle1,axis1,axis2) -#endif //USE_OVERLAP_TEST_ON_REMOVES - ) + && testOverlap2D(handle0, handle1, axis1, axis2) +#endif //USE_OVERLAP_TEST_ON_REMOVES + ) { //this is done during the overlappingpairarray iteration/narrowphase collision - - m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); + m_pairCache->removeOverlappingPair(handle0, handle1, dispatcher); if (m_userPairCallback) - m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); - - - + m_userPairCallback->removeOverlappingPair(handle0, handle1, dispatcher); } // update edge reference in other handle - pHandlePrev->m_minEdges[axis]++;; + pHandlePrev->m_minEdges[axis]++; + ; } else pHandlePrev->m_maxEdges[axis]++; @@ -965,11 +900,9 @@ void btAxisSweep3Internal::sortMaxDown(int axis, BP_FP_INT_TYPE pPrev--; } - #ifdef DEBUG_BROADPHASE debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - +#endif //DEBUG_BROADPHASE } // sorting a max edge upwards can only ever *add* overlaps @@ -984,19 +917,19 @@ void btAxisSweep3Internal::sortMaxUp(int axis, BP_FP_INT_TYPE ed { Handle* pHandleNext = getHandle(pNext->m_handle); - const int axis1 = (1 << axis) & 3; - const int axis2 = (1 << axis1) & 3; + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; if (!pNext->IsMax()) { // if next edge is a minimum check the bounds and add an overlap if necessary - if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2)) + if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext, axis1, axis2)) { Handle* handle0 = getHandle(pEdge->m_handle); Handle* handle1 = getHandle(pNext->m_handle); - m_pairCache->addOverlappingPair(handle0,handle1); + m_pairCache->addOverlappingPair(handle0, handle1); if (m_userPairCallback) - m_userPairCallback->addOverlappingPair(handle0,handle1); + m_userPairCallback->addOverlappingPair(handle0, handle1); } // update edge reference in other handle @@ -1016,7 +949,6 @@ void btAxisSweep3Internal::sortMaxUp(int axis, BP_FP_INT_TYPE ed pEdge++; pNext++; } - } #endif diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index fb68e0024..b097eca5f 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -13,10 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BT_BROADPHASE_INTERFACE_H -#define BT_BROADPHASE_INTERFACE_H - - +#ifndef BT_BROADPHASE_INTERFACE_H +#define BT_BROADPHASE_INTERFACE_H struct btDispatcherInfo; class btDispatcher; @@ -24,27 +22,23 @@ class btDispatcher; class btOverlappingPairCache; - - -struct btBroadphaseAabbCallback +struct btBroadphaseAabbCallback { virtual ~btBroadphaseAabbCallback() {} - virtual bool process(const btBroadphaseProxy* proxy) = 0; + virtual bool process(const btBroadphaseProxy* proxy) = 0; }; - -struct btBroadphaseRayCallback : public btBroadphaseAabbCallback +struct btBroadphaseRayCallback : public btBroadphaseAabbCallback { ///added some cached data to accelerate ray-AABB tests - btVector3 m_rayDirectionInverse; - unsigned int m_signs[3]; - btScalar m_lambda_max; + btVector3 m_rayDirectionInverse; + unsigned int m_signs[3]; + btScalar m_lambda_max; virtual ~btBroadphaseRayCallback() {} - + protected: - - btBroadphaseRayCallback() {} + btBroadphaseRayCallback() {} }; #include "LinearMath/btVector3.h" @@ -57,30 +51,29 @@ class btBroadphaseInterface public: virtual ~btBroadphaseInterface() {} - 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; + 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; - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0; + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0)) = 0; - virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0; + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0; ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb - virtual void calculateOverlappingPairs(btDispatcher* dispatcher)=0; + virtual void calculateOverlappingPairs(btDispatcher* dispatcher) = 0; - virtual btOverlappingPairCache* getOverlappingPairCache()=0; - virtual const btOverlappingPairCache* getOverlappingPairCache() const =0; + virtual btOverlappingPairCache* getOverlappingPairCache() = 0; + virtual const btOverlappingPairCache* getOverlappingPairCache() const = 0; ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame ///will add some transform later - virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0; + virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const = 0; ///reset broadphase internal structures, to ensure determinism/reproducability - virtual void resetPool(btDispatcher* dispatcher) { (void) dispatcher; }; - - virtual void printStats() = 0; + virtual void resetPool(btDispatcher* dispatcher) { (void)dispatcher; }; + virtual void printStats() = 0; }; -#endif //BT_BROADPHASE_INTERFACE_H +#endif //BT_BROADPHASE_INTERFACE_H diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp index 0fd4ef46b..7ee065aac 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp @@ -15,4 +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 +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/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index adaf083a2..825caeef5 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -16,11 +16,10 @@ subject to the following restrictions: #ifndef BT_BROADPHASE_PROXY_H #define BT_BROADPHASE_PROXY_H -#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE +#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE #include "LinearMath/btVector3.h" #include "LinearMath/btAlignedAllocator.h" - /// btDispatcher uses these types /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave /// to facilitate type checking @@ -35,8 +34,8 @@ enum BroadphaseNativeTypes CONVEX_HULL_SHAPE_PROXYTYPE, CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE, CUSTOM_POLYHEDRAL_SHAPE_TYPE, -//implicit convex shapes -IMPLICIT_CONVEX_SHAPES_START_HERE, + //implicit convex shapes + IMPLICIT_CONVEX_SHAPES_START_HERE, SPHERE_SHAPE_PROXYTYPE, MULTI_SPHERE_SHAPE_PROXYTYPE, CAPSULE_SHAPE_PROXYTYPE, @@ -49,8 +48,8 @@ IMPLICIT_CONVEX_SHAPES_START_HERE, BOX_2D_SHAPE_PROXYTYPE, CONVEX_2D_SHAPE_PROXYTYPE, CUSTOM_CONVEX_SHAPE_TYPE, -//concave shapes -CONCAVE_SHAPES_START_HERE, + //concave shapes + CONCAVE_SHAPES_START_HERE, //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! TRIANGLE_MESH_SHAPE_PROXYTYPE, SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE, @@ -58,15 +57,16 @@ CONCAVE_SHAPES_START_HERE, FAST_CONCAVE_MESH_PROXYTYPE, //terrain TERRAIN_SHAPE_PROXYTYPE, -///Used for GIMPACT Trimesh integration + ///Used for GIMPACT Trimesh integration GIMPACT_SHAPE_PROXYTYPE, -///Multimaterial mesh - MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE, - + ///Multimaterial mesh + MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE, + EMPTY_SHAPE_PROXYTYPE, STATIC_PLANE_PROXYTYPE, CUSTOM_CONCAVE_SHAPE_TYPE, -CONCAVE_SHAPES_END_HERE, + SDF_SHAPE_PROXYTYPE = CUSTOM_CONCAVE_SHAPE_TYPE, + CONCAVE_SHAPES_END_HERE, COMPOUND_SHAPE_PROXYTYPE, @@ -76,38 +76,37 @@ CONCAVE_SHAPES_END_HERE, INVALID_SHAPE_PROXYTYPE, MAX_BROADPHASE_COLLISION_TYPES - + }; - -///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases. +///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases. ///It stores collision shape type information, collision filter information and a client object, typically a btCollisionObject or btRigidBody. -ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy +ATTRIBUTE_ALIGNED16(struct) +btBroadphaseProxy { + BT_DECLARE_ALIGNED_ALLOCATOR(); -BT_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 + 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 btCollisionObject or Rigidbody class - void* m_clientObject; - int m_collisionFilterGroup; - int m_collisionFilterMask; + 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. + 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; + btVector3 m_aabbMin; + btVector3 m_aabbMax; SIMD_FORCE_INLINE int getUid() const { @@ -115,47 +114,45 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); } //used for memory pools - btBroadphaseProxy() :m_clientObject(0) + btBroadphaseProxy() : m_clientObject(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) + 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) { } - - static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType) { - return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE); + return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE); } - static SIMD_FORCE_INLINE bool isConvex(int proxyType) + static SIMD_FORCE_INLINE bool isConvex(int proxyType) { return (proxyType < CONCAVE_SHAPES_START_HERE); } - static SIMD_FORCE_INLINE bool isNonMoving(int proxyType) + static SIMD_FORCE_INLINE bool isNonMoving(int proxyType) { - return (isConcave(proxyType) && !(proxyType==GIMPACT_SHAPE_PROXYTYPE)); + return (isConcave(proxyType) && !(proxyType == GIMPACT_SHAPE_PROXYTYPE)); } - static SIMD_FORCE_INLINE bool isConcave(int proxyType) + static SIMD_FORCE_INLINE bool isConcave(int proxyType) { return ((proxyType > CONCAVE_SHAPES_START_HERE) && - (proxyType < CONCAVE_SHAPES_END_HERE)); + (proxyType < CONCAVE_SHAPES_END_HERE)); } - static SIMD_FORCE_INLINE bool isCompound(int proxyType) + static SIMD_FORCE_INLINE bool isCompound(int proxyType) { return (proxyType == COMPOUND_SHAPE_PROXYTYPE); } - static SIMD_FORCE_INLINE bool isSoftBody(int proxyType) + static SIMD_FORCE_INLINE bool isSoftBody(int proxyType) { return (proxyType == SOFTBODY_SHAPE_PROXYTYPE); } @@ -167,67 +164,62 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); static SIMD_FORCE_INLINE bool isConvex2d(int proxyType) { - return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE); + return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE); } - - -} -; +}; class btCollisionAlgorithm; struct btBroadphaseProxy; - - ///The btBroadphasePair class contains a pair of aabb-overlapping objects. ///A btDispatcher can search a btCollisionAlgorithm that performs exact/narrowphase collision detection on the actual collision shapes. -ATTRIBUTE_ALIGNED16(struct) btBroadphasePair +ATTRIBUTE_ALIGNED16(struct) +btBroadphasePair { - btBroadphasePair () - : - m_pProxy0(0), - m_pProxy1(0), - m_algorithm(0), - m_internalInfo1(0) + btBroadphasePair() + : m_pProxy0(0), + m_pProxy1(0), + m_algorithm(0), + m_internalInfo1(0) { } -BT_DECLARE_ALIGNED_ALLOCATOR(); + BT_DECLARE_ALIGNED_ALLOCATOR(); btBroadphasePair(const btBroadphasePair& other) - : m_pProxy0(other.m_pProxy0), - m_pProxy1(other.m_pProxy1), - m_algorithm(other.m_algorithm), - m_internalInfo1(other.m_internalInfo1) + : m_pProxy0(other.m_pProxy0), + m_pProxy1(other.m_pProxy1), + m_algorithm(other.m_algorithm), + m_internalInfo1(other.m_internalInfo1) { } - btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) + btBroadphasePair(btBroadphaseProxy & proxy0, btBroadphaseProxy & proxy1) { - //keep them sorted, so the std::set operations work if (proxy0.m_uniqueId < proxy1.m_uniqueId) - { - m_pProxy0 = &proxy0; - m_pProxy1 = &proxy1; - } - else - { - m_pProxy0 = &proxy1; - m_pProxy1 = &proxy0; - } + { + m_pProxy0 = &proxy0; + m_pProxy1 = &proxy1; + } + else + { + m_pProxy0 = &proxy1; + m_pProxy1 = &proxy0; + } m_algorithm = 0; m_internalInfo1 = 0; - } - + btBroadphaseProxy* m_pProxy0; btBroadphaseProxy* m_pProxy1; - - mutable btCollisionAlgorithm* m_algorithm; - union { void* m_internalInfo1; int m_internalTmpValue;};//don't use this data, it will be removed in future version. + mutable btCollisionAlgorithm* m_algorithm; + union { + void* m_internalInfo1; + int m_internalTmpValue; + }; //don't use this data, it will be removed in future version. }; /* @@ -239,31 +231,25 @@ SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePa } */ - - class btBroadphasePairSortPredicate { - public: +public: + bool operator()(const btBroadphasePair& a, const btBroadphasePair& b) const + { + const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1; + const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1; + const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1; + const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1; - bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) const - { - const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1; - const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1; - const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1; - const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1; - - return uidA0 > uidB0 || - (a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) || - (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); - } + return uidA0 > uidB0 || + (a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) || + (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); + } }; - -SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) +SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) { - return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1); + return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1); } - -#endif //BT_BROADPHASE_PROXY_H - +#endif //BT_BROADPHASE_PROXY_H diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp index c95d1be0f..6e36d3bd7 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp @@ -20,4 +20,3 @@ btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructio { m_dispatcher = ci.m_dispatcher1; } - diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h index 405656236..b00c0b1b4 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -25,57 +25,51 @@ class btManifoldResult; class btCollisionObject; struct btCollisionObjectWrapper; struct btDispatcherInfo; -class btPersistentManifold; +class btPersistentManifold; -typedef btAlignedObjectArray btManifoldArray; +typedef btAlignedObjectArray btManifoldArray; struct btCollisionAlgorithmConstructionInfo { btCollisionAlgorithmConstructionInfo() - :m_dispatcher1(0), - m_manifold(0) + : m_dispatcher1(0), + m_manifold(0) { } - btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp) - :m_dispatcher1(dispatcher) + btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher, int temp) + : m_dispatcher1(dispatcher) { (void)temp; } - btDispatcher* m_dispatcher1; - btPersistentManifold* m_manifold; - -// int getDispatcherId(); + btDispatcher* m_dispatcher1; + btPersistentManifold* m_manifold; + // int getDispatcherId(); }; - ///btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatcher. ///It is persistent over frames class btCollisionAlgorithm { +protected: + btDispatcher* m_dispatcher; protected: + // int getDispatcherId(); - btDispatcher* m_dispatcher; - -protected: -// int getDispatcherId(); - public: - - btCollisionAlgorithm() {}; + btCollisionAlgorithm(){}; btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); - virtual ~btCollisionAlgorithm() {}; + virtual ~btCollisionAlgorithm(){}; - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) = 0; - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) = 0; - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) = 0; + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) = 0; }; - -#endif //BT_COLLISION_ALGORITHM_H +#endif //BT_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index d791d0741..166cb04c0 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -17,210 +17,226 @@ subject to the following restrictions: #include "btDbvt.h" // -typedef btAlignedObjectArray tNodeArray; -typedef btAlignedObjectArray tConstNodeArray; +typedef btAlignedObjectArray tNodeArray; +typedef btAlignedObjectArray tConstNodeArray; // struct btDbvtNodeEnumerator : btDbvt::ICollide { - tConstNodeArray nodes; + tConstNodeArray nodes; void Process(const btDbvtNode* n) { nodes.push_back(n); } }; // -static DBVT_INLINE int indexof(const btDbvtNode* node) +static DBVT_INLINE int indexof(const btDbvtNode* node) { - return(node->parent->childs[1]==node); + return (node->parent->childs[1] == node); } // -static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, +static DBVT_INLINE btDbvtVolume merge(const btDbvtVolume& a, const btDbvtVolume& b) { -#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE) - ATTRIBUTE_ALIGNED16( char locals[sizeof(btDbvtAabbMm)]); - btDbvtVolume* ptr = (btDbvtVolume*) locals; - btDbvtVolume& res=*ptr; +#ifdef BT_USE_SSE + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]); + btDbvtVolume* ptr = (btDbvtVolume*)locals; + btDbvtVolume& res = *ptr; #else - btDbvtVolume res; + btDbvtVolume res; #endif - Merge(a,b,res); - return(res); + Merge(a, b, res); + return (res); } // volume+edge lengths -static DBVT_INLINE btScalar size(const btDbvtVolume& a) +static DBVT_INLINE btScalar size(const btDbvtVolume& a) { - const btVector3 edges=a.Lengths(); - return( edges.x()*edges.y()*edges.z()+ - edges.x()+edges.y()+edges.z()); + const btVector3 edges = a.Lengths(); + return (edges.x() * edges.y() * edges.z() + + edges.x() + edges.y() + edges.z()); } // -static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) +static void getmaxdepth(const btDbvtNode* node, int depth, int& maxdepth) { - if(node->isinternal()) + if (node->isinternal()) { - getmaxdepth(node->childs[0],depth+1,maxdepth); - getmaxdepth(node->childs[1],depth+1,maxdepth); - } else maxdepth=btMax(maxdepth,depth); + getmaxdepth(node->childs[0], depth + 1, maxdepth); + getmaxdepth(node->childs[1], depth + 1, maxdepth); + } + else + maxdepth = btMax(maxdepth, depth); } // -static DBVT_INLINE void deletenode( btDbvt* pdbvt, - btDbvtNode* node) +static DBVT_INLINE void deletenode(btDbvt* pdbvt, + btDbvtNode* node) { btAlignedFree(pdbvt->m_free); - pdbvt->m_free=node; + pdbvt->m_free = node; } // -static void recursedeletenode( btDbvt* pdbvt, - btDbvtNode* node) +static void recursedeletenode(btDbvt* pdbvt, + btDbvtNode* node) { - if(!node->isleaf()) + if (node == 0) return; + if (!node->isleaf()) { - recursedeletenode(pdbvt,node->childs[0]); - recursedeletenode(pdbvt,node->childs[1]); + recursedeletenode(pdbvt, node->childs[0]); + recursedeletenode(pdbvt, node->childs[1]); } - if(node==pdbvt->m_root) pdbvt->m_root=0; - deletenode(pdbvt,node); + if (node == pdbvt->m_root) pdbvt->m_root = 0; + deletenode(pdbvt, node); } // -static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - void* data) +static DBVT_INLINE btDbvtNode* createnode(btDbvt* pdbvt, + btDbvtNode* parent, + void* data) { - btDbvtNode* node; - if(pdbvt->m_free) - { node=pdbvt->m_free;pdbvt->m_free=0; } - else - { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); } - node->parent = parent; - node->data = data; - node->childs[1] = 0; - return(node); -} - -// -static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - const btDbvtVolume& volume, - void* data) -{ - btDbvtNode* node=createnode(pdbvt,parent,data); - node->volume=volume; - return(node); -} - -// -static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - const btDbvtVolume& volume0, - const btDbvtVolume& volume1, - void* data) -{ - btDbvtNode* node=createnode(pdbvt,parent,data); - Merge(volume0,volume1,node->volume); - return(node); -} - -// -static void insertleaf( btDbvt* pdbvt, - btDbvtNode* root, - btDbvtNode* leaf) -{ - if(!pdbvt->m_root) + btDbvtNode* node; + if (pdbvt->m_free) { - pdbvt->m_root = leaf; - leaf->parent = 0; + node = pdbvt->m_free; + pdbvt->m_free = 0; } else { - if(!root->isleaf()) + node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); + } + node->parent = parent; + node->data = data; + node->childs[1] = 0; + return (node); +} + +// +static DBVT_INLINE btDbvtNode* createnode(btDbvt* pdbvt, + btDbvtNode* parent, + const btDbvtVolume& volume, + void* data) +{ + btDbvtNode* node = createnode(pdbvt, parent, data); + node->volume = volume; + return (node); +} + +// +static DBVT_INLINE btDbvtNode* createnode(btDbvt* pdbvt, + btDbvtNode* parent, + const btDbvtVolume& volume0, + const btDbvtVolume& volume1, + void* data) +{ + btDbvtNode* node = createnode(pdbvt, parent, data); + Merge(volume0, volume1, node->volume); + return (node); +} + +// +static void insertleaf(btDbvt* pdbvt, + btDbvtNode* root, + btDbvtNode* leaf) +{ + if (!pdbvt->m_root) + { + pdbvt->m_root = leaf; + leaf->parent = 0; + } + else + { + if (!root->isleaf()) { - do { - root=root->childs[Select( leaf->volume, - root->childs[0]->volume, - root->childs[1]->volume)]; - } while(!root->isleaf()); + do + { + root = root->childs[Select(leaf->volume, + root->childs[0]->volume, + root->childs[1]->volume)]; + } while (!root->isleaf()); } - btDbvtNode* prev=root->parent; - btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0); - if(prev) + btDbvtNode* prev = root->parent; + btDbvtNode* node = createnode(pdbvt, prev, leaf->volume, root->volume, 0); + if (prev) { - prev->childs[indexof(root)] = node; - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - do { - if(!prev->volume.Contain(node->volume)) - Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + prev->childs[indexof(root)] = node; + node->childs[0] = root; + root->parent = node; + node->childs[1] = leaf; + leaf->parent = node; + do + { + if (!prev->volume.Contain(node->volume)) + Merge(prev->childs[0]->volume, prev->childs[1]->volume, prev->volume); else break; - node=prev; - } while(0!=(prev=node->parent)); + node = prev; + } while (0 != (prev = node->parent)); } else { - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - pdbvt->m_root = node; + node->childs[0] = root; + root->parent = node; + node->childs[1] = leaf; + leaf->parent = node; + pdbvt->m_root = node; } } } // -static btDbvtNode* removeleaf( btDbvt* pdbvt, - btDbvtNode* leaf) +static btDbvtNode* removeleaf(btDbvt* pdbvt, + btDbvtNode* leaf) { - if(leaf==pdbvt->m_root) + if (leaf == pdbvt->m_root) { - pdbvt->m_root=0; - return(0); + pdbvt->m_root = 0; + return (0); } else { - btDbvtNode* parent=leaf->parent; - btDbvtNode* prev=parent->parent; - btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; - if(prev) + btDbvtNode* parent = leaf->parent; + btDbvtNode* prev = parent->parent; + btDbvtNode* sibling = parent->childs[1 - indexof(leaf)]; + if (prev) { - prev->childs[indexof(parent)]=sibling; - sibling->parent=prev; - deletenode(pdbvt,parent); - while(prev) + prev->childs[indexof(parent)] = sibling; + sibling->parent = prev; + deletenode(pdbvt, parent); + while (prev) { - const btDbvtVolume pb=prev->volume; - Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); - if(NotEqual(pb,prev->volume)) + const btDbvtVolume pb = prev->volume; + Merge(prev->childs[0]->volume, prev->childs[1]->volume, prev->volume); + if (NotEqual(pb, prev->volume)) { - prev=prev->parent; - } else break; + prev = prev->parent; + } + else + break; } - return(prev?prev:pdbvt->m_root); + return (prev ? prev : pdbvt->m_root); } else - { - pdbvt->m_root=sibling; - sibling->parent=0; - deletenode(pdbvt,parent); - return(pdbvt->m_root); - } + { + pdbvt->m_root = sibling; + sibling->parent = 0; + deletenode(pdbvt, parent); + return (pdbvt->m_root); + } } } // -static void fetchleaves(btDbvt* pdbvt, - btDbvtNode* root, - tNodeArray& leaves, - int depth=-1) +static void fetchleaves(btDbvt* pdbvt, + btDbvtNode* root, + tNodeArray& leaves, + int depth = -1) { - if(root->isinternal()&&depth) + if (root->isinternal() && depth) { - fetchleaves(pdbvt,root->childs[0],leaves,depth-1); - fetchleaves(pdbvt,root->childs[1],leaves,depth-1); - deletenode(pdbvt,root); + fetchleaves(pdbvt, root->childs[0], leaves, depth - 1); + fetchleaves(pdbvt, root->childs[1], leaves, depth - 1); + deletenode(pdbvt, root); } else { @@ -229,51 +245,50 @@ static void fetchleaves(btDbvt* pdbvt, } // -static bool leftOfAxis( const btDbvtNode* node, - const btVector3& org, - const btVector3& axis) +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) +static int split(btDbvtNode** leaves, + int count, + const btVector3& org, + const btVector3& axis) { - int begin=0; - int end=count; - for(;;) + int begin = 0; + int end = count; + for (;;) { - while(begin!=end && leftOfAxis(leaves[begin],org,axis)) + while (begin != end && leftOfAxis(leaves[begin], org, axis)) { ++begin; } - if(begin==end) + if (begin == end) { break; } - while(begin!=end && !leftOfAxis(leaves[end-1],org,axis)) + while (begin != end && !leftOfAxis(leaves[end - 1], org, axis)) { --end; } - if(begin==end) + if (begin == end) { break; } // swap out of place nodes --end; - btDbvtNode* temp=leaves[begin]; - leaves[begin]=leaves[end]; - leaves[end]=temp; + btDbvtNode* temp = leaves[begin]; + leaves[begin] = leaves[end]; + leaves[end] = temp; ++begin; } @@ -281,150 +296,153 @@ static int split( btDbvtNode** leaves, } // -static btDbvtVolume bounds( btDbvtNode** leaves, - int count) +static btDbvtVolume bounds(btDbvtNode** leaves, + int count) { -#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE - ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); - btDbvtVolume* ptr = (btDbvtVolume*) locals; - btDbvtVolume& volume=*ptr; - volume=leaves[0]->volume; +#ifdef BT_USE_SSE + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); + btDbvtVolume* ptr = (btDbvtVolume*)locals; + btDbvtVolume& volume = *ptr; + volume = leaves[0]->volume; #else - btDbvtVolume volume=leaves[0]->volume; + btDbvtVolume volume = leaves[0]->volume; #endif - for(int i=1,ni=count;ivolume,volume); + Merge(volume, leaves[i]->volume, volume); } - return(volume); + return (volume); } // -static void bottomup( btDbvt* pdbvt, - btDbvtNode** leaves, - int count) +static void bottomup(btDbvt* pdbvt, + btDbvtNode** leaves, + int count) { - while(count>1) + while (count > 1) { - btScalar minsize=SIMD_INFINITY; - int minidx[2]={-1,-1}; - for(int i=0;ivolume,leaves[j]->volume)); - if(szvolume, leaves[j]->volume)); + if (sz < minsize) { - minsize = sz; - minidx[0] = i; - minidx[1] = j; + minsize = sz; + minidx[0] = i; + minidx[1] = j; } } } - btDbvtNode* n[] = {leaves[minidx[0]],leaves[minidx[1]]}; - btDbvtNode* p = createnode(pdbvt,0,n[0]->volume,n[1]->volume,0); - p->childs[0] = n[0]; - p->childs[1] = n[1]; - n[0]->parent = p; - n[1]->parent = p; - leaves[minidx[0]] = p; - leaves[minidx[1]] = leaves[count-1]; + btDbvtNode* n[] = {leaves[minidx[0]], leaves[minidx[1]]}; + btDbvtNode* p = createnode(pdbvt, 0, n[0]->volume, n[1]->volume, 0); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leaves[minidx[0]] = p; + leaves[minidx[1]] = leaves[count - 1]; --count; } } // -static btDbvtNode* topdown(btDbvt* pdbvt, - btDbvtNode** leaves, - int count, - int bu_treshold) +static btDbvtNode* topdown(btDbvt* pdbvt, + btDbvtNode** leaves, + int count, + int bu_treshold) { - static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; - btAssert(bu_treshold>2); - if(count>1) + static const btVector3 axis[] = {btVector3(1, 0, 0), + btVector3(0, 1, 0), + btVector3(0, 0, 1)}; + btAssert(bu_treshold > 2); + if (count > 1) { - if(count>bu_treshold) + if (count > bu_treshold) { - const btDbvtVolume vol=bounds(leaves,count); - const btVector3 org=vol.Center(); - int partition; - int bestaxis=-1; - int bestmidp=count; - int splitcount[3][2]={{0,0},{0,0},{0,0}}; + const btDbvtVolume vol = bounds(leaves, count); + const btVector3 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) + const btVector3 x = leaves[i]->volume.Center() - org; + for (int j = 0; j < 3; ++j) { - ++splitcount[j][btDot(x,axis[j])>0?1:0]; + ++splitcount[j][btDot(x, axis[j]) > 0 ? 1 : 0]; } } - for( i=0;i<3;++i) + for (i = 0; i < 3; ++i) { - if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) + if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { - const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); - if(midp=0) + if (bestaxis >= 0) { - partition=split(leaves,count,org,axis[bestaxis]); - btAssert(partition!=0 && partition!=count); + partition = split(leaves, count, org, axis[bestaxis]); + btAssert(partition != 0 && partition != count); } else { - partition=count/2+1; + partition = count / 2 + 1; } - btDbvtNode* node=createnode(pdbvt,0,vol,0); - 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); + btDbvtNode* node = createnode(pdbvt, 0, vol, 0); + 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,count); - return(leaves[0]); + bottomup(pdbvt, leaves, count); + return (leaves[0]); } } - return(leaves[0]); + return (leaves[0]); } // -static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) +static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n, btDbvtNode*& r) { - btDbvtNode* p=n->parent; + btDbvtNode* p = n->parent; btAssert(n->isinternal()); - if(p>n) + if (p > n) { - const int i=indexof(n); - const int j=1-i; - btDbvtNode* s=p->childs[j]; - btDbvtNode* q=p->parent; - btAssert(n==p->childs[i]); - if(q) q->childs[indexof(p)]=n; else r=n; - s->parent=n; - p->parent=n; - n->parent=q; - p->childs[0]=n->childs[0]; - p->childs[1]=n->childs[1]; - n->childs[0]->parent=p; - n->childs[1]->parent=p; - n->childs[i]=p; - n->childs[j]=s; - btSwap(p->volume,n->volume); - return(p); + const int i = indexof(n); + const int j = 1 - i; + btDbvtNode* s = p->childs[j]; + btDbvtNode* q = p->parent; + btAssert(n == p->childs[i]); + if (q) + q->childs[indexof(p)] = n; + else + r = n; + s->parent = n; + p->parent = n; + n->parent = q; + p->childs[0] = n->childs[0]; + p->childs[1] = n->childs[1]; + n->childs[0]->parent = p; + n->childs[1]->parent = p; + n->childs[i] = p; + n->childs[j] = s; + btSwap(p->volume, n->volume); + return (p); } - return(n); + return (n); } #if 0 @@ -442,11 +460,11 @@ static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) // btDbvt::btDbvt() { - m_root = 0; - m_free = 0; - m_lkhd = -1; - m_leaves = 0; - m_opath = 0; + m_root = 0; + m_free = 0; + m_lkhd = -1; + m_leaves = 0; + m_opath = 0; } // @@ -456,228 +474,233 @@ btDbvt::~btDbvt() } // -void btDbvt::clear() +void btDbvt::clear() { - if(m_root) - recursedeletenode(this,m_root); + if (m_root) + recursedeletenode(this, m_root); btAlignedFree(m_free); - m_free=0; - m_lkhd = -1; + m_free = 0; + m_lkhd = -1; m_stkStack.clear(); - m_opath = 0; - + m_opath = 0; } // -void btDbvt::optimizeBottomUp() +void btDbvt::optimizeBottomUp() { - if(m_root) + if (m_root) { tNodeArray leaves; leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - bottomup(this,&leaves[0],leaves.size()); - m_root=leaves[0]; + fetchleaves(this, m_root, leaves); + bottomup(this, &leaves[0], leaves.size()); + m_root = leaves[0]; } } // -void btDbvt::optimizeTopDown(int bu_treshold) +void btDbvt::optimizeTopDown(int bu_treshold) { - if(m_root) + if (m_root) { - tNodeArray leaves; + tNodeArray leaves; leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - m_root=topdown(this,&leaves[0],leaves.size(),bu_treshold); + fetchleaves(this, m_root, leaves); + m_root = topdown(this, &leaves[0], leaves.size(), bu_treshold); } } // -void btDbvt::optimizeIncremental(int passes) +void btDbvt::optimizeIncremental(int passes) { - if(passes<0) passes=m_leaves; - if(m_root&&(passes>0)) + if (passes < 0) passes = m_leaves; + if (m_root && (passes > 0)) { - do { - btDbvtNode* node=m_root; - unsigned bit=0; - while(node->isinternal()) + do + { + btDbvtNode* node = m_root; + unsigned bit = 0; + while (node->isinternal()) { - node=sort(node,m_root)->childs[(m_opath>>bit)&1]; - bit=(bit+1)&(sizeof(unsigned)*8-1); + node = sort(node, m_root)->childs[(m_opath >> bit) & 1]; + bit = (bit + 1) & (sizeof(unsigned) * 8 - 1); } update(node); ++m_opath; - } while(--passes); + } while (--passes); } } // -btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) +btDbvtNode* btDbvt::insert(const btDbvtVolume& volume, void* data) { - btDbvtNode* leaf=createnode(this,0,volume,data); - insertleaf(this,m_root,leaf); + btDbvtNode* leaf = createnode(this, 0, volume, data); + insertleaf(this, m_root, leaf); ++m_leaves; - return(leaf); + return (leaf); } // -void btDbvt::update(btDbvtNode* leaf,int lookahead) +void btDbvt::update(btDbvtNode* leaf, int lookahead) { - btDbvtNode* root=removeleaf(this,leaf); - if(root) + btDbvtNode* root = removeleaf(this, leaf); + if (root) { - if(lookahead>=0) + if (lookahead >= 0) { - for(int i=0;(iparent;++i) + for (int i = 0; (i < lookahead) && root->parent; ++i) { - root=root->parent; + root = root->parent; } - } else root=m_root; + } + else + root = m_root; } - insertleaf(this,root,leaf); + insertleaf(this, root, leaf); } // -void btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume) +void btDbvt::update(btDbvtNode* leaf, btDbvtVolume& volume) { - btDbvtNode* root=removeleaf(this,leaf); - if(root) + btDbvtNode* root = removeleaf(this, leaf); + if (root) { - if(m_lkhd>=0) + if (m_lkhd >= 0) { - for(int i=0;(iparent;++i) + for (int i = 0; (i < m_lkhd) && root->parent; ++i) { - root=root->parent; + root = root->parent; } - } else root=m_root; + } + else + root = m_root; } - leaf->volume=volume; - insertleaf(this,root,leaf); + leaf->volume = volume; + insertleaf(this, root, leaf); } // -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin) +bool btDbvt::update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity, btScalar margin) { - if(leaf->volume.Contain(volume)) return(false); - volume.Expand(btVector3(margin,margin,margin)); + if (leaf->volume.Contain(volume)) return (false); + volume.Expand(btVector3(margin, margin, margin)); volume.SignedExpand(velocity); - update(leaf,volume); - return(true); + update(leaf, volume); + return (true); } // -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity) +bool btDbvt::update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity) { - if(leaf->volume.Contain(volume)) return(false); + if (leaf->volume.Contain(volume)) return (false); volume.SignedExpand(velocity); - update(leaf,volume); - return(true); + update(leaf, volume); + return (true); } // -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin) +bool btDbvt::update(btDbvtNode* leaf, btDbvtVolume& volume, btScalar margin) { - if(leaf->volume.Contain(volume)) return(false); - volume.Expand(btVector3(margin,margin,margin)); - update(leaf,volume); - return(true); + if (leaf->volume.Contain(volume)) return (false); + volume.Expand(btVector3(margin, margin, margin)); + update(leaf, volume); + return (true); } // -void btDbvt::remove(btDbvtNode* leaf) +void btDbvt::remove(btDbvtNode* leaf) { - removeleaf(this,leaf); - deletenode(this,leaf); + removeleaf(this, leaf); + deletenode(this, leaf); --m_leaves; } // -void btDbvt::write(IWriter* iwriter) const +void btDbvt::write(IWriter* iwriter) const { - btDbvtNodeEnumerator nodes; - nodes.nodes.reserve(m_leaves*2); - enumNodes(m_root,nodes); - iwriter->Prepare(m_root,nodes.nodes.size()); - for(int i=0;iPrepare(m_root, nodes.nodes.size()); + for (int i = 0; i < nodes.nodes.size(); ++i) { - const btDbvtNode* n=nodes.nodes[i]; - int p=-1; - if(n->parent) p=nodes.nodes.findLinearSearch(n->parent); - if(n->isinternal()) + const btDbvtNode* n = nodes.nodes[i]; + int p = -1; + if (n->parent) p = nodes.nodes.findLinearSearch(n->parent); + if (n->isinternal()) { - const int c0=nodes.nodes.findLinearSearch(n->childs[0]); - const int c1=nodes.nodes.findLinearSearch(n->childs[1]); - iwriter->WriteNode(n,i,p,c0,c1); + const int c0 = nodes.nodes.findLinearSearch(n->childs[0]); + const int c1 = nodes.nodes.findLinearSearch(n->childs[1]); + iwriter->WriteNode(n, i, p, c0, c1); } else { - iwriter->WriteLeaf(n,i,p); - } + iwriter->WriteLeaf(n, i, p); + } } } // -void btDbvt::clone(btDbvt& dest,IClone* iclone) const +void btDbvt::clone(btDbvt& dest, IClone* iclone) const { dest.clear(); - if(m_root!=0) - { - btAlignedObjectArray stack; + if (m_root != 0) + { + btAlignedObjectArray stack; stack.reserve(m_leaves); - stack.push_back(sStkCLN(m_root,0)); - do { - const int i=stack.size()-1; - const sStkCLN e=stack[i]; - btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); + stack.push_back(sStkCLN(m_root, 0)); + do + { + const int i = stack.size() - 1; + const sStkCLN e = stack[i]; + btDbvtNode* n = createnode(&dest, e.parent, e.node->volume, e.node->data); stack.pop_back(); - if(e.parent!=0) - e.parent->childs[i&1]=n; + if (e.parent != 0) + e.parent->childs[i & 1] = n; else - dest.m_root=n; - if(e.node->isinternal()) + dest.m_root = n; + if (e.node->isinternal()) { - stack.push_back(sStkCLN(e.node->childs[0],n)); - stack.push_back(sStkCLN(e.node->childs[1],n)); + stack.push_back(sStkCLN(e.node->childs[0], n)); + stack.push_back(sStkCLN(e.node->childs[1], n)); } else { iclone->CloneLeaf(n); } - } while(stack.size()>0); + } while (stack.size() > 0); } } // -int btDbvt::maxdepth(const btDbvtNode* node) +int btDbvt::maxdepth(const btDbvtNode* node) { - int depth=0; - if(node) getmaxdepth(node,1,depth); - return(depth); + int depth = 0; + if (node) getmaxdepth(node, 1, depth); + return (depth); } // -int btDbvt::countLeaves(const btDbvtNode* node) +int btDbvt::countLeaves(const btDbvtNode* node) { - if(node->isinternal()) - return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); + if (node->isinternal()) + return (countLeaves(node->childs[0]) + countLeaves(node->childs[1])); else - return(1); + return (1); } // -void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves) +void btDbvt::extractLeaves(const btDbvtNode* node, btAlignedObjectArray& leaves) { - if(node->isinternal()) + if (node->isinternal()) { - extractLeaves(node->childs[0],leaves); - extractLeaves(node->childs[1],leaves); + extractLeaves(node->childs[0], leaves); + extractLeaves(node->childs[1], leaves); } else { leaves.push_back(node); - } + } } // @@ -726,603 +749,608 @@ struct btDbvtBenchmark { struct NilPolicy : btDbvt::ICollide { - NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {} - void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; } - void Process(const btDbvtNode*) { ++m_pcount; } - void Process(const btDbvtNode*,btScalar depth) + NilPolicy() : m_pcount(0), m_depth(-SIMD_INFINITY), m_checksort(true) {} + void Process(const btDbvtNode*, const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*, btScalar depth) { ++m_pcount; - if(m_checksort) - { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); } + if (m_checksort) + { + if (depth >= m_depth) + m_depth = depth; + else + printf("wrong depth: %f (should be >= %f)\r\n", depth, m_depth); + } } - int m_pcount; - btScalar m_depth; - bool m_checksort; + int m_pcount; + btScalar m_depth; + bool m_checksort; }; struct P14 : btDbvt::ICollide { struct Node { - const btDbvtNode* leaf; - btScalar depth; + const btDbvtNode* leaf; + btScalar depth; }; - void Process(const btDbvtNode* leaf,btScalar depth) + void Process(const btDbvtNode* leaf, btScalar depth) { - Node n; - n.leaf = leaf; - n.depth = depth; + Node n; + n.leaf = leaf; + n.depth = depth; } - static int sortfnc(const Node& a,const Node& b) + static int sortfnc(const Node& a, const Node& b) { - if(a.depthb.depth) return(-1); - return(0); + if (a.depth < b.depth) return (+1); + if (a.depth > b.depth) return (-1); + return (0); } - btAlignedObjectArray m_nodes; + btAlignedObjectArray m_nodes; }; struct P15 : btDbvt::ICollide { struct Node { - const btDbvtNode* leaf; - btScalar depth; + const btDbvtNode* leaf; + btScalar depth; }; void Process(const btDbvtNode* leaf) { - Node n; - n.leaf = leaf; - n.depth = dot(leaf->volume.Center(),m_axis); + Node n; + n.leaf = leaf; + n.depth = dot(leaf->volume.Center(), m_axis); } - static int sortfnc(const Node& a,const Node& b) + static int sortfnc(const Node& a, const Node& b) { - if(a.depthb.depth) return(-1); - return(0); + if (a.depth < b.depth) return (+1); + if (a.depth > b.depth) return (-1); + return (0); } - btAlignedObjectArray m_nodes; - btVector3 m_axis; + btAlignedObjectArray m_nodes; + btVector3 m_axis; }; - static btScalar RandUnit() + static btScalar RandUnit() { - return(rand()/(btScalar)RAND_MAX); + return (rand() / (btScalar)RAND_MAX); } - static btVector3 RandVector3() + static btVector3 RandVector3() { - return(btVector3(RandUnit(),RandUnit(),RandUnit())); + return (btVector3(RandUnit(), RandUnit(), RandUnit())); } - static btVector3 RandVector3(btScalar cs) + static btVector3 RandVector3(btScalar cs) { - return(RandVector3()*cs-btVector3(cs,cs,cs)/2); + return (RandVector3() * cs - btVector3(cs, cs, cs) / 2); } - static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) + static btDbvtVolume RandVolume(btScalar cs, btScalar eb, btScalar es) { - return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); + return (btDbvtVolume::FromCE(RandVector3(cs), btVector3(eb, eb, eb) + RandVector3() * es)); } - static btTransform RandTransform(btScalar cs) + static btTransform RandTransform(btScalar cs) { - btTransform t; + btTransform t; t.setOrigin(RandVector3(cs)); - t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); - return(t); + t.setRotation(btQuaternion(RandUnit() * SIMD_PI * 2, RandUnit() * SIMD_PI * 2, RandUnit() * SIMD_PI * 2).normalized()); + return (t); } - static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt) + static void RandTree(btScalar cs, btScalar eb, btScalar es, int leaves, btDbvt& dbvt) { dbvt.clear(); - for(int i=0;i volumes; - btAlignedObjectArray results; + btAlignedObjectArray volumes; + btAlignedObjectArray results; volumes.resize(cfgLeaves); results.resize(cfgLeaves); - for(int i=0;i volumes; - btAlignedObjectArray results; + btAlignedObjectArray volumes; + btAlignedObjectArray results; volumes.resize(cfgLeaves); results.resize(cfgLeaves); - for(int i=0;i transforms; - btDbvtBenchmark::NilPolicy policy; + btDbvt dbvt[2]; + btAlignedObjectArray transforms; + btDbvtBenchmark::NilPolicy policy; transforms.resize(cfgBenchmark5_Iterations); - for(int i=0;i transforms; - btDbvtBenchmark::NilPolicy policy; + btDbvt dbvt; + btAlignedObjectArray transforms; + btDbvtBenchmark::NilPolicy policy; transforms.resize(cfgBenchmark6_Iterations); - for(int i=0;i rayorg; - btAlignedObjectArray raydir; - btDbvtBenchmark::NilPolicy policy; + btDbvt dbvt; + btAlignedObjectArray rayorg; + btAlignedObjectArray raydir; + btDbvtBenchmark::NilPolicy policy; rayorg.resize(cfgBenchmark7_Iterations); raydir.resize(cfgBenchmark7_Iterations); - for(int i=0;i leaves; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + btDbvt dbvt; + btAlignedObjectArray leaves; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); dbvt.optimizeTopDown(); - dbvt.extractLeaves(dbvt.m_root,leaves); + dbvt.extractLeaves(dbvt.m_root, leaves); printf("[9] updates (teleport): "); wallclock.reset(); - for(int i=0;i(leaves[rand()%cfgLeaves]), - btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); + dbvt.update(const_cast(leaves[rand() % cfgLeaves]), + btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale)); } } - const int time=(int)wallclock.getTimeMilliseconds(); - const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations; - printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time); + const int time = (int)wallclock.getTimeMilliseconds(); + const int up = cfgBenchmark9_Passes * cfgBenchmark9_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n", time, (time - cfgBenchmark9_Reference) * 100 / time, up * 1000 / time); } - if(cfgBenchmark10_Enable) - {// Benchmark 10 + if (cfgBenchmark10_Enable) + { // Benchmark 10 srand(380843); - btDbvt dbvt; - btAlignedObjectArray leaves; - btAlignedObjectArray vectors; + btDbvt dbvt; + btAlignedObjectArray leaves; + btAlignedObjectArray vectors; vectors.resize(cfgBenchmark10_Iterations); - for(int i=0;i(leaves[rand()%cfgLeaves]); - btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); - dbvt.update(l,v); + for (int j = 0; j < cfgBenchmark10_Iterations; ++j) + { + const btVector3& d = vectors[j]; + btDbvtNode* l = const_cast(leaves[rand() % cfgLeaves]); + btDbvtVolume v = btDbvtVolume::FromMM(l->volume.Mins() + d, l->volume.Maxs() + d); + dbvt.update(l, v); } } - const int time=(int)wallclock.getTimeMilliseconds(); - const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations; - printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time); + const int time = (int)wallclock.getTimeMilliseconds(); + const int up = cfgBenchmark10_Passes * cfgBenchmark10_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n", time, (time - cfgBenchmark10_Reference) * 100 / time, up * 1000 / time); } - if(cfgBenchmark11_Enable) - {// Benchmark 11 + if (cfgBenchmark11_Enable) + { // Benchmark 11 srand(380843); - btDbvt dbvt; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + btDbvt dbvt; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); dbvt.optimizeTopDown(); printf("[11] optimize (incremental): "); - wallclock.reset(); - for(int i=0;i volumes; - btAlignedObjectArray results; + btAlignedObjectArray volumes; + btAlignedObjectArray results; volumes.resize(cfgLeaves); results.resize(cfgLeaves); - for(int i=0;i vectors; - btDbvtBenchmark::NilPolicy policy; + btDbvt dbvt; + btAlignedObjectArray vectors; + btDbvtBenchmark::NilPolicy policy; vectors.resize(cfgBenchmark13_Iterations); - for(int i=0;i vectors; - btDbvtBenchmark::P14 policy; + btDbvt dbvt; + btAlignedObjectArray vectors; + btDbvtBenchmark::P14 policy; vectors.resize(cfgBenchmark14_Iterations); - for(int i=0;i vectors; - btDbvtBenchmark::P15 policy; + btDbvt dbvt; + btAlignedObjectArray vectors; + btDbvtBenchmark::P15 policy; vectors.resize(cfgBenchmark15_Iterations); - for(int i=0;i batch; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + btDbvt dbvt; + btAlignedObjectArray batch; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt); dbvt.optimizeTopDown(); batch.reserve(cfgBenchmark16_BatchCount); - printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); + printf("[16] insert/remove batch(%u): ", cfgBenchmark16_BatchCount); wallclock.reset(); - for(int i=0;i volumes; - btAlignedObjectArray results; - btAlignedObjectArray indices; + btAlignedObjectArray volumes; + btAlignedObjectArray results; + btAlignedObjectArray indices; volumes.resize(cfgLeaves); results.resize(cfgLeaves); indices.resize(cfgLeaves); - for(int i=0;i= 1400) -#define DBVT_USE_TEMPLATE 1 +#if (defined(_MSC_VER) && _MSC_VER >= 1400) +#define DBVT_USE_TEMPLATE 1 #else -#define DBVT_USE_TEMPLATE 0 +#define DBVT_USE_TEMPLATE 0 #endif #else -#define DBVT_USE_TEMPLATE 0 +#define DBVT_USE_TEMPLATE 0 #endif // Use only intrinsics instead of inline asm -#define DBVT_USE_INTRINSIC_SSE 1 +#define DBVT_USE_INTRINSIC_SSE 1 // Using memmov for collideOCL -#define DBVT_USE_MEMMOVE 1 +#define DBVT_USE_MEMMOVE 1 // Enable benchmarking code -#define DBVT_ENABLE_BENCHMARK 0 +#define DBVT_ENABLE_BENCHMARK 0 // Inlining -#define DBVT_INLINE SIMD_FORCE_INLINE +#define DBVT_INLINE SIMD_FORCE_INLINE // Specific methods implementation //SSE gives errors on a MSVC 7.1 -#if defined (BT_USE_SSE) //&& defined (_WIN32) -#define DBVT_SELECT_IMPL DBVT_IMPL_SSE -#define DBVT_MERGE_IMPL DBVT_IMPL_SSE -#define DBVT_INT0_IMPL DBVT_IMPL_SSE +#if defined(BT_USE_SSE) //&& defined (_WIN32) +#define DBVT_SELECT_IMPL DBVT_IMPL_SSE +#define DBVT_MERGE_IMPL DBVT_IMPL_SSE +#define DBVT_INT0_IMPL DBVT_IMPL_SSE #else -#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC -#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC -#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC +#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC +#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC +#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC #endif -#if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \ - (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \ - (DBVT_INT0_IMPL==DBVT_IMPL_SSE) +#if (DBVT_SELECT_IMPL == DBVT_IMPL_SSE) || \ + (DBVT_MERGE_IMPL == DBVT_IMPL_SSE) || \ + (DBVT_INT0_IMPL == DBVT_IMPL_SSE) #include #endif @@ -78,21 +77,24 @@ subject to the following restrictions: // #if DBVT_USE_TEMPLATE -#define DBVT_VIRTUAL +#define DBVT_VIRTUAL #define DBVT_VIRTUAL_DTOR(a) -#define DBVT_PREFIX template -#define DBVT_IPOLICY T& policy -#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker; +#define DBVT_PREFIX template +#define DBVT_IPOLICY T& policy +#define DBVT_CHECKTYPE \ + static const ICollide& typechecker = *(T*)1; \ + (void)typechecker; #else -#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} -#define DBVT_VIRTUAL virtual +#define DBVT_VIRTUAL_DTOR(a) \ + virtual ~a() {} +#define DBVT_VIRTUAL virtual #define DBVT_PREFIX -#define DBVT_IPOLICY ICollide& policy +#define DBVT_IPOLICY ICollide& policy #define DBVT_CHECKTYPE #endif #if DBVT_USE_MEMMOVE -#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__) #include #endif #include @@ -122,194 +124,193 @@ subject to the following restrictions: #error "DBVT_INT0_IMPL undefined" #endif - // // Defaults volumes // -/* btDbvtAabbMm */ -struct btDbvtAabbMm +/* btDbvtAabbMm */ +struct btDbvtAabbMm { - DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } - DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } - DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } - DBVT_INLINE const btVector3& Mins() const { return(mi); } - DBVT_INLINE const btVector3& Maxs() const { return(mx); } - static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); - static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); - static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); - static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); - static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); - DBVT_INLINE void Expand(const btVector3& e); - DBVT_INLINE void SignedExpand(const btVector3& e); - DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; - DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; - DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; - DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - - DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btVector3& b); + DBVT_INLINE btVector3 Center() const { return ((mi + mx) / 2); } + DBVT_INLINE btVector3 Lengths() const { return (mx - mi); } + DBVT_INLINE btVector3 Extents() const { return ((mx - mi) / 2); } + DBVT_INLINE const btVector3& Mins() const { return (mi); } + DBVT_INLINE const btVector3& Maxs() const { return (mx); } + static inline btDbvtAabbMm FromCE(const btVector3& c, const btVector3& e); + static inline btDbvtAabbMm FromCR(const btVector3& c, btScalar r); + static inline btDbvtAabbMm FromMM(const btVector3& mi, const btVector3& mx); + static inline btDbvtAabbMm FromPoints(const btVector3* pts, int n); + static inline btDbvtAabbMm FromPoints(const btVector3** ppts, int n); + DBVT_INLINE void Expand(const btVector3& e); + DBVT_INLINE void SignedExpand(const btVector3& e); + DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; + DBVT_INLINE int Classify(const btVector3& n, btScalar o, int s) const; + DBVT_INLINE btScalar ProjectMinimum(const btVector3& v, unsigned signs) const; + DBVT_INLINE friend bool Intersect(const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + + DBVT_INLINE friend bool Intersect(const btDbvtAabbMm& a, + const btVector3& b); + + DBVT_INLINE friend btScalar Proximity(const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend int Select(const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend void Merge(const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r); + DBVT_INLINE friend bool NotEqual(const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + + DBVT_INLINE btVector3& tMins() { return (mi); } + DBVT_INLINE btVector3& tMaxs() { return (mx); } - DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - DBVT_INLINE friend int Select( const btDbvtAabbMm& o, - const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - btDbvtAabbMm& r); - DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - - DBVT_INLINE btVector3& tMins() { return(mi); } - DBVT_INLINE btVector3& tMaxs() { return(mx); } - private: - DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; + DBVT_INLINE void AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const; + private: - btVector3 mi,mx; + btVector3 mi, mx; }; -// Types -typedef btDbvtAabbMm btDbvtVolume; +// Types +typedef btDbvtAabbMm btDbvtVolume; -/* btDbvtNode */ -struct btDbvtNode +/* btDbvtNode */ +struct btDbvtNode { - btDbvtVolume volume; - btDbvtNode* parent; - DBVT_INLINE bool isleaf() const { return(childs[1]==0); } - DBVT_INLINE bool isinternal() const { return(!isleaf()); } - union - { - btDbvtNode* childs[2]; - void* data; - int dataAsInt; + btDbvtVolume volume; + btDbvtNode* parent; + DBVT_INLINE bool isleaf() const { return (childs[1] == 0); } + DBVT_INLINE bool isinternal() const { return (!isleaf()); } + union { + btDbvtNode* childs[2]; + void* data; + int dataAsInt; }; }; 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. -struct btDbvt +struct btDbvt { - /* Stack element */ - struct sStkNN + /* Stack element */ + struct sStkNN { - const btDbvtNode* a; - const btDbvtNode* b; + const btDbvtNode* a; + const btDbvtNode* b; sStkNN() {} - sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} + sStkNN(const btDbvtNode* na, const btDbvtNode* nb) : a(na), b(nb) {} }; - struct sStkNP + struct sStkNP { - const btDbvtNode* node; - int mask; - sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} + const btDbvtNode* node; + int mask; + sStkNP(const btDbvtNode* n, unsigned m) : node(n), mask(m) {} }; - struct sStkNPS + struct sStkNPS { - const btDbvtNode* node; - int mask; - btScalar value; + const btDbvtNode* node; + int mask; + btScalar value; sStkNPS() {} - sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} + sStkNPS(const btDbvtNode* n, unsigned m, btScalar v) : node(n), mask(m), value(v) {} }; - struct sStkCLN + struct sStkCLN { - const btDbvtNode* node; - btDbvtNode* parent; - sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} + const btDbvtNode* node; + btDbvtNode* parent; + sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {} }; // Policies/Interfaces - /* ICollide */ - struct ICollide - { + /* ICollide */ + struct ICollide + { DBVT_VIRTUAL_DTOR(ICollide) - DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} - DBVT_VIRTUAL void Process(const btDbvtNode*) {} - DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } - DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } - DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } + DBVT_VIRTUAL void Process(const btDbvtNode*, const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode* n, btScalar) { Process(n); } + DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); } + DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); } }; - /* IWriter */ - struct IWriter + /* IWriter */ + struct IWriter { virtual ~IWriter() {} - virtual void Prepare(const btDbvtNode* root,int numnodes)=0; - virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; - virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; + virtual void Prepare(const btDbvtNode* root, int numnodes) = 0; + virtual void WriteNode(const btDbvtNode*, int index, int parent, int child0, int child1) = 0; + virtual void WriteLeaf(const btDbvtNode*, int index, int parent) = 0; }; - /* IClone */ - struct IClone + /* IClone */ + struct IClone { - virtual ~IClone() {} - virtual void CloneLeaf(btDbvtNode*) {} + virtual ~IClone() {} + virtual void CloneLeaf(btDbvtNode*) {} }; // Constants - enum { - SIMPLE_STACKSIZE = 64, - DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 + enum + { + SIMPLE_STACKSIZE = 64, + DOUBLE_STACKSIZE = SIMPLE_STACKSIZE * 2 }; // Fields - btDbvtNode* m_root; - btDbvtNode* m_free; - int m_lkhd; - int m_leaves; - unsigned m_opath; - - - btAlignedObjectArray m_stkStack; + btDbvtNode* m_root; + btDbvtNode* m_free; + int m_lkhd; + int m_leaves; + unsigned m_opath; + btAlignedObjectArray m_stkStack; // Methods btDbvt(); ~btDbvt(); - void clear(); - bool empty() const { return(0==m_root); } - void optimizeBottomUp(); - void optimizeTopDown(int bu_treshold=128); - void optimizeIncremental(int passes); - btDbvtNode* insert(const btDbvtVolume& box,void* data); - void update(btDbvtNode* leaf,int lookahead=-1); - void update(btDbvtNode* leaf,btDbvtVolume& volume); - bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin); - bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity); - bool update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin); - void remove(btDbvtNode* leaf); - void write(IWriter* iwriter) const; - void clone(btDbvt& dest,IClone* iclone=0) const; - static int maxdepth(const btDbvtNode* node); - static int countLeaves(const btDbvtNode* node); - static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves); + void clear(); + bool empty() const { return (0 == m_root); } + void optimizeBottomUp(); + void optimizeTopDown(int bu_treshold = 128); + void optimizeIncremental(int passes); + btDbvtNode* insert(const btDbvtVolume& box, void* data); + void update(btDbvtNode* leaf, int lookahead = -1); + void update(btDbvtNode* leaf, btDbvtVolume& volume); + bool update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity, btScalar margin); + bool update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity); + bool update(btDbvtNode* leaf, btDbvtVolume& volume, btScalar margin); + void remove(btDbvtNode* leaf); + void write(IWriter* iwriter) const; + void clone(btDbvt& dest, IClone* iclone = 0) const; + static int maxdepth(const btDbvtNode* node); + static int countLeaves(const btDbvtNode* node); + static void extractLeaves(const btDbvtNode* node, btAlignedObjectArray& leaves); #if DBVT_ENABLE_BENCHMARK - static void benchmark(); + static void benchmark(); #else - static void benchmark(){} + static void benchmark() + { + } #endif // DBVT_IPOLICY must support ICollide policy/interface DBVT_PREFIX - static void enumNodes( const btDbvtNode* root, - DBVT_IPOLICY); + static void enumNodes(const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX - static void enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY); + static void enumLeaves(const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX - void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); + void collideTT(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); DBVT_PREFIX - void collideTTpersistentStack( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); + void collideTTpersistentStack(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); #if 0 DBVT_PREFIX void collideTT( const btDbvtNode* root0, @@ -325,82 +326,89 @@ struct btDbvt #endif DBVT_PREFIX - void collideTV( const btDbvtNode* root, - const btDbvtVolume& volume, - DBVT_IPOLICY) const; - + 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; - - - - + 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 - static void rayTest( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - DBVT_IPOLICY); + static void rayTest(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY); ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts DBVT_PREFIX - void rayTestInternal( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayDirectionInverse, - unsigned int signs[3], - btScalar lambda_max, - const btVector3& aabbMin, - const btVector3& aabbMax, - btAlignedObjectArray& stack, - DBVT_IPOLICY) const; + void rayTestInternal(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayDirectionInverse, + unsigned int signs[3], + btScalar lambda_max, + const btVector3& aabbMin, + const btVector3& aabbMax, + btAlignedObjectArray& stack, + DBVT_IPOLICY) const; DBVT_PREFIX - static void collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY); + static void collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY); DBVT_PREFIX - static void collideOCL( const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - const btVector3& sortaxis, - int count, - DBVT_IPOLICY, - bool fullsort=true); + static void collideOCL(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fullsort = true); DBVT_PREFIX - static void collideTU( const btDbvtNode* root, - DBVT_IPOLICY); - // Helpers - static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) + static void collideTU(const btDbvtNode* root, + DBVT_IPOLICY); + // Helpers + static DBVT_INLINE int nearest(const int* i, const btDbvt::sStkNPS* a, btScalar v, int l, int h) { - int m=0; - while(l>1; - if(a[i[m]].value>=v) l=m+1; else h=m; + m = (l + h) >> 1; + if (a[i[m]].value >= v) + l = m + 1; + else + h = m; } - return(h); + return (h); } - static DBVT_INLINE int allocate( btAlignedObjectArray& ifree, - btAlignedObjectArray& stock, - const sStkNPS& value) + static DBVT_INLINE int allocate(btAlignedObjectArray& ifree, + btAlignedObjectArray& stock, + const sStkNPS& value) { - int i; - if(ifree.size()>0) - { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=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); + { + i = stock.size(); + stock.push_back(value); + } + return (i); } // private: - btDbvt(const btDbvt&) {} + btDbvt(const btDbvt&) {} }; // @@ -408,227 +416,252 @@ private: // // -inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) +inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c, const btVector3& e) { btDbvtAabbMm box; - box.mi=c-e;box.mx=c+e; - return(box); + box.mi = c - e; + box.mx = c + e; + return (box); } // -inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) +inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c, btScalar r) { - return(FromCE(c,btVector3(r,r,r))); + return (FromCE(c, btVector3(r, r, r))); } // -inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) +inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi, const btVector3& mx) { btDbvtAabbMm box; - box.mi=mi;box.mx=mx; - return(box); + box.mi = mi; + box.mx = mx; + return (box); } // -inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) +inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts, int n) { btDbvtAabbMm box; - box.mi=box.mx=pts[0]; - for(int i=1;i0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); - if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); - if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); + if (e.x() > 0) + mx.setX(mx.x() + e[0]); + else + mi.setX(mi.x() + e[0]); + if (e.y() > 0) + mx.setY(mx.y() + e[1]); + else + mi.setY(mi.y() + e[1]); + if (e.z() > 0) + mx.setZ(mx.z() + e[2]); + else + mi.setZ(mi.z() + e[2]); } // -DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const +DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const { - return( (mi.x()<=a.mi.x())&& - (mi.y()<=a.mi.y())&& - (mi.z()<=a.mi.z())&& - (mx.x()>=a.mx.x())&& - (mx.y()>=a.mx.y())&& - (mx.z()>=a.mx.z())); + return ((mi.x() <= a.mi.x()) && + (mi.y() <= a.mi.y()) && + (mi.z() <= a.mi.z()) && + (mx.x() >= a.mx.x()) && + (mx.y() >= a.mx.y()) && + (mx.z() >= a.mx.z())); } // -DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const +DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n, btScalar o, int s) const { - btVector3 pi,px; - switch(s) + btVector3 pi, px; + switch (s) { - case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); - pi=btVector3(mx.x(),mx.y(),mx.z());break; - case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); - pi=btVector3(mi.x(),mx.y(),mx.z());break; - case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); - pi=btVector3(mx.x(),mi.y(),mx.z());break; - case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); - pi=btVector3(mi.x(),mi.y(),mx.z());break; - case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); - pi=btVector3(mx.x(),mx.y(),mi.z());break; - case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); - pi=btVector3(mi.x(),mx.y(),mi.z());break; - case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); - pi=btVector3(mx.x(),mi.y(),mi.z());break; - case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); - pi=btVector3(mi.x(),mi.y(),mi.z());break; + case (0 + 0 + 0): + px = btVector3(mi.x(), mi.y(), mi.z()); + pi = btVector3(mx.x(), mx.y(), mx.z()); + break; + case (1 + 0 + 0): + px = btVector3(mx.x(), mi.y(), mi.z()); + pi = btVector3(mi.x(), mx.y(), mx.z()); + break; + case (0 + 2 + 0): + px = btVector3(mi.x(), mx.y(), mi.z()); + pi = btVector3(mx.x(), mi.y(), mx.z()); + break; + case (1 + 2 + 0): + px = btVector3(mx.x(), mx.y(), mi.z()); + pi = btVector3(mi.x(), mi.y(), mx.z()); + break; + case (0 + 0 + 4): + px = btVector3(mi.x(), mi.y(), mx.z()); + pi = btVector3(mx.x(), mx.y(), mi.z()); + break; + case (1 + 0 + 4): + px = btVector3(mx.x(), mi.y(), mx.z()); + pi = btVector3(mi.x(), mx.y(), mi.z()); + break; + case (0 + 2 + 4): + px = btVector3(mi.x(), mx.y(), mx.z()); + pi = btVector3(mx.x(), mi.y(), mi.z()); + break; + case (1 + 2 + 4): + px = btVector3(mx.x(), mx.y(), mx.z()); + pi = btVector3(mi.x(), mi.y(), mi.z()); + break; } - if((btDot(n,px)+o)<0) return(-1); - if((btDot(n,pi)+o)>=0) return(+1); - return(0); + if ((btDot(n, px) + o) < 0) return (-1); + if ((btDot(n, pi) + o) >= 0) return (+1); + return (0); } // -DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const +DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v, unsigned signs) const { - const btVector3* b[]={&mx,&mi}; - const btVector3 p( b[(signs>>0)&1]->x(), - b[(signs>>1)&1]->y(), - b[(signs>>2)&1]->z()); - return(btDot(p,v)); + const btVector3* b[] = {&mx, &mi}; + const btVector3 p(b[(signs >> 0) & 1]->x(), + b[(signs >> 1) & 1]->y(), + b[(signs >> 2) & 1]->z()); + return (btDot(p, v)); } // -DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const +DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const { - for(int i=0;i<3;++i) + for (int i = 0; i < 3; ++i) { - if(d[i]<0) - { smi+=mx[i]*d[i];smx+=mi[i]*d[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]; } + { + smi += mi[i] * d[i]; + smx += mx[i] * d[i]; + } } } // -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) +DBVT_INLINE bool Intersect(const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { -#if DBVT_INT0_IMPL == DBVT_IMPL_SSE - const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), - _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); -#if defined (_WIN32) - const __int32* pu((const __int32*)&rt); +#if DBVT_INT0_IMPL == DBVT_IMPL_SSE + const __m128 rt(_mm_or_ps(_mm_cmplt_ps(_mm_load_ps(b.mx), _mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx), _mm_load_ps(b.mi)))); +#if defined(_WIN32) + const __int32* pu((const __int32*)&rt); #else - const int* pu((const int*)&rt); + const int* pu((const int*)&rt); #endif - return((pu[0]|pu[1]|pu[2])==0); + 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())); + 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 } - - // -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btVector3& b) +DBVT_INLINE bool Intersect(const btDbvtAabbMm& a, + const btVector3& b) { - return( (b.x()>=a.mi.x())&& - (b.y()>=a.mi.y())&& - (b.z()>=a.mi.z())&& - (b.x()<=a.mx.x())&& - (b.y()<=a.mx.y())&& - (b.z()<=a.mx.z())); + return ((b.x() >= a.mi.x()) && + (b.y() >= a.mi.y()) && + (b.z() >= a.mi.z()) && + (b.x() <= a.mx.x()) && + (b.y() <= a.mx.y()) && + (b.z() <= a.mx.z())); } - - - - ////////////////////////////////////// - // -DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) -{ - const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); - return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); -} - - - -// -DBVT_INLINE int Select( const btDbvtAabbMm& o, - const btDbvtAabbMm& a, +DBVT_INLINE btScalar Proximity(const btDbvtAabbMm& a, const btDbvtAabbMm& b) { -#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE - -#if defined (_WIN32) - static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; + const btVector3 d = (a.mi + a.mx) - (b.mi + b.mx); + return (btFabs(d.x()) + btFabs(d.y()) + btFabs(d.z())); +} + +// +DBVT_INLINE int Select(const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE + +#if defined(_WIN32) + static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff}; #else - static ATTRIBUTE_ALIGNED16(const unsigned int) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 /*0x7fffffff*/}; + static ATTRIBUTE_ALIGNED16(const unsigned int) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x00000000 /*0x7fffffff*/}; #endif ///@todo: the intrinsic version is 11% slower #if DBVT_USE_INTRINSIC_SSE - union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory + union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory { - __m128 ssereg; - float floats[4]; - int ints[4]; + __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)); - + __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)); + btSSEUnion tmp; - tmp.ssereg = _mm_cmple_ss(bmi,ami); - return tmp.ints[0]&1; + tmp.ssereg = _mm_cmple_ss(bmi, ami); + return tmp.ints[0] & 1; #else - ATTRIBUTE_ALIGNED16(__int32 r[1]); + ATTRIBUTE_ALIGNED16(__int32 r[1]); __asm { mov eax,o @@ -656,46 +689,52 @@ DBVT_INLINE int Select( const btDbvtAabbMm& o, cmpless xmm2,xmm1 movss r,xmm2 } - return(r[0]&1); + return (r[0] & 1); #endif #else - return(Proximity(o,a)b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; + if (a.mi[i] < b.mi[i]) + r.mi[i] = a.mi[i]; + else + r.mi[i] = b.mi[i]; + if (a.mx[i] > b.mx[i]) + r.mx[i] = a.mx[i]; + else + r.mx[i] = b.mx[i]; } #endif } // -DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) +DBVT_INLINE bool NotEqual(const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { - return( (a.mi.x()!=b.mi.x())|| - (a.mi.y()!=b.mi.y())|| - (a.mi.z()!=b.mi.z())|| - (a.mx.x()!=b.mx.x())|| - (a.mx.y()!=b.mx.y())|| - (a.mx.z()!=b.mx.z())); + 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())); } // @@ -704,162 +743,162 @@ DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, // DBVT_PREFIX -inline void btDbvt::enumNodes( const btDbvtNode* root, - DBVT_IPOLICY) +inline void btDbvt::enumNodes(const btDbvtNode* root, + DBVT_IPOLICY) { DBVT_CHECKTYPE - policy.Process(root); - if(root->isinternal()) + policy.Process(root); + if (root->isinternal()) { - enumNodes(root->childs[0],policy); - enumNodes(root->childs[1],policy); + enumNodes(root->childs[0], policy); + enumNodes(root->childs[1], policy); } } // DBVT_PREFIX -inline void btDbvt::enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY) +inline void btDbvt::enumLeaves(const btDbvtNode* root, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root->isinternal()) - { - enumLeaves(root->childs[0],policy); - enumLeaves(root->childs[1],policy); - } - else - { - policy.Process(root); - } + if (root->isinternal()) + { + enumLeaves(root->childs[0], policy); + enumLeaves(root->childs[1], policy); + } + else + { + policy.Process(root); + } } // DBVT_PREFIX -inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY) +inline void btDbvt::collideTT(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root0&&root1) + if (root0 && root1) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + btAlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0] = sStkNN(root0, root1); + do { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - btAlignedObjectArray stkStack; - stkStack.resize(DOUBLE_STACKSIZE); - stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=stkStack[--depth]; - if(depth>treshold) + 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.resize(stkStack.size()*2); - treshold=stkStack.size()-4; + 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]); } - if(p.a==p.b) + } + else if (Intersect(p.a->volume, p.b->volume)) + { + if (p.a->isinternal()) { - if(p.a->isinternal()) + if (p.b->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(Intersect(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); - } + 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 { - 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); - } + stkStack[depth++] = sStkNN(p.a->childs[0], p.b); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b); } } - } while(depth); - } + 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); + } } - - DBVT_PREFIX -inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY) +inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root0&&root1) + if (root0 && root1) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + + m_stkStack.resize(DOUBLE_STACKSIZE); + m_stkStack[0] = sStkNN(root0, root1); + do { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - - m_stkStack.resize(DOUBLE_STACKSIZE); - m_stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=m_stkStack[--depth]; - if(depth>treshold) + 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.resize(m_stkStack.size()*2); - treshold=m_stkStack.size()-4; + 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]); } - if(p.a==p.b) + } + else if (Intersect(p.a->volume, p.b->volume)) + { + if (p.a->isinternal()) { - if(p.a->isinternal()) + if (p.b->isinternal()) { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); - } - } - else if(Intersect(p.a->volume,p.b->volume)) - { - if(p.a->isinternal()) - { - if(p.b->isinternal()) - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); - } + 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 { - 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); - } + m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b); + m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b); } } - } while(depth); - } + 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 @@ -929,33 +968,35 @@ inline void btDbvt::collideTT( const btDbvtNode* root0, const btTransform xform=xform0.inverse()*xform1; collideTT(root0,root1,xform,policy); } -#endif +#endif DBVT_PREFIX -inline void btDbvt::collideTV( const btDbvtNode* root, - const btDbvtVolume& vol, - DBVT_IPOLICY) const +inline void btDbvt::collideTV(const btDbvtNode* root, + const btDbvtVolume& vol, + DBVT_IPOLICY) const { DBVT_CHECKTYPE - if(root) + if (root) { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - btAlignedObjectArray stack; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume(vol); + btAlignedObjectArray stack; stack.resize(0); #ifndef BT_DISABLE_STACK_TEMP_MEMORY - char tempmemory[SIMPLE_STACKSIZE*sizeof(const btDbvtNode*)]; + char tempmemory[SIMPLE_STACKSIZE * sizeof(const btDbvtNode*)]; stack.initializeFromBuffer(tempmemory, 0, SIMPLE_STACKSIZE); #else stack.reserve(SIMPLE_STACKSIZE); -#endif //BT_DISABLE_STACK_TEMP_MEMORY +#endif //BT_DISABLE_STACK_TEMP_MEMORY stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; + do + { + const btDbvtNode* n = stack[stack.size() - 1]; stack.pop_back(); - if(Intersect(n->volume,volume)) + if (Intersect(n->volume, volume)) { - if(n->isinternal()) + if (n->isinternal()) { stack.push_back(n->childs[0]); stack.push_back(n->childs[1]); @@ -965,30 +1006,32 @@ inline void btDbvt::collideTV( const btDbvtNode* root, policy.Process(n); } } - } while(stack.size()>0); + } while (stack.size() > 0); } } // DBVT_PREFIX -inline void btDbvt::collideTVNoStackAlloc( const btDbvtNode* root, - const btDbvtVolume& vol, - btNodeStack& stack, - DBVT_IPOLICY) const +inline void btDbvt::collideTVNoStackAlloc(const btDbvtNode* root, + const btDbvtVolume& vol, + btNodeStack& stack, + DBVT_IPOLICY) const { DBVT_CHECKTYPE - if(root) + if (root) { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume(vol); stack.resize(0); stack.reserve(SIMPLE_STACKSIZE); stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; + do + { + const btDbvtNode* n = stack[stack.size() - 1]; stack.pop_back(); - if(Intersect(n->volume,volume)) + if (Intersect(n->volume, volume)) { - if(n->isinternal()) + if (n->isinternal()) { stack.push_back(n->childs[0]); stack.push_back(n->childs[1]); @@ -998,328 +1041,346 @@ inline void btDbvt::collideTVNoStackAlloc( const btDbvtNode* root, policy.Process(n); } } - } while(stack.size()>0); + } while (stack.size() > 0); } } - DBVT_PREFIX -inline void btDbvt::rayTestInternal( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayDirectionInverse, - unsigned int signs[3], - btScalar lambda_max, - const btVector3& aabbMin, - const btVector3& aabbMax, - btAlignedObjectArray& stack, - DBVT_IPOLICY ) const +inline void btDbvt::rayTestInternal(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayDirectionInverse, + unsigned int signs[3], + btScalar lambda_max, + const btVector3& aabbMin, + const btVector3& aabbMax, + btAlignedObjectArray& stack, + DBVT_IPOLICY) const { - (void) rayTo; + (void)rayTo; DBVT_CHECKTYPE - if(root) + if (root) { btVector3 resultNormal; - int depth=1; - int treshold=DOUBLE_STACKSIZE-2; + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 2; stack.resize(DOUBLE_STACKSIZE); - stack[0]=root; + stack[0] = root; btVector3 bounds[2]; - do + do { - const btDbvtNode* node=stack[--depth]; - bounds[0] = node->volume.Mins()-aabbMax; - bounds[1] = node->volume.Maxs()-aabbMin; - btScalar tmin=1.f,lambda_min=0.f; - unsigned int result1=false; - result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); - if(result1) + const btDbvtNode* node = stack[--depth]; + bounds[0] = node->volume.Mins() - aabbMax; + bounds[1] = node->volume.Maxs() - aabbMin; + btScalar tmin = 1.f, lambda_min = 0.f; + unsigned int result1 = false; + result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max); + if (result1) { - if(node->isinternal()) + if (node->isinternal()) { - if(depth>treshold) + if (depth > treshold) { - stack.resize(stack.size()*2); - treshold=stack.size()-2; + stack.resize(stack.size() * 2); + treshold = stack.size() - 2; } - stack[depth++]=node->childs[0]; - stack[depth++]=node->childs[1]; + stack[depth++] = node->childs[0]; + stack[depth++] = node->childs[1]; } else { policy.Process(node); } } - } while(depth); + } while (depth); } } // DBVT_PREFIX -inline void btDbvt::rayTest( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, +inline void btDbvt::rayTest(const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if (root) + { + btVector3 rayDir = (rayTo - rayFrom); + rayDir.normalize(); + + ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; + unsigned int signs[3] = {rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + + btScalar lambda_max = rayDir.dot(rayTo - rayFrom); + + btVector3 resultNormal; + + btAlignedObjectArray stack; + + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 2; + + 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 + { + const btDbvtNode* node = stack[--depth]; + + bounds[0] = node->volume.Mins(); + bounds[1] = node->volume.Maxs(); + + btScalar tmin = 1.f, lambda_min = 0.f; + unsigned int result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max); + +#ifdef COMPARE_BTRAY_AABB2 + btScalar param = 1.f; + bool result2 = btRayAabb(rayFrom, rayTo, node->volume.Mins(), node->volume.Maxs(), param, resultNormal); + btAssert(result1 == result2); +#endif //TEST_BTRAY_AABB2 + + if (result1) + { + 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); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, DBVT_IPOLICY) { DBVT_CHECKTYPE - if(root) + if (root) + { + const int inside = (1 << count) - 1; + btAlignedObjectArray stack; + int signs[sizeof(unsigned) * 8]; + btAssert(count < int(sizeof(signs) / sizeof(signs[0]))); + for (int i = 0; i < count; ++i) { - btVector3 rayDir = (rayTo-rayFrom); - rayDir.normalize (); - - ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT - btVector3 rayDirectionInverse; - rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; - rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; - rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; - unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; - - btScalar lambda_max = rayDir.dot(rayTo-rayFrom); - - btVector3 resultNormal; - - btAlignedObjectArray stack; - - int depth=1; - int treshold=DOUBLE_STACKSIZE-2; - - 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 { - const btDbvtNode* node=stack[--depth]; - - bounds[0] = node->volume.Mins(); - bounds[1] = node->volume.Maxs(); - - btScalar tmin=1.f,lambda_min=0.f; - unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); - -#ifdef COMPARE_BTRAY_AABB2 - btScalar param=1.f; - bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); - btAssert(result1 == result2); -#endif //TEST_BTRAY_AABB2 - - if(result1) - { - 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); - + signs[i] = ((normals[i].x() >= 0) ? 1 : 0) + + ((normals[i].y() >= 0) ? 2 : 0) + + ((normals[i].z() >= 0) ? 4 : 0); } -} - -// -DBVT_PREFIX -inline void btDbvt::collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root) + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(sStkNP(root, 0)); + do { - const int inside=(1< stack; - int signs[sizeof(unsigned)*8]; - btAssert(count=0)?1:0)+ - ((normals[i].y()>=0)?2:0)+ - ((normals[i].z()>=0)?4:0); + if (0 == (se.mask & j)) + { + const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]); + switch (side) + { + case -1: + out = true; + break; + case +1: + se.mask |= j; + break; + } + } } - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(sStkNP(root,0)); - do { - sStkNP se=stack[stack.size()-1]; - bool out=false; - stack.pop_back(); - for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); - switch(side) - { - case -1: out=true;break; - case +1: se.mask|=j;break; - } - } - } - if(!out) - { - if((se.mask!=inside)&&(se.node->isinternal())) - { - stack.push_back(sStkNP(se.node->childs[0],se.mask)); - stack.push_back(sStkNP(se.node->childs[1],se.mask)); - } - else - { - if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); - } - } - } while(stack.size()); - } -} - -// -DBVT_PREFIX -inline void btDbvt::collideOCL( const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - const btVector3& sortaxis, - int count, - DBVT_IPOLICY, - bool fsort) -{ - DBVT_CHECKTYPE - if(root) - { - const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ - (sortaxis[1]>=0?2:0)+ - (sortaxis[2]>=0?4:0); - const int inside=(1< stock; - btAlignedObjectArray ifree; - btAlignedObjectArray stack; - int signs[sizeof(unsigned)*8]; - btAssert(count=0)?1:0)+ - ((normals[i].y()>=0)?2:0)+ - ((normals[i].z()>=0)?4:0); + 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); + } } - stock.reserve(SIMPLE_STACKSIZE); - stack.reserve(SIMPLE_STACKSIZE); - ifree.reserve(SIMPLE_STACKSIZE); - stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); - do { - const int id=stack[stack.size()-1]; - sStkNPS se=stock[id]; - stack.pop_back();ifree.push_back(id); - if(se.mask!=inside) - { - bool out=false; - for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); - switch(side) - { - case -1: out=true;break; - case +1: se.mask|=j;break; - } - } - } - if(out) continue; - } - if(policy.Descent(se.node)) - { - if(se.node->isinternal()) - { - const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; - sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), - sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; - const int q=nes[0].value0)) - { - /* Insert 0 */ - j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); - stack.push_back(0); - - //void * memmove ( void * destination, const void * source, size_t num ); - -#if DBVT_USE_MEMMOVE - { - int num_items_to_move = stack.size()-1-j; - if(num_items_to_move > 0) - memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move); - } -#else - for(int k=stack.size()-1;k>j;--k) { - stack[k]=stack[k-1]; - } -#endif - stack[j]=allocate(ifree,stock,nes[q]); - /* Insert 1 */ - j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); - stack.push_back(0); -#if DBVT_USE_MEMMOVE - { - int num_items_to_move = stack.size()-1-j; - if(num_items_to_move > 0) - memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move); - } -#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()); - } + } while (stack.size()); + } } // DBVT_PREFIX -inline void btDbvt::collideTU( const btDbvtNode* root, - DBVT_IPOLICY) +inline void btDbvt::collideOCL(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fsort) { DBVT_CHECKTYPE - if(root) + if (root) + { + const unsigned srtsgns = (sortaxis[0] >= 0 ? 1 : 0) + + (sortaxis[1] >= 0 ? 2 : 0) + + (sortaxis[2] >= 0 ? 4 : 0); + const int inside = (1 << count) - 1; + btAlignedObjectArray stock; + btAlignedObjectArray ifree; + btAlignedObjectArray stack; + int signs[sizeof(unsigned) * 8]; + btAssert(count < int(sizeof(signs) / sizeof(signs[0]))); + for (int i = 0; i < count; ++i) { - btAlignedObjectArray stack; - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(policy.Descent(n)) - { - if(n->isinternal()) - { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } - else - { policy.Process(n); } - } - } while(stack.size()>0); + signs[i] = ((normals[i].x() >= 0) ? 1 : 0) + + ((normals[i].y() >= 0) ? 2 : 0) + + ((normals[i].z() >= 0) ? 4 : 0); } + stock.reserve(SIMPLE_STACKSIZE); + stack.reserve(SIMPLE_STACKSIZE); + ifree.reserve(SIMPLE_STACKSIZE); + stack.push_back(allocate(ifree, stock, sStkNPS(root, 0, root->volume.ProjectMinimum(sortaxis, srtsgns)))); + do + { + const int id = stack[stack.size() - 1]; + sStkNPS se = stock[id]; + stack.pop_back(); + ifree.push_back(id); + if (se.mask != inside) + { + bool out = false; + for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1) + { + if (0 == (se.mask & j)) + { + const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]); + switch (side) + { + case -1: + out = true; + break; + case +1: + se.mask |= j; + break; + } + } + } + if (out) continue; + } + if (policy.Descent(se.node)) + { + if (se.node->isinternal()) + { + const btDbvtNode* pns[] = {se.node->childs[0], se.node->childs[1]}; + sStkNPS nes[] = {sStkNPS(pns[0], se.mask, pns[0]->volume.ProjectMinimum(sortaxis, srtsgns)), + sStkNPS(pns[1], se.mask, pns[1]->volume.ProjectMinimum(sortaxis, srtsgns))}; + const int q = nes[0].value < nes[1].value ? 1 : 0; + int j = stack.size(); + if (fsort && (j > 0)) + { + /* Insert 0 */ + j = nearest(&stack[0], &stock[0], nes[q].value, 0, stack.size()); + stack.push_back(0); + + //void * memmove ( void * destination, const void * source, size_t num ); + +#if DBVT_USE_MEMMOVE + { + int num_items_to_move = stack.size() - 1 - j; + if (num_items_to_move > 0) + memmove(&stack[j + 1], &stack[j], sizeof(int) * num_items_to_move); + } +#else + for (int k = stack.size() - 1; k > j; --k) + { + stack[k] = stack[k - 1]; + } +#endif + stack[j] = allocate(ifree, stock, nes[q]); + /* Insert 1 */ + j = nearest(&stack[0], &stock[0], nes[1 - q].value, j, stack.size()); + stack.push_back(0); +#if DBVT_USE_MEMMOVE + { + int num_items_to_move = stack.size() - 1 - j; + if (num_items_to_move > 0) + memmove(&stack[j + 1], &stack[j], sizeof(int) * num_items_to_move); + } +#else + for (int k = stack.size() - 1; k > j; --k) + { + stack[k] = stack[k - 1]; + } +#endif + stack[j] = allocate(ifree, stock, nes[1 - q]); + } + else + { + stack.push_back(allocate(ifree, stock, nes[q])); + stack.push_back(allocate(ifree, stock, nes[1 - q])); + } + } + else + { + policy.Process(se.node, se.value); + } + } + } while (stack.size()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTU(const btDbvtNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if (root) + { + btAlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do + { + const btDbvtNode* n = stack[stack.size() - 1]; + stack.pop_back(); + if (policy.Descent(n)) + { + if (n->isinternal()) + { + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); + } + else + { + policy.Process(n); + } + } + } while (stack.size() > 0); + } } // diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index 4d12b1c9c..7b39dbdc0 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -17,33 +17,32 @@ subject to the following restrictions: #include "btDbvtBroadphase.h" #include "LinearMath/btThreads.h" - +btScalar gDbvtMargin = btScalar(0.05); // // Profiling // -#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK +#if DBVT_BP_PROFILE || DBVT_BP_ENABLE_BENCHMARK #include #endif #if DBVT_BP_PROFILE -struct ProfileScope +struct ProfileScope { - __forceinline ProfileScope(btClock& clock,unsigned long& value) : - m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + __forceinline ProfileScope(btClock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds()) { } __forceinline ~ProfileScope() { - (*m_value)+=m_clock->getTimeMicroseconds()-m_base; + (*m_value) += m_clock->getTimeMicroseconds() - m_base; } - btClock* m_clock; - unsigned long* m_value; - unsigned long m_base; + btClock* m_clock; + unsigned long* m_value; + unsigned long m_base; }; -#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) +#define SPC(_value_) ProfileScope spc_scope(m_clock, _value_) #else -#define SPC(_value_) +#define SPC(_value_) #endif // @@ -52,66 +51,75 @@ struct ProfileScope // template -static inline void listappend(T* item,T*& list) +static inline void listappend(T* item, T*& list) { - item->links[0]=0; - item->links[1]=list; - if(list) list->links[0]=item; - list=item; + item->links[0] = 0; + item->links[1] = list; + if (list) list->links[0] = item; + list = item; } // template -static inline void listremove(T* item,T*& list) +static inline void listremove(T* item, T*& list) { - if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; - if(item->links[1]) item->links[1]->links[0]=item->links[0]; + if (item->links[0]) + item->links[0]->links[1] = item->links[1]; + else + list = item->links[1]; + if (item->links[1]) item->links[1]->links[0] = item->links[0]; } // template -static inline int listcount(T* root) +static inline int listcount(T* root) { - int n=0; - while(root) { ++n;root=root->links[1]; } - return(n); + int n = 0; + while (root) + { + ++n; + root = root->links[1]; + } + return (n); } // template -static inline void clear(T& value) +static inline void clear(T& value) { - static const struct ZeroDummy : T {} zerodummy; - value=zerodummy; + static const struct ZeroDummy : T + { + } zerodummy; + value = zerodummy; } // // Colliders // -/* Tree collider */ -struct btDbvtTreeCollider : btDbvt::ICollide +/* Tree collider */ +struct btDbvtTreeCollider : btDbvt::ICollide { - btDbvtBroadphase* pbp; - btDbvtProxy* proxy; + btDbvtBroadphase* pbp; + btDbvtProxy* proxy; btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} - void Process(const btDbvtNode* na,const btDbvtNode* nb) + void Process(const btDbvtNode* na, const btDbvtNode* nb) { - if(na!=nb) + if (na != nb) { - btDbvtProxy* pa=(btDbvtProxy*)na->data; - btDbvtProxy* pb=(btDbvtProxy*)nb->data; + btDbvtProxy* pa = (btDbvtProxy*)na->data; + btDbvtProxy* pb = (btDbvtProxy*)nb->data; #if DBVT_BP_SORTPAIRS - if(pa->m_uniqueId>pb->m_uniqueId) - btSwap(pa,pb); + if (pa->m_uniqueId > pb->m_uniqueId) + btSwap(pa, pb); #endif - pbp->m_paircache->addOverlappingPair(pa,pb); + pbp->m_paircache->addOverlappingPair(pa, pb); ++pbp->m_newpairs; } } - void Process(const btDbvtNode* n) + void Process(const btDbvtNode* n) { - Process(n,proxy->leaf); + Process(n, proxy->leaf); } }; @@ -122,31 +130,31 @@ struct btDbvtTreeCollider : btDbvt::ICollide // btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) { - m_deferedcollide = false; - m_needcleanup = true; - m_releasepaircache = (paircache!=0)?false:true; - m_prediction = 0; - m_stageCurrent = 0; - m_fixedleft = 0; - m_fupdates = 1; - m_dupdates = 0; - m_cupdates = 10; - m_newpairs = 1; - m_updates_call = 0; - m_updates_done = 0; - m_updates_ratio = 0; - m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); - m_gid = 0; - m_pid = 0; - m_cid = 0; - for(int i=0;i<=STAGECOUNT;++i) + m_deferedcollide = false; + m_needcleanup = true; + m_releasepaircache = (paircache != 0) ? false : true; + m_prediction = 0; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + m_paircache = paircache ? paircache : new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16)) btHashedOverlappingPairCache(); + m_gid = 0; + m_pid = 0; + m_cid = 0; + for (int i = 0; i <= STAGECOUNT; ++i) { - m_stageRoots[i]=0; + m_stageRoots[i] = 0; } #if BT_THREADSAFE - m_rayTestStacks.resize(BT_MAX_THREAD_COUNT); + m_rayTestStacks.resize(BT_MAX_THREAD_COUNT); #else - m_rayTestStacks.resize(1); + m_rayTestStacks.resize(1); #endif #if DBVT_BP_PROFILE clear(m_profiling); @@ -156,7 +164,7 @@ btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) // btDbvtBroadphase::~btDbvtBroadphase() { - if(m_releasepaircache) + if (m_releasepaircache) { m_paircache->~btOverlappingPairCache(); btAlignedFree(m_paircache); @@ -164,305 +172,294 @@ btDbvtBroadphase::~btDbvtBroadphase() } // -btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, - const btVector3& aabbMax, - int /*shapeType*/, - void* userPtr, - int collisionFilterGroup, - int collisionFilterMask, - btDispatcher* /*dispatcher*/) +btBroadphaseProxy* btDbvtBroadphase::createProxy(const btVector3& aabbMin, + const btVector3& aabbMax, + int /*shapeType*/, + void* userPtr, + int collisionFilterGroup, + int collisionFilterMask, + btDispatcher* /*dispatcher*/) { - btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, - collisionFilterGroup, - collisionFilterMask); + btDbvtProxy* proxy = new (btAlignedAlloc(sizeof(btDbvtProxy), 16)) btDbvtProxy(aabbMin, aabbMax, userPtr, + collisionFilterGroup, + collisionFilterMask); - btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); + btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin, aabbMax); //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); - proxy->stage = m_stageCurrent; - proxy->m_uniqueId = ++m_gid; - proxy->leaf = m_sets[0].insert(aabb,proxy); - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(!m_deferedcollide) + proxy->stage = m_stageCurrent; + proxy->m_uniqueId = ++m_gid; + proxy->leaf = m_sets[0].insert(aabb, proxy); + listappend(proxy, m_stageRoots[m_stageCurrent]); + if (!m_deferedcollide) { - btDbvtTreeCollider collider(this); - collider.proxy=proxy; - m_sets[0].collideTV(m_sets[0].m_root,aabb,collider); - m_sets[1].collideTV(m_sets[1].m_root,aabb,collider); + btDbvtTreeCollider collider(this); + collider.proxy = proxy; + m_sets[0].collideTV(m_sets[0].m_root, aabb, collider); + m_sets[1].collideTV(m_sets[1].m_root, aabb, collider); } - return(proxy); + return (proxy); } // -void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, - btDispatcher* dispatcher) +void btDbvtBroadphase::destroyProxy(btBroadphaseProxy* absproxy, + btDispatcher* dispatcher) { - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - if(proxy->stage==STAGECOUNT) + btDbvtProxy* proxy = (btDbvtProxy*)absproxy; + if (proxy->stage == STAGECOUNT) m_sets[1].remove(proxy->leaf); else m_sets[0].remove(proxy->leaf); - listremove(proxy,m_stageRoots[proxy->stage]); - m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); + listremove(proxy, m_stageRoots[proxy->stage]); + m_paircache->removeOverlappingPairsContainingProxy(proxy, dispatcher); btAlignedFree(proxy); - m_needcleanup=true; + m_needcleanup = true; } -void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const +void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy, btVector3& aabbMin, btVector3& aabbMax) const { - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + btDbvtProxy* proxy = (btDbvtProxy*)absproxy; aabbMin = proxy->m_aabbMin; aabbMax = proxy->m_aabbMax; } -struct BroadphaseRayTester : btDbvt::ICollide +struct BroadphaseRayTester : btDbvt::ICollide { btBroadphaseRayCallback& m_rayCallback; BroadphaseRayTester(btBroadphaseRayCallback& orgCallback) - :m_rayCallback(orgCallback) + : m_rayCallback(orgCallback) { } - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; + btDbvtProxy* proxy = (btDbvtProxy*)leaf->data; m_rayCallback.process(proxy); } -}; +}; -void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) +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]; + 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; - } + // 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; + //todo(erwincoumans, "why do we get tsan issue here?") + if (0)//threadIndex < m_rayTestStacks.size()) + //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, - rayTo, - rayCallback.m_rayDirectionInverse, - rayCallback.m_signs, - rayCallback.m_lambda_max, - aabbMin, - aabbMax, - *stack, - callback); - - m_sets[1].rayTestInternal( m_sets[1].m_root, - rayFrom, - rayTo, - rayCallback.m_rayDirectionInverse, - rayCallback.m_signs, - rayCallback.m_lambda_max, - aabbMin, - aabbMax, - *stack, - callback); + m_sets[0].rayTestInternal(m_sets[0].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + *stack, + callback); + m_sets[1].rayTestInternal(m_sets[1].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + *stack, + callback); } - -struct BroadphaseAabbTester : btDbvt::ICollide +struct BroadphaseAabbTester : btDbvt::ICollide { btBroadphaseAabbCallback& m_aabbCallback; BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback) - :m_aabbCallback(orgCallback) + : m_aabbCallback(orgCallback) { } - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; + btDbvtProxy* proxy = (btDbvtProxy*)leaf->data; m_aabbCallback.process(proxy); } -}; +}; -void btDbvtBroadphase::aabbTest(const btVector3& aabbMin,const btVector3& aabbMax,btBroadphaseAabbCallback& aabbCallback) +void btDbvtBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& aabbCallback) { BroadphaseAabbTester callback(aabbCallback); - const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::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); - + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::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 btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, - const btVector3& aabbMin, - const btVector3& aabbMax, - btDispatcher* /*dispatcher*/) +void btDbvtBroadphase::setAabb(btBroadphaseProxy* absproxy, + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* /*dispatcher*/) { - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); + btDbvtProxy* proxy = (btDbvtProxy*)absproxy; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + aabb = btDbvtVolume::FromMM(aabbMin, aabbMax); #if DBVT_BP_PREVENTFALSEUPDATE - if(NotEqual(aabb,proxy->leaf->volume)) + if (NotEqual(aabb, proxy->leaf->volume)) #endif { - bool docollide=false; - if(proxy->stage==STAGECOUNT) - {/* fixed -> dynamic set */ + 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; + proxy->leaf = m_sets[0].insert(aabb, proxy); + docollide = true; } else - {/* dynamic set */ + { /* dynamic set */ ++m_updates_call; - if(Intersect(proxy->leaf->volume,aabb)) - {/* Moving */ + if (Intersect(proxy->leaf->volume, aabb)) + { /* Moving */ - const btVector3 delta=aabbMin-proxy->m_aabbMin; - btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); - if(delta[0]<0) velocity[0]=-velocity[0]; - if(delta[1]<0) velocity[1]=-velocity[1]; - if(delta[2]<0) velocity[2]=-velocity[2]; - if ( -#ifdef DBVT_BP_MARGIN - m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) -#else - m_sets[0].update(proxy->leaf,aabb,velocity) -#endif - ) + const btVector3 delta = aabbMin - proxy->m_aabbMin; + btVector3 velocity(((proxy->m_aabbMax - proxy->m_aabbMin) / 2) * m_prediction); + if (delta[0] < 0) velocity[0] = -velocity[0]; + if (delta[1] < 0) velocity[1] = -velocity[1]; + if (delta[2] < 0) velocity[2] = -velocity[2]; + if ( + m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin) + + ) { ++m_updates_done; - docollide=true; + docollide = true; } } else - {/* Teleporting */ - m_sets[0].update(proxy->leaf,aabb); + { /* Teleporting */ + m_sets[0].update(proxy->leaf, aabb); ++m_updates_done; - docollide=true; - } + docollide = true; + } } - listremove(proxy,m_stageRoots[proxy->stage]); + listremove(proxy, m_stageRoots[proxy->stage]); proxy->m_aabbMin = aabbMin; proxy->m_aabbMax = aabbMax; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(docollide) + proxy->stage = m_stageCurrent; + listappend(proxy, m_stageRoots[m_stageCurrent]); + if (docollide) { - m_needcleanup=true; - if(!m_deferedcollide) + m_needcleanup = true; + if (!m_deferedcollide) { - btDbvtTreeCollider collider(this); - m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); + btDbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider); } - } + } } } - // -void btDbvtBroadphase::setAabbForceUpdate( btBroadphaseProxy* absproxy, - const btVector3& aabbMin, - const btVector3& aabbMax, - btDispatcher* /*dispatcher*/) +void btDbvtBroadphase::setAabbForceUpdate(btBroadphaseProxy* absproxy, + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* /*dispatcher*/) { - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); - bool docollide=false; - if(proxy->stage==STAGECOUNT) - {/* fixed -> dynamic set */ + btDbvtProxy* proxy = (btDbvtProxy*)absproxy; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + aabb = btDbvtVolume::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; + proxy->leaf = m_sets[0].insert(aabb, proxy); + docollide = true; } else - {/* dynamic set */ + { /* dynamic set */ ++m_updates_call; - /* Teleporting */ - m_sets[0].update(proxy->leaf,aabb); + /* Teleporting */ + m_sets[0].update(proxy->leaf, aabb); ++m_updates_done; - docollide=true; + docollide = true; } - listremove(proxy,m_stageRoots[proxy->stage]); + listremove(proxy, m_stageRoots[proxy->stage]); proxy->m_aabbMin = aabbMin; proxy->m_aabbMax = aabbMax; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(docollide) + proxy->stage = m_stageCurrent; + listappend(proxy, m_stageRoots[m_stageCurrent]); + if (docollide) { - m_needcleanup=true; - if(!m_deferedcollide) + m_needcleanup = true; + if (!m_deferedcollide) { - btDbvtTreeCollider collider(this); - m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); + btDbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider); } - } + } } // -void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { collide(dispatcher); #if DBVT_BP_PROFILE - if(0==(m_pid%DBVT_BP_PROFILING_RATE)) - { - printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); - unsigned int total=m_profiling.m_total; - if(total<=0) total=1; - printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); - printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); - printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); - printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); - const unsigned long sum=m_profiling.m_ddcollide+ - m_profiling.m_fdcollide+ - m_profiling.m_cleanup; - printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); - printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE)); + if (0 == (m_pid % DBVT_BP_PROFILING_RATE)) + { + printf("fixed(%u) dynamics(%u) pairs(%u)\r\n", m_sets[1].m_leaves, m_sets[0].m_leaves, m_paircache->getNumOverlappingPairs()); + unsigned int total = m_profiling.m_total; + if (total <= 0) total = 1; + printf("ddcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_ddcollide * 100) / total, m_profiling.m_ddcollide / DBVT_BP_PROFILING_RATE); + printf("fdcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_fdcollide * 100) / total, m_profiling.m_fdcollide / DBVT_BP_PROFILING_RATE); + printf("cleanup: %u%% (%uus)\r\n", (50 + m_profiling.m_cleanup * 100) / total, m_profiling.m_cleanup / DBVT_BP_PROFILING_RATE); + printf("total: %uus\r\n", total / DBVT_BP_PROFILING_RATE); + const unsigned long sum = m_profiling.m_ddcollide + + m_profiling.m_fdcollide + + m_profiling.m_cleanup; + printf("leaked: %u%% (%uus)\r\n", 100 - ((50 + sum * 100) / total), (total - sum) / DBVT_BP_PROFILING_RATE); + printf("job counts: %u%%\r\n", (m_profiling.m_jobcount * 100) / ((m_sets[0].m_leaves + m_sets[1].m_leaves) * DBVT_BP_PROFILING_RATE)); clear(m_profiling); m_clock.reset(); } #endif performDeferredRemoval(dispatcher); - } void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher) { - if (m_paircache->hasDeferredRemoval()) { - - btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); + btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); //perform a sort, to find duplicates and to sort 'invalid' pairs to the end overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); int invalidPair = 0; - int i; btBroadphasePair previousPair; previousPair.m_pProxy0 = 0; previousPair.m_pProxy1 = 0; previousPair.m_algorithm = 0; - - - for (i=0;ileaf->volume,pb->leaf->volume); + btDbvtProxy* pa = (btDbvtProxy*)pair.m_pProxy0; + btDbvtProxy* pb = (btDbvtProxy*)pair.m_pProxy1; + bool hasOverlap = Intersect(pa->leaf->volume, pb->leaf->volume); if (hasOverlap) { needsRemoval = false; - } else + } + else { needsRemoval = true; } - } else + } + else { //remove duplicate needsRemoval = true; //should have no algorithm btAssert(!pair.m_algorithm); } - + if (needsRemoval) { - m_paircache->cleanOverlappingPair(pair,dispatcher); + m_paircache->cleanOverlappingPair(pair, dispatcher); pair.m_pProxy0 = 0; pair.m_pProxy1 = 0; invalidPair++; - } - + } } //perform a sort, to sort 'invalid' pairs to the end @@ -511,7 +509,7 @@ void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher) } // -void btDbvtBroadphase::collide(btDispatcher* dispatcher) +void btDbvtBroadphase::collide(btDispatcher* dispatcher) { /*printf("---------------------------------------------------------\n"); printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves); @@ -528,295 +526,303 @@ void btDbvtBroadphase::collide(btDispatcher* dispatcher) } */ - - SPC(m_profiling.m_total); - /* optimize */ - m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); - if(m_fixedleft) + /* optimize */ + m_sets[0].optimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100); + if (m_fixedleft) { - const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; - m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); - m_fixedleft=btMax(0,m_fixedleft-count); + const int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100; + m_sets[1].optimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100); + m_fixedleft = btMax(0, m_fixedleft - count); } - /* dynamic -> fixed set */ - m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; - btDbvtProxy* current=m_stageRoots[m_stageCurrent]; - if(current) + /* dynamic -> fixed set */ + m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT; + btDbvtProxy* current = m_stageRoots[m_stageCurrent]; + if (current) { #if DBVT_BP_ACCURATESLEEPING - btDbvtTreeCollider collider(this); + btDbvtTreeCollider collider(this); #endif - do { - btDbvtProxy* next=current->links[1]; - listremove(current,m_stageRoots[current->stage]); - listappend(current,m_stageRoots[STAGECOUNT]); + do + { + btDbvtProxy* next = current->links[1]; + listremove(current, m_stageRoots[current->stage]); + listappend(current, m_stageRoots[STAGECOUNT]); #if DBVT_BP_ACCURATESLEEPING - m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); - collider.proxy=current; - btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); - btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); + m_paircache->removeOverlappingPairsContainingProxy(current, dispatcher); + collider.proxy = current; + btDbvt::collideTV(m_sets[0].m_root, current->aabb, collider); + btDbvt::collideTV(m_sets[1].m_root, current->aabb, collider); #endif m_sets[0].remove(current->leaf); - ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); - current->leaf = m_sets[1].insert(curAabb,current); - current->stage = STAGECOUNT; - current = next; - } while(current); - m_fixedleft=m_sets[1].m_leaves; - m_needcleanup=true; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + curAabb = btDbvtVolume::FromMM(current->m_aabbMin, current->m_aabbMax); + current->leaf = m_sets[1].insert(curAabb, current); + current->stage = STAGECOUNT; + current = next; + } while (current); + m_fixedleft = m_sets[1].m_leaves; + m_needcleanup = true; } - /* collide dynamics */ + /* collide dynamics */ { - btDbvtTreeCollider collider(this); - if(m_deferedcollide) + btDbvtTreeCollider collider(this); + if (m_deferedcollide) { SPC(m_profiling.m_fdcollide); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider); } - if(m_deferedcollide) + if (m_deferedcollide) { SPC(m_profiling.m_ddcollide); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider); } } - /* clean up */ - if(m_needcleanup) + /* clean up */ + if (m_needcleanup) { SPC(m_profiling.m_cleanup); - btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); - if(pairs.size()>0) + btBroadphasePairArray& pairs = m_paircache->getOverlappingPairArray(); + if (pairs.size() > 0) { - - int ni=btMin(pairs.size(),btMax(m_newpairs,(pairs.size()*m_cupdates)/100)); - for(int i=0;i(m_newpairs, (pairs.size() * m_cupdates) / 100)); + for (int i = 0; i < ni; ++i) { - btBroadphasePair& p=pairs[(m_cid+i)%pairs.size()]; - btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0; - btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1; - if(!Intersect(pa->leaf->volume,pb->leaf->volume)) + btBroadphasePair& p = pairs[(m_cid + i) % pairs.size()]; + btDbvtProxy* pa = (btDbvtProxy*)p.m_pProxy0; + btDbvtProxy* pb = (btDbvtProxy*)p.m_pProxy1; + if (!Intersect(pa->leaf->volume, pb->leaf->volume)) { #if DBVT_BP_SORTPAIRS - if(pa->m_uniqueId>pb->m_uniqueId) - btSwap(pa,pb); + if (pa->m_uniqueId > pb->m_uniqueId) + btSwap(pa, pb); #endif - m_paircache->removeOverlappingPair(pa,pb,dispatcher); - --ni;--i; + m_paircache->removeOverlappingPair(pa, pb, dispatcher); + --ni; + --i; } } - if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; + if (pairs.size() > 0) + m_cid = (m_cid + ni) % pairs.size(); + else + m_cid = 0; } } ++m_pid; - m_newpairs=1; - m_needcleanup=false; - if(m_updates_call>0) - { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; } + m_newpairs = 1; + m_needcleanup = false; + if (m_updates_call > 0) + { + m_updates_ratio = m_updates_done / (btScalar)m_updates_call; + } else - { m_updates_ratio=0; } - m_updates_done/=2; - m_updates_call/=2; + { + m_updates_ratio = 0; + } + m_updates_done /= 2; + m_updates_call /= 2; } // -void btDbvtBroadphase::optimize() +void btDbvtBroadphase::optimize() { m_sets[0].optimizeTopDown(); m_sets[1].optimizeTopDown(); } // -btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() +btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() { - return(m_paircache); + return (m_paircache); } // -const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const +const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const { - return(m_paircache); + return (m_paircache); } // -void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const +void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const { + ATTRIBUTE_ALIGNED16(btDbvtVolume) + bounds; - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; - - if(!m_sets[0].empty()) - if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, - m_sets[1].m_root->volume,bounds); + if (!m_sets[0].empty()) + if (!m_sets[1].empty()) + Merge(m_sets[0].m_root->volume, + m_sets[1].m_root->volume, bounds); else - bounds=m_sets[0].m_root->volume; - else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; + bounds = m_sets[0].m_root->volume; + else if (!m_sets[1].empty()) + bounds = m_sets[1].m_root->volume; else - bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); - aabbMin=bounds.Mins(); - aabbMax=bounds.Maxs(); + bounds = btDbvtVolume::FromCR(btVector3(0, 0, 0), 0); + aabbMin = bounds.Mins(); + aabbMax = bounds.Maxs(); } void btDbvtBroadphase::resetPool(btDispatcher* dispatcher) { - int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves; if (!totalObjects) { //reset internal dynamic tree data structures m_sets[0].clear(); m_sets[1].clear(); - - m_deferedcollide = false; - m_needcleanup = true; - m_stageCurrent = 0; - m_fixedleft = 0; - m_fupdates = 1; - m_dupdates = 0; - m_cupdates = 10; - m_newpairs = 1; - m_updates_call = 0; - m_updates_done = 0; - m_updates_ratio = 0; - - m_gid = 0; - m_pid = 0; - m_cid = 0; - for(int i=0;i<=STAGECOUNT;++i) + + m_deferedcollide = false; + m_needcleanup = true; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + + m_gid = 0; + m_pid = 0; + m_cid = 0; + for (int i = 0; i <= STAGECOUNT; ++i) { - m_stageRoots[i]=0; + m_stageRoots[i] = 0; } } } // -void btDbvtBroadphase::printStats() -{} +void btDbvtBroadphase::printStats() +{ +} // #if DBVT_BP_ENABLE_BENCHMARK -struct btBroadphaseBenchmark +struct btBroadphaseBenchmark { - struct Experiment + struct Experiment { - const char* name; - int object_count; - int update_count; - int spawn_count; - int iterations; - btScalar speed; - btScalar amplitude; + const char* name; + int object_count; + int update_count; + int spawn_count; + int iterations; + btScalar speed; + btScalar amplitude; }; - struct Object + struct Object { - btVector3 center; - btVector3 extents; - btBroadphaseProxy* proxy; - btScalar time; - void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) + btVector3 center; + btVector3 extents; + btBroadphaseProxy* proxy; + btScalar time; + void update(btScalar speed, btScalar amplitude, btBroadphaseInterface* pbi) { - time += speed; - center[0] = btCos(time*(btScalar)2.17)*amplitude+ - btSin(time)*amplitude/2; - center[1] = btCos(time*(btScalar)1.38)*amplitude+ - btSin(time)*amplitude; - center[2] = btSin(time*(btScalar)0.777)*amplitude; - pbi->setAabb(proxy,center-extents,center+extents,0); + time += speed; + center[0] = btCos(time * (btScalar)2.17) * amplitude + + btSin(time) * amplitude / 2; + center[1] = btCos(time * (btScalar)1.38) * amplitude + + btSin(time) * amplitude; + center[2] = btSin(time * (btScalar)0.777) * amplitude; + pbi->setAabb(proxy, center - extents, center + extents, 0); } }; - static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } - static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } - static void OutputTime(const char* name,btClock& c,unsigned count=0) + static int UnsignedRand(int range = RAND_MAX - 1) { return (rand() % (range + 1)); } + static btScalar UnitRand() { return (UnsignedRand(16384) / (btScalar)16384); } + static void OutputTime(const char* name, btClock& c, unsigned count = 0) { - const unsigned long us=c.getTimeMicroseconds(); - const unsigned long ms=(us+500)/1000; - const btScalar sec=us/(btScalar)(1000*1000); - if(count>0) - printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); + const unsigned long us = c.getTimeMicroseconds(); + const unsigned long ms = (us + 500) / 1000; + const btScalar sec = us / (btScalar)(1000 * 1000); + if (count > 0) + printf("%s : %u us (%u ms), %.2f/s\r\n", name, us, ms, count / sec); else - printf("%s : %u us (%u ms)\r\n",name,us,ms); + printf("%s : %u us (%u ms)\r\n", name, us, ms); } }; -void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) +void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) { - static const btBroadphaseBenchmark::Experiment experiments[]= - { - {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, - /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, + static const btBroadphaseBenchmark::Experiment experiments[] = + { + {"1024o.10%", 1024, 10, 0, 8192, (btScalar)0.005, (btScalar)100}, + /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ - }; - static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); - btAlignedObjectArray objects; - btClock wallclock; - /* Begin */ - for(int iexp=0;iexp objects; + btClock wallclock; + /* Begin */ + for (int iexp = 0; iexp < nexperiments; ++iexp) { - const btBroadphaseBenchmark::Experiment& experiment=experiments[iexp]; - const int object_count=experiment.object_count; - const int update_count=(object_count*experiment.update_count)/100; - const int spawn_count=(object_count*experiment.spawn_count)/100; - const btScalar speed=experiment.speed; - const btScalar amplitude=experiment.amplitude; - printf("Experiment #%u '%s':\r\n",iexp,experiment.name); - printf("\tObjects: %u\r\n",object_count); - printf("\tUpdate: %u\r\n",update_count); - printf("\tSpawn: %u\r\n",spawn_count); - printf("\tSpeed: %f\r\n",speed); - printf("\tAmplitude: %f\r\n",amplitude); + const btBroadphaseBenchmark::Experiment& experiment = experiments[iexp]; + const int object_count = experiment.object_count; + const int update_count = (object_count * experiment.update_count) / 100; + const int spawn_count = (object_count * experiment.spawn_count) / 100; + const btScalar speed = experiment.speed; + const btScalar amplitude = experiment.amplitude; + printf("Experiment #%u '%s':\r\n", iexp, experiment.name); + printf("\tObjects: %u\r\n", object_count); + printf("\tUpdate: %u\r\n", update_count); + printf("\tSpawn: %u\r\n", spawn_count); + printf("\tSpeed: %f\r\n", speed); + printf("\tAmplitude: %f\r\n", amplitude); srand(180673); - /* Create objects */ + /* Create objects */ wallclock.reset(); objects.reserve(object_count); - for(int i=0;icenter[0]=btBroadphaseBenchmark::UnitRand()*50; - po->center[1]=btBroadphaseBenchmark::UnitRand()*50; - po->center[2]=btBroadphaseBenchmark::UnitRand()*50; - po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; - po->time=btBroadphaseBenchmark::UnitRand()*2000; - po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); + btBroadphaseBenchmark::Object* po = new btBroadphaseBenchmark::Object(); + po->center[0] = btBroadphaseBenchmark::UnitRand() * 50; + po->center[1] = btBroadphaseBenchmark::UnitRand() * 50; + po->center[2] = btBroadphaseBenchmark::UnitRand() * 50; + po->extents[0] = btBroadphaseBenchmark::UnitRand() * 2 + 2; + po->extents[1] = btBroadphaseBenchmark::UnitRand() * 2 + 2; + po->extents[2] = btBroadphaseBenchmark::UnitRand() * 2 + 2; + po->time = btBroadphaseBenchmark::UnitRand() * 2000; + po->proxy = pbi->createProxy(po->center - po->extents, po->center + po->extents, 0, po, 1, 1, 0, 0); objects.push_back(po); } - btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); - /* First update */ + btBroadphaseBenchmark::OutputTime("\tInitialization", wallclock); + /* First update */ wallclock.reset(); - for(int i=0;iupdate(speed,amplitude,pbi); + objects[i]->update(speed, amplitude, pbi); } - btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); - /* Updates */ + btBroadphaseBenchmark::OutputTime("\tFirst update", wallclock); + /* Updates */ wallclock.reset(); - for(int i=0;iupdate(speed,amplitude,pbi); + for (int j = 0; j < update_count; ++j) + { + objects[j]->update(speed, amplitude, pbi); } pbi->calculateOverlappingPairs(0); } - btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); - /* Clean up */ + btBroadphaseBenchmark::OutputTime("\tUpdate", wallclock, experiment.iterations); + /* Clean up */ wallclock.reset(); - for(int i=0;idestroyProxy(objects[i]->proxy,0); + pbi->destroyProxy(objects[i]->proxy, 0); delete objects[i]; } objects.resize(0); - btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); + btBroadphaseBenchmark::OutputTime("\tRelease", wallclock); } - } #else -void btDbvtBroadphase::benchmark(btBroadphaseInterface*) -{} +void btDbvtBroadphase::benchmark(btBroadphaseInterface*) +{ +} #endif #if DBVT_BP_PROFILE -#undef SPC +#undef SPC #endif - diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index 8feb95d51..a71feef53 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -24,15 +24,16 @@ subject to the following restrictions: // Compile time config // -#define DBVT_BP_PROFILE 0 +#define DBVT_BP_PROFILE 0 //#define DBVT_BP_SORTPAIRS 1 -#define DBVT_BP_PREVENTFALSEUPDATE 0 -#define DBVT_BP_ACCURATESLEEPING 0 -#define DBVT_BP_ENABLE_BENCHMARK 0 -#define DBVT_BP_MARGIN (btScalar)0.05 +#define DBVT_BP_PREVENTFALSEUPDATE 0 +#define DBVT_BP_ACCURATESLEEPING 0 +#define DBVT_BP_ENABLE_BENCHMARK 0 +//#define DBVT_BP_MARGIN (btScalar)0.05 +extern btScalar gDbvtMargin; #if DBVT_BP_PROFILE -#define DBVT_BP_PROFILING_RATE 256 +#define DBVT_BP_PROFILING_RATE 256 #include "LinearMath/btQuickprof.h" #endif @@ -41,90 +42,90 @@ subject to the following restrictions: // struct btDbvtProxy : btBroadphaseProxy { - /* Fields */ + /* Fields */ //btDbvtAabbMm aabb; - btDbvtNode* leaf; - btDbvtProxy* links[2]; - int stage; - /* ctor */ - btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) : - btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) + btDbvtNode* leaf; + btDbvtProxy* links[2]; + int stage; + /* ctor */ + btDbvtProxy(const btVector3& aabbMin, const btVector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : btBroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask) { - links[0]=links[1]=0; + links[0] = links[1] = 0; } }; -typedef btAlignedObjectArray btDbvtProxyArray; +typedef btAlignedObjectArray btDbvtProxyArray; ///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt). ///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other. ///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3. -struct btDbvtBroadphase : btBroadphaseInterface +struct btDbvtBroadphase : btBroadphaseInterface { - /* Config */ - enum { - DYNAMIC_SET = 0, /* Dynamic set index */ - FIXED_SET = 1, /* Fixed set index */ - STAGECOUNT = 2 /* Number of stages */ + /* Config */ + enum + { + DYNAMIC_SET = 0, /* Dynamic set index */ + FIXED_SET = 1, /* Fixed set index */ + STAGECOUNT = 2 /* Number of stages */ }; - /* Fields */ - btDbvt m_sets[2]; // Dbvt sets - btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list - btOverlappingPairCache* m_paircache; // Pair cache - btScalar m_prediction; // Velocity prediction - int m_stageCurrent; // Current stage - int m_fupdates; // % of fixed updates per frame - int m_dupdates; // % of dynamic updates per frame - int m_cupdates; // % of cleanup updates per frame - int m_newpairs; // Number of pairs created - int m_fixedleft; // Fixed optimization left - unsigned m_updates_call; // Number of updates call - unsigned m_updates_done; // Number of updates done - btScalar m_updates_ratio; // m_updates_done/m_updates_call - int m_pid; // Parse id - int m_cid; // Cleanup index - int m_gid; // Gen id - bool m_releasepaircache; // Release pair cache on delete - bool m_deferedcollide; // Defere dynamic/static collision to collide call - bool m_needcleanup; // Need to run cleanup? - btAlignedObjectArray< btAlignedObjectArray > m_rayTestStacks; + /* Fields */ + btDbvt m_sets[2]; // Dbvt sets + btDbvtProxy* m_stageRoots[STAGECOUNT + 1]; // Stages list + btOverlappingPairCache* m_paircache; // Pair cache + btScalar m_prediction; // Velocity prediction + int m_stageCurrent; // Current stage + int m_fupdates; // % of fixed updates per frame + int m_dupdates; // % of dynamic updates per frame + int m_cupdates; // % of cleanup updates per frame + int m_newpairs; // Number of pairs created + int m_fixedleft; // Fixed optimization left + unsigned m_updates_call; // Number of updates call + unsigned m_updates_done; // Number of updates done + btScalar m_updates_ratio; // m_updates_done/m_updates_call + int m_pid; // Parse id + int m_cid; // Cleanup index + int m_gid; // Gen id + bool m_releasepaircache; // Release pair cache on delete + bool m_deferedcollide; // Defere dynamic/static collision to collide call + bool m_needcleanup; // Need to run cleanup? + btAlignedObjectArray > m_rayTestStacks; #if DBVT_BP_PROFILE - btClock m_clock; - struct { - unsigned long m_total; - unsigned long m_ddcollide; - unsigned long m_fdcollide; - unsigned long m_cleanup; - unsigned long m_jobcount; - } m_profiling; + btClock m_clock; + struct + { + unsigned long m_total; + unsigned long m_ddcollide; + unsigned long m_fdcollide; + unsigned long m_cleanup; + unsigned long m_jobcount; + } m_profiling; #endif - /* Methods */ - btDbvtBroadphase(btOverlappingPairCache* paircache=0); + /* Methods */ + btDbvtBroadphase(btOverlappingPairCache* paircache = 0); ~btDbvtBroadphase(); - void collide(btDispatcher* dispatcher); - void optimize(); - + void collide(btDispatcher* dispatcher); + void optimize(); + /* btBroadphaseInterface Implementation */ - 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)); - virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); - - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - virtual btOverlappingPairCache* getOverlappingPairCache(); - virtual const btOverlappingPairCache* getOverlappingPairCache() const; - virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; - virtual void printStats(); + 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)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); + virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const; + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); + virtual btOverlappingPairCache* getOverlappingPairCache(); + virtual const btOverlappingPairCache* getOverlappingPairCache() const; + virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const; + virtual void printStats(); ///reset broadphase internal structures, to ensure determinism/reproducability virtual void resetPool(btDispatcher* dispatcher); - void performDeferredRemoval(btDispatcher* dispatcher); - - void setVelocityPrediction(btScalar prediction) + void performDeferredRemoval(btDispatcher* dispatcher); + + void setVelocityPrediction(btScalar prediction) { m_prediction = prediction; } @@ -133,15 +134,13 @@ struct btDbvtBroadphase : btBroadphaseInterface return m_prediction; } - ///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. + ///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. ///it is not part of the btBroadphaseInterface but specific to btDbvtBroadphase. ///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( btBroadphaseProxy* absproxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* /*dispatcher*/); - - static void benchmark(btBroadphaseInterface*); - + void setAabbForceUpdate(btBroadphaseProxy* absproxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/); + static void benchmark(btBroadphaseInterface*); }; #endif diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp index 20768225b..d76d408aa 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp @@ -17,6 +17,4 @@ subject to the following restrictions: btDispatcher::~btDispatcher() { - } - diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 7b0f9489a..b09b7d4d4 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -20,7 +20,7 @@ subject to the following restrictions: class btCollisionAlgorithm; struct btBroadphaseProxy; class btRigidBody; -class btCollisionObject; +class btCollisionObject; class btOverlappingPairCache; struct btCollisionObjectWrapper; @@ -35,33 +35,34 @@ struct btDispatcherInfo DISPATCH_CONTINUOUS }; btDispatcherInfo() - :m_timeStep(btScalar(0.)), - m_stepCount(0), - m_dispatchFunc(DISPATCH_DISCRETE), - m_timeOfImpact(btScalar(1.)), - m_useContinuous(true), - m_debugDraw(0), - m_enableSatConvex(false), - m_enableSPU(true), - m_useEpa(true), - m_allowedCcdPenetration(btScalar(0.04)), - m_useConvexConservativeDistanceUtil(false), - m_convexConservativeDistanceThreshold(0.0f) + : m_timeStep(btScalar(0.)), + m_stepCount(0), + m_dispatchFunc(DISPATCH_DISCRETE), + m_timeOfImpact(btScalar(1.)), + m_useContinuous(true), + m_debugDraw(0), + m_enableSatConvex(false), + m_enableSPU(true), + m_useEpa(true), + m_allowedCcdPenetration(btScalar(0.04)), + m_useConvexConservativeDistanceUtil(false), + m_convexConservativeDistanceThreshold(0.0f), + m_deterministicOverlappingPairs(false) { - } - btScalar m_timeStep; - int m_stepCount; - int m_dispatchFunc; - mutable btScalar m_timeOfImpact; - bool m_useContinuous; - class btIDebugDraw* m_debugDraw; - bool m_enableSatConvex; - bool m_enableSPU; - bool m_useEpa; - btScalar m_allowedCcdPenetration; - bool m_useConvexConservativeDistanceUtil; - btScalar m_convexConservativeDistanceThreshold; + btScalar m_timeStep; + int m_stepCount; + int m_dispatchFunc; + mutable btScalar m_timeOfImpact; + bool m_useContinuous; + class btIDebugDraw* m_debugDraw; + bool m_enableSatConvex; + bool m_enableSPU; + bool m_useEpa; + btScalar m_allowedCcdPenetration; + bool m_useConvexConservativeDistanceUtil; + btScalar m_convexConservativeDistanceThreshold; + bool m_deterministicOverlappingPairs; }; enum ebtDispatcherQueryType @@ -74,40 +75,36 @@ enum ebtDispatcherQueryType ///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic). class btDispatcher { - - public: - virtual ~btDispatcher() ; + virtual ~btDispatcher(); - virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 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; + virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0, const btCollisionObject* b1) = 0; - virtual void releaseManifold(btPersistentManifold* manifold)=0; + virtual void releaseManifold(btPersistentManifold* manifold) = 0; - virtual void clearManifold(btPersistentManifold* manifold)=0; + virtual void clearManifold(btPersistentManifold* manifold) = 0; - virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1) = 0; + virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) = 0; - virtual bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1)=0; + virtual bool needsResponse(const btCollisionObject* body0, const btCollisionObject* body1) = 0; - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) =0; + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) = 0; virtual int getNumManifolds() const = 0; virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0; - virtual btPersistentManifold** getInternalManifoldPointer() = 0; + virtual btPersistentManifold** getInternalManifoldPointer() = 0; - virtual btPoolAllocator* getInternalManifoldPool() = 0; + virtual btPoolAllocator* getInternalManifoldPool() = 0; - virtual const btPoolAllocator* getInternalManifoldPool() const = 0; + virtual const btPoolAllocator* getInternalManifoldPool() const = 0; - virtual void* allocateCollisionAlgorithm(int size) = 0; - - virtual void freeCollisionAlgorithm(void* ptr) = 0; + virtual void* allocateCollisionAlgorithm(int size) = 0; + virtual void freeCollisionAlgorithm(void* ptr) = 0; }; - -#endif //BT_DISPATCHER_H +#endif //BT_DISPATCHER_H diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 55ebf06f1..8ce1087c9 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btOverlappingPairCache.h" #include "btDispatcher.h" @@ -23,127 +21,95 @@ subject to the following restrictions: #include -int gOverlappingPairs = 0; - -int gRemovePairs =0; -int gAddedPairs =0; -int gFindPairs =0; - - - - -btHashedOverlappingPairCache::btHashedOverlappingPairCache(): - m_overlapFilterCallback(0), - m_ghostPairCallback(0) +btHashedOverlappingPairCache::btHashedOverlappingPairCache() : m_overlapFilterCallback(0), + m_ghostPairCallback(0) { - int initialAllocatedSize= 2; + int initialAllocatedSize = 2; m_overlappingPairArray.reserve(initialAllocatedSize); growTables(); } - - - btHashedOverlappingPairCache::~btHashedOverlappingPairCache() { } - - -void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) +void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) { if (pair.m_algorithm && dispatcher) { { pair.m_algorithm->~btCollisionAlgorithm(); dispatcher->freeCollisionAlgorithm(pair.m_algorithm); - pair.m_algorithm=0; + pair.m_algorithm = 0; } } } - - - -void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) { - - class CleanPairCallback : public btOverlapCallback + class CleanPairCallback : public btOverlapCallback { btBroadphaseProxy* m_cleanProxy; - btOverlappingPairCache* m_pairCache; + btOverlappingPairCache* m_pairCache; btDispatcher* m_dispatcher; public: - CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) - :m_cleanProxy(cleanProxy), - m_pairCache(pairCache), - m_dispatcher(dispatcher) + CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) + : m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { if ((pair.m_pProxy0 == m_cleanProxy) || (pair.m_pProxy1 == m_cleanProxy)) { - m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + m_pairCache->cleanOverlappingPair(pair, m_dispatcher); } return false; } - }; - CleanPairCallback cleanPairs(proxy,this,dispatcher); - - processAllOverlappingPairs(&cleanPairs,dispatcher); + CleanPairCallback cleanPairs(proxy, this, dispatcher); + processAllOverlappingPairs(&cleanPairs, dispatcher); } - - - -void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher) { - - class RemovePairCallback : public btOverlapCallback + class RemovePairCallback : public btOverlapCallback { btBroadphaseProxy* m_obsoleteProxy; public: RemovePairCallback(btBroadphaseProxy* obsoleteProxy) - :m_obsoleteProxy(obsoleteProxy) + : m_obsoleteProxy(obsoleteProxy) { } - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { return ((pair.m_pProxy0 == m_obsoleteProxy) || - (pair.m_pProxy1 == m_obsoleteProxy)); + (pair.m_pProxy1 == m_obsoleteProxy)); } - }; - RemovePairCallback removeCallback(proxy); - processAllOverlappingPairs(&removeCallback,dispatcher); + processAllOverlappingPairs(&removeCallback, dispatcher); } - - - - btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - gFindPairs++; - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); + if (proxy0->m_uniqueId > proxy1->m_uniqueId) + btSwap(proxy0, proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); - /*if (proxyId1 > proxyId2) + /*if (proxyId1 > proxyId2) btSwap(proxyId1, proxyId2);*/ - int hash = static_cast(getHash(static_cast(proxyId1), static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + int hash = static_cast(getHash(static_cast(proxyId1), static_cast(proxyId2)) & (m_overlappingPairArray.capacity() - 1)); if (hash >= m_hashTable.size()) { @@ -168,9 +134,8 @@ btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* prox //#include -void btHashedOverlappingPairCache::growTables() +void btHashedOverlappingPairCache::growTables() { - int newCapacity = m_overlappingPairArray.capacity(); if (m_hashTable.size() < newCapacity) @@ -181,10 +146,9 @@ void btHashedOverlappingPairCache::growTables() m_hashTable.resize(newCapacity); m_next.resize(newCapacity); - int i; - for (i= 0; i < newCapacity; ++i) + for (i = 0; i < newCapacity; ++i) { m_hashTable[i] = BT_NULL_PAIR; } @@ -193,35 +157,31 @@ void btHashedOverlappingPairCache::growTables() m_next[i] = BT_NULL_PAIR; } - for(i=0;igetUid(); int proxyId2 = pair.m_pProxy1->getUid(); /*if (proxyId1 > proxyId2) btSwap(proxyId1, proxyId2);*/ - int hashValue = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + 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; } - - } } btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); + if (proxy0->m_uniqueId > proxy1->m_uniqueId) + btSwap(proxy0, proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); /*if (proxyId1 > proxyId2) btSwap(proxyId1, proxyId2);*/ - int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask - + int hash = static_cast(getHash(static_cast(proxyId1), static_cast(proxyId2)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); if (pair != NULL) @@ -243,7 +203,7 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx //this is where we add an actual pair, so also call the 'ghost' if (m_ghostPairCallback) - m_ghostPairCallback->addOverlappingPair(proxy0,proxy1); + m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); int newCapacity = m_overlappingPairArray.capacity(); @@ -251,15 +211,14 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx { growTables(); //hash with new capacity - hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + hash = static_cast(getHash(static_cast(proxyId1), static_cast(proxyId2)) & (m_overlappingPairArray.capacity() - 1)); } - - pair = new (mem) btBroadphasePair(*proxy0,*proxy1); -// pair->m_pProxy0 = proxy0; -// pair->m_pProxy1 = proxy1; + + pair = new (mem) btBroadphasePair(*proxy0, *proxy1); + // pair->m_pProxy0 = proxy0; + // pair->m_pProxy1 = proxy1; pair->m_algorithm = 0; pair->m_internalTmpValue = 0; - m_next[count] = m_hashTable[hash]; m_hashTable[hash] = count; @@ -267,20 +226,17 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx return pair; } - - -void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) +void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher) { - gRemovePairs++; - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); + if (proxy0->m_uniqueId > proxy1->m_uniqueId) + btSwap(proxy0, proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); - /*if (proxyId1 > proxyId2) + /*if (proxyId1 > proxyId2) btSwap(proxyId1, proxyId2);*/ - int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + int hash = static_cast(getHash(static_cast(proxyId1), static_cast(proxyId2)) & (m_overlappingPairArray.capacity() - 1)); btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); if (pair == NULL) @@ -288,7 +244,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro return 0; } - cleanOverlappingPair(*pair,dispatcher); + cleanOverlappingPair(*pair, dispatcher); void* userData = pair->m_internalInfo1; @@ -326,7 +282,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro int lastPairIndex = m_overlappingPairArray.size() - 1; if (m_ghostPairCallback) - m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); + m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher); // If the removed pair is the last pair, we are done. if (lastPairIndex == pairIndex) @@ -337,8 +293,8 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro // Remove the last pair from the hash table. const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex]; - /* missing swap here too, Nat. */ - int lastHash = static_cast(getHash(static_cast(last->m_pProxy0->getUid()), static_cast(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1)); + /* missing swap here too, Nat. */ + int lastHash = static_cast(getHash(static_cast(last->m_pProxy0->getUid()), static_cast(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity() - 1)); index = m_hashTable[lastHash]; btAssert(index != BT_NULL_PAIR); @@ -373,43 +329,105 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro } //#include #include "LinearMath/btQuickprof.h" -void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) +void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher) { BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs"); int i; -// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); - for (i=0;iprocessOverlap(*pair)) { - removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher); - - gOverlappingPairs--; - } else + removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher); + } + else { i++; } } } -void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) +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; i < indices.size(); i++) + { + const btBroadphasePair& p = pa[i]; + const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_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; i < indices.size();) + { + btBroadphasePair* pair = &pa[indices[i].m_orgIndex]; + if (callback->processOverlap(*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 btBroadphasePairArray tmpPairs; int i; - for (i=0;iremoveOverlappingPair(proxy0, proxy1,dispatcher); - - m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); + m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher); + + m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1); m_overlappingPairArray.pop_back(); return userData; } @@ -451,99 +465,73 @@ void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro return 0; } - - - - - - - -btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { //don't add overlap with own btAssert(proxy0 != proxy1); - if (!needsBroadphaseCollision(proxy0,proxy1)) + if (!needsBroadphaseCollision(proxy0, proxy1)) return 0; - + void* mem = &m_overlappingPairArray.expandNonInitializing(); - btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); - - gOverlappingPairs++; - gAddedPairs++; - + btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0, *proxy1); + if (m_ghostPairCallback) m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); return pair; - } ///this findPair becomes really slow. Either sort the list to speedup the query, or ///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. ///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) ///Also we can use a 2D bitmap, which can be useful for a future GPU implementation - btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - if (!needsBroadphaseCollision(proxy0,proxy1)) + if (!needsBroadphaseCollision(proxy0, proxy1)) return 0; - btBroadphasePair tmpPair(*proxy0,*proxy1); + btBroadphasePair tmpPair(*proxy0, *proxy1); int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); if (findIndex < m_overlappingPairArray.size()) { //btAssert(it != m_overlappingPairSet.end()); - btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; + btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; return pair; } return 0; } - - - - - - - - - //#include -void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) +void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher) { - int i; - for (i=0;iprocessOverlap(*pair)) { - cleanOverlappingPair(*pair,dispatcher); + cleanOverlappingPair(*pair, dispatcher); pair->m_pProxy0 = 0; pair->m_pProxy1 = 0; - m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1); m_overlappingPairArray.pop_back(); - gOverlappingPairs--; - } else + } + else { i++; } } } - - - -btSortedOverlappingPairCache::btSortedOverlappingPairCache(): - m_blockedForChanges(false), - m_hasDeferredRemoval(true), - m_overlapFilterCallback(0), - m_ghostPairCallback(0) +btSortedOverlappingPairCache::btSortedOverlappingPairCache() : m_blockedForChanges(false), + m_hasDeferredRemoval(true), + m_overlapFilterCallback(0), + m_ghostPairCallback(0) { - int initialAllocatedSize= 2; + int initialAllocatedSize = 2; m_overlappingPairArray.reserve(initialAllocatedSize); } @@ -551,82 +539,73 @@ btSortedOverlappingPairCache::~btSortedOverlappingPairCache() { } -void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) +void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) { if (pair.m_algorithm) { { pair.m_algorithm->~btCollisionAlgorithm(); dispatcher->freeCollisionAlgorithm(pair.m_algorithm); - pair.m_algorithm=0; - gRemovePairs--; + pair.m_algorithm = 0; } } } - -void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) { - - class CleanPairCallback : public btOverlapCallback + class CleanPairCallback : public btOverlapCallback { btBroadphaseProxy* m_cleanProxy; - btOverlappingPairCache* m_pairCache; + btOverlappingPairCache* m_pairCache; btDispatcher* m_dispatcher; public: - CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) - :m_cleanProxy(cleanProxy), - m_pairCache(pairCache), - m_dispatcher(dispatcher) + CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) + : m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { if ((pair.m_pProxy0 == m_cleanProxy) || (pair.m_pProxy1 == m_cleanProxy)) { - m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + m_pairCache->cleanOverlappingPair(pair, m_dispatcher); } return false; } - }; - CleanPairCallback cleanPairs(proxy,this,dispatcher); - - processAllOverlappingPairs(&cleanPairs,dispatcher); + CleanPairCallback cleanPairs(proxy, this, dispatcher); + processAllOverlappingPairs(&cleanPairs, dispatcher); } - -void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher) { - - class RemovePairCallback : public btOverlapCallback + class RemovePairCallback : public btOverlapCallback { btBroadphaseProxy* m_obsoleteProxy; public: RemovePairCallback(btBroadphaseProxy* obsoleteProxy) - :m_obsoleteProxy(obsoleteProxy) + : m_obsoleteProxy(obsoleteProxy) { } - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { return ((pair.m_pProxy0 == m_obsoleteProxy) || - (pair.m_pProxy1 == m_obsoleteProxy)); + (pair.m_pProxy1 == m_obsoleteProxy)); } - }; RemovePairCallback removeCallback(proxy); - processAllOverlappingPairs(&removeCallback,dispatcher); + processAllOverlappingPairs(&removeCallback, dispatcher); } -void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) +void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) { //should already be sorted } - diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index f7be7d45b..a85782bc8 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BT_OVERLAPPING_PAIR_CACHE_H #define BT_OVERLAPPING_PAIR_CACHE_H - #include "btBroadphaseInterface.h" #include "btBroadphaseProxy.h" #include "btOverlappingPairCallback.h" @@ -24,177 +23,163 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" class btDispatcher; -typedef btAlignedObjectArray btBroadphasePairArray; +typedef btAlignedObjectArray btBroadphasePairArray; -struct btOverlapCallback +struct btOverlapCallback { virtual ~btOverlapCallback() - {} + { + } //return true for deletion of the pair - virtual bool processOverlap(btBroadphasePair& pair) = 0; - + virtual bool processOverlap(btBroadphasePair& pair) = 0; }; struct btOverlapFilterCallback { virtual ~btOverlapFilterCallback() - {} + { + } // return true when pairs need collision - virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0; + virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const = 0; }; - - - - - - -extern int gRemovePairs; -extern int gAddedPairs; -extern int gFindPairs; - -const int BT_NULL_PAIR=0xffffffff; +const int BT_NULL_PAIR = 0xffffffff; ///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases. ///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations. class btOverlappingPairCache : public btOverlappingPairCallback { public: - virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor + virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor - virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0; - - virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0; + virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0; - virtual btBroadphasePairArray& getOverlappingPairArray() = 0; + virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0; - virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0; + virtual btBroadphasePairArray& getOverlappingPairArray() = 0; + + virtual void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) = 0; virtual int getNumOverlappingPairs() const = 0; - virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0; + virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0; - virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; + virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; + 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; - - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0; - - virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0; + virtual bool hasDeferredRemoval() = 0; + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) = 0; + virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0; }; /// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com -ATTRIBUTE_ALIGNED16(class) btHashedOverlappingPairCache : public btOverlappingPairCache +ATTRIBUTE_ALIGNED16(class) +btHashedOverlappingPairCache : public btOverlappingPairCache { - btBroadphasePairArray m_overlappingPairArray; + btBroadphasePairArray m_overlappingPairArray; btOverlapFilterCallback* m_overlapFilterCallback; protected: - - btAlignedObjectArray m_hashTable; - btAlignedObjectArray m_next; - btOverlappingPairCallback* m_ghostPairCallback; - + btAlignedObjectArray m_hashTable; + btAlignedObjectArray m_next; + btOverlappingPairCallback* m_ghostPairCallback; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btHashedOverlappingPairCache(); virtual ~btHashedOverlappingPairCache(); - - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + void removeOverlappingPairsContainingProxy(btBroadphaseProxy * proxy, btDispatcher * dispatcher); - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); - - SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const + virtual void* removeOverlappingPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1, btDispatcher * dispatcher); + + SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) const { if (m_overlapFilterCallback) - return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1); bool collides = (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 btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) { - gAddedPairs++; - - if (!needsBroadphaseCollision(proxy0,proxy1)) + if (!needsBroadphaseCollision(proxy0, proxy1)) return 0; - return internalAddPair(proxy0,proxy1); + return internalAddPair(proxy0, proxy1); } - + void cleanProxyFromPairs(btBroadphaseProxy * proxy, btDispatcher * dispatcher); - void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher * dispatcher); - - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); + virtual void processAllOverlappingPairs(btOverlapCallback * callback, btDispatcher * dispatcher, const struct btDispatcherInfo& dispatchInfo); - virtual btBroadphasePair* getOverlappingPairArrayPtr() + virtual btBroadphasePair* getOverlappingPairArrayPtr() { return &m_overlappingPairArray[0]; } - const btBroadphasePair* getOverlappingPairArrayPtr() const + const btBroadphasePair* getOverlappingPairArrayPtr() const { return &m_overlappingPairArray[0]; } - btBroadphasePairArray& getOverlappingPairArray() + btBroadphasePairArray& getOverlappingPairArray() { return m_overlappingPairArray; } - const btBroadphasePairArray& getOverlappingPairArray() const + const btBroadphasePairArray& getOverlappingPairArray() const { return m_overlappingPairArray; } - void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); + void cleanOverlappingPair(btBroadphasePair & pair, btDispatcher * dispatcher); - - - btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); + btBroadphasePair* findPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1); int GetCount() const { return m_overlappingPairArray.size(); } -// btBroadphasePair* GetPairs() { return m_pairs; } + // btBroadphasePair* GetPairs() { return m_pairs; } btOverlapFilterCallback* getOverlapFilterCallback() { return m_overlapFilterCallback; } - void setOverlapFilterCallback(btOverlapFilterCallback* callback) + void setOverlapFilterCallback(btOverlapFilterCallback * callback) { m_overlapFilterCallback = callback; } - int getNumOverlappingPairs() const + int getNumOverlappingPairs() const { return m_overlappingPairArray.size(); } -private: - - btBroadphasePair* internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - void growTables(); +private: + btBroadphasePair* internalAddPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1); + + void growTables(); SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2) - { + { return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2; } @@ -214,40 +199,37 @@ private: } */ - SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) { unsigned int key = proxyId1 | (proxyId2 << 16); // Thomas Wang's hash key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); key += ~(key << 11); - key ^= (key >> 16); + key ^= (key >> 16); return key; } - - - SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash) + SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1, int hash) { int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); - #if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. +#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. if (proxyId1 > proxyId2) btSwap(proxyId1, proxyId2); - #endif +#endif int index = m_hashTable[hash]; - - while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + + while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) { index = m_next[index]; } - if ( index == BT_NULL_PAIR ) + if (index == BT_NULL_PAIR) { return NULL; } @@ -257,155 +239,136 @@ private: return &m_overlappingPairArray[index]; } - virtual bool hasDeferredRemoval() + virtual bool hasDeferredRemoval() { return false; } - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + virtual void setInternalGhostPairCallback(btOverlappingPairCallback * ghostPairCallback) { m_ghostPairCallback = ghostPairCallback; } - virtual void sortOverlappingPairs(btDispatcher* dispatcher); - - - + virtual void sortOverlappingPairs(btDispatcher * dispatcher); }; - - - ///btSortedOverlappingPairCache maintains the objects with overlapping AABB ///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase -class btSortedOverlappingPairCache : public btOverlappingPairCache +class btSortedOverlappingPairCache : public btOverlappingPairCache { - protected: - //avoid brute-force finding all the time - btBroadphasePairArray m_overlappingPairArray; +protected: + //avoid brute-force finding all the time + btBroadphasePairArray m_overlappingPairArray; - //during the dispatch, check that user doesn't destroy/create proxy - bool m_blockedForChanges; + //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 - btOverlapFilterCallback* m_overlapFilterCallback; + ///by default, do the removal during the pair traversal + bool m_hasDeferredRemoval; - btOverlappingPairCallback* m_ghostPairCallback; + //if set, use the callback instead of the built in filter in needBroadphaseCollision + btOverlapFilterCallback* m_overlapFilterCallback; - public: - - btSortedOverlappingPairCache(); - virtual ~btSortedOverlappingPairCache(); + btOverlappingPairCallback* m_ghostPairCallback; - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); +public: + btSortedOverlappingPairCache(); + virtual ~btSortedOverlappingPairCache(); - void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); + virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* dispatcher); - void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); - - btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher); - btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - - void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher); - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); + btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); - inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const - { - if (m_overlapFilterCallback) - return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher); - bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; - collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); - - return collides; - } - - btBroadphasePairArray& getOverlappingPairArray() - { - return m_overlappingPairArray; - } + void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher); - const btBroadphasePairArray& getOverlappingPairArray() const - { - return m_overlappingPairArray; - } + inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const + { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1); - + bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + return collides; + } - btBroadphasePair* getOverlappingPairArrayPtr() - { - return &m_overlappingPairArray[0]; - } + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } - const btBroadphasePair* getOverlappingPairArrayPtr() const - { - return &m_overlappingPairArray[0]; - } + const btBroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } - int getNumOverlappingPairs() const - { - return m_overlappingPairArray.size(); - } - - btOverlapFilterCallback* getOverlapFilterCallback() - { - return m_overlapFilterCallback; - } + btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } - void setOverlapFilterCallback(btOverlapFilterCallback* callback) - { - m_overlapFilterCallback = callback; - } + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } - virtual bool hasDeferredRemoval() - { - return m_hasDeferredRemoval; - } + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) - { - m_ghostPairCallback = ghostPairCallback; - } + btOverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } - virtual void sortOverlappingPairs(btDispatcher* dispatcher); - + void setOverlapFilterCallback(btOverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } + virtual bool hasDeferredRemoval() + { + return m_hasDeferredRemoval; + } + + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + + virtual void sortOverlappingPairs(btDispatcher* dispatcher); }; - - ///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing. class btNullPairCache : public btOverlappingPairCache { - - btBroadphasePairArray m_overlappingPairArray; + btBroadphasePairArray m_overlappingPairArray; public: - - virtual btBroadphasePair* getOverlappingPairArrayPtr() + virtual btBroadphasePair* getOverlappingPairArrayPtr() { return &m_overlappingPairArray[0]; } - const btBroadphasePair* getOverlappingPairArrayPtr() const + const btBroadphasePair* getOverlappingPairArrayPtr() const { return &m_overlappingPairArray[0]; } - btBroadphasePairArray& getOverlappingPairArray() + btBroadphasePairArray& getOverlappingPairArray() { return m_overlappingPairArray; } - - virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/) - { + virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/, btDispatcher* /*dispatcher*/) + { } virtual int getNumOverlappingPairs() const @@ -413,16 +376,15 @@ public: return 0; } - virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) - { - - } - - virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) + virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/, btDispatcher* /*dispatcher*/) { } - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/) + virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) + { + } + + virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* /*dispatcher*/) { } @@ -431,39 +393,33 @@ public: return 0; } - virtual bool hasDeferredRemoval() + virtual bool hasDeferredRemoval() { return true; } - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) { - } - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/) + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/) { return 0; } - virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/) + virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/, btDispatcher* /*dispatcher*/) { return 0; } - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/, btDispatcher* /*dispatcher*/) { } - - virtual void sortOverlappingPairs(btDispatcher* dispatcher) + + virtual void sortOverlappingPairs(btDispatcher* dispatcher) { - (void) dispatcher; + (void)dispatcher; } - - }; - -#endif //BT_OVERLAPPING_PAIR_CACHE_H - - +#endif //BT_OVERLAPPING_PAIR_CACHE_H diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h index 3e069fa5e..d16c72542 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h @@ -18,26 +18,24 @@ subject to the following restrictions: #define OVERLAPPING_PAIR_CALLBACK_H class btDispatcher; -struct btBroadphasePair; +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() {} - + btOverlappingPairCallback() {} + public: virtual ~btOverlappingPairCallback() { - } - - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) = 0; - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) = 0; + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0; - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) = 0; + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher) = 0; + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0, btDispatcher* dispatcher) = 0; }; -#endif //OVERLAPPING_PAIR_CALLBACK_H +#endif //OVERLAPPING_PAIR_CALLBACK_H diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index 875d89c53..b814fd84d 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -21,43 +21,38 @@ subject to the following restrictions: #define RAYAABB2 -btQuantizedBvh::btQuantizedBvh() : - m_bulletVersion(BT_BULLET_VERSION), - m_useQuantization(false), - //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) - m_traversalMode(TRAVERSAL_STACKLESS) - //m_traversalMode(TRAVERSAL_RECURSIVE) - ,m_subtreeHeaderCount(0) //PCK: add this line +btQuantizedBvh::btQuantizedBvh() : m_bulletVersion(BT_BULLET_VERSION), + m_useQuantization(false), + //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) + m_traversalMode(TRAVERSAL_STACKLESS) + //m_traversalMode(TRAVERSAL_RECURSIVE) + , + m_subtreeHeaderCount(0) //PCK: add this line { - m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY); - m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + m_bvhAabbMin.setValue(-SIMD_INFINITY, -SIMD_INFINITY, -SIMD_INFINITY); + m_bvhAabbMax.setValue(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY); } - - - - void btQuantizedBvh::buildInternal() { ///assumes that caller filled in the m_quantizedLeafNodes m_useQuantization = true; int numLeafNodes = 0; - + if (m_useQuantization) { //now we have an array of leafnodes in m_leafNodes numLeafNodes = m_quantizedLeafNodes.size(); - m_quantizedContiguousNodes.resize(2*numLeafNodes); - + m_quantizedContiguousNodes.resize(2 * numLeafNodes); } m_curNodeIndex = 0; - buildTree(0,numLeafNodes); + 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()) + if (m_useQuantization && !m_SubtreeHeaders.size()) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); @@ -73,29 +68,24 @@ void btQuantizedBvh::buildInternal() m_leafNodes.clear(); } - - ///just for debugging, to visualize the individual patches/subtrees #ifdef DEBUG_PATCH_COLORS -btVector3 color[4]= -{ - btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1), - btVector3(0,1,1) -}; -#endif //DEBUG_PATCH_COLORS +btVector3 color[4] = + { + btVector3(1, 0, 0), + btVector3(0, 1, 0), + btVector3(0, 0, 1), + btVector3(0, 1, 1)}; +#endif //DEBUG_PATCH_COLORS - - -void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin) +void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, btScalar quantizationMargin) { //enlarge the AABB to avoid division by zero when initializing the quantization values - btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); + btVector3 clampValue(quantizationMargin, quantizationMargin, quantizationMargin); m_bvhAabbMin = bvhAabbMin - clampValue; m_bvhAabbMax = bvhAabbMax + clampValue; btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin; - m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize; + m_bvhQuantization = btVector3(btScalar(65533.0), btScalar(65533.0), btScalar(65533.0)) / aabbSize; m_useQuantization = true; @@ -103,25 +93,22 @@ void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btV unsigned short vecIn[3]; btVector3 v; { - quantize(vecIn,m_bvhAabbMin,false); + quantize(vecIn, m_bvhAabbMin, false); 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); - m_bvhAabbMax.setMax(v+clampValue); + m_bvhAabbMin.setMin(v - clampValue); } aabbSize = m_bvhAabbMax - m_bvhAabbMin; - m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize; + m_bvhQuantization = btVector3(btScalar(65533.0), btScalar(65533.0), btScalar(65533.0)) / aabbSize; + { + quantize(vecIn, m_bvhAabbMax, true); + v = unQuantize(vecIn); + m_bvhAabbMax.setMax(v + clampValue); + } + aabbSize = m_bvhAabbMax - m_bvhAabbMin; + m_bvhQuantization = btVector3(btScalar(65533.0), btScalar(65533.0), btScalar(65533.0)) / aabbSize; } } - - - btQuantizedBvh::~btQuantizedBvh() { } @@ -129,104 +116,100 @@ btQuantizedBvh::~btQuantizedBvh() #ifdef DEBUG_TREE_BUILDING int gStackDepth = 0; int gMaxStackDepth = 0; -#endif //DEBUG_TREE_BUILDING +#endif //DEBUG_TREE_BUILDING -void btQuantizedBvh::buildTree (int startIndex,int endIndex) +void btQuantizedBvh::buildTree(int startIndex, int endIndex) { #ifdef DEBUG_TREE_BUILDING gStackDepth++; if (gStackDepth > gMaxStackDepth) gMaxStackDepth = gStackDepth; -#endif //DEBUG_TREE_BUILDING - +#endif //DEBUG_TREE_BUILDING int splitAxis, splitIndex, i; - int numIndices =endIndex-startIndex; + int numIndices = endIndex - startIndex; int curIndex = m_curNodeIndex; - btAssert(numIndices>0); + btAssert(numIndices > 0); - if (numIndices==1) + if (numIndices == 1) { #ifdef DEBUG_TREE_BUILDING gStackDepth--; -#endif //DEBUG_TREE_BUILDING - - assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex); +#endif //DEBUG_TREE_BUILDING + + assignInternalNodeFromLeafNode(m_curNodeIndex, startIndex); m_curNodeIndex++; - return; + 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); + splitAxis = calcSplittingAxis(startIndex, endIndex); + + splitIndex = sortAndCalcSplittingIndex(startIndex, endIndex, splitAxis); int internalNodeIndex = m_curNodeIndex; - + //set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value. //the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values - setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization - setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization - - - for (i=startIndex;im_escapeIndex; - + int leftChildNodexIndex = m_curNodeIndex; //build left child tree - buildTree(startIndex,splitIndex); + buildTree(startIndex, splitIndex); int rightChildNodexIndex = m_curNodeIndex; //build right child tree - buildTree(splitIndex,endIndex); + buildTree(splitIndex, endIndex); #ifdef DEBUG_TREE_BUILDING gStackDepth--; -#endif //DEBUG_TREE_BUILDING +#endif //DEBUG_TREE_BUILDING int escapeIndex = m_curNodeIndex - curIndex; if (m_useQuantization) { //escapeIndex is the number of nodes of this subtree - const int sizeQuantizedNode =sizeof(btQuantizedBvhNode); + const int sizeQuantizedNode = sizeof(btQuantizedBvhNode); const int treeSizeInBytes = escapeIndex * sizeQuantizedNode; if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) { - updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex); + updateSubtreeHeaders(leftChildNodexIndex, rightChildNodexIndex); } - } else + } + else { - } - setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex); - + setInternalNodeEscapeIndex(internalNodeIndex, escapeIndex); } -void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex) +void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex) { btAssert(m_useQuantization); btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex]; int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex(); - int leftSubTreeSizeInBytes = leftSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); - + int leftSubTreeSizeInBytes = leftSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); + btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex]; int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex(); - int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); + int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); - if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + if (leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); subtree.setAabbFromQuantizeNode(leftChildNode); @@ -234,7 +217,7 @@ void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChild subtree.m_subtreeSize = leftSubTreeSize; } - if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + if (rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); subtree.setAabbFromQuantizeNode(rightChildNode); @@ -246,32 +229,31 @@ void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChild m_subtreeHeaderCount = m_SubtreeHeaders.size(); } - -int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis) +int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis) { int i; - int splitIndex =startIndex; + int splitIndex = startIndex; int numIndices = endIndex - startIndex; btScalar splitValue; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - for (i=startIndex;i splitValue) { //swap - swapLeafNodes(i,splitIndex); + swapLeafNodes(i, splitIndex); splitIndex++; } } @@ -281,56 +263,53 @@ int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int sp //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) + //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))); - + int rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); + if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); + bool unbal = (splitIndex == startIndex) || (splitIndex == (endIndex)); (void)unbal; btAssert(!unbal); return splitIndex; } - -int btQuantizedBvh::calcSplittingAxis(int startIndex,int endIndex) +int btQuantizedBvh::calcSplittingAxis(int startIndex, int endIndex) { int i; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); - int numIndices = endIndex-startIndex; + btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.)); + int numIndices = endIndex - startIndex; - for (i=startIndex;im_aabbMinOrg,rootNode->m_aabbMaxOrg); + aabbOverlap = TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode->m_aabbMinOrg, rootNode->m_aabbMaxOrg); isLeafNode = rootNode->m_escapeIndex == -1; - + //PCK: unsigned instead of bool if (isLeafNode && (aabbOverlap != 0)) { - nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); - } - + nodeCallback->processNode(rootNode->m_subPart, rootNode->m_triangleIndex); + } + //PCK: unsigned instead of bool if ((aabbOverlap != 0) || isLeafNode) { rootNode++; curIndex++; - } else + } + else { escapeIndex = rootNode->m_escapeIndex; rootNode += escapeIndex; @@ -410,7 +389,6 @@ void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const } if (maxIterations < walkIterations) maxIterations = walkIterations; - } /* @@ -434,39 +412,38 @@ void btQuantizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback } */ -void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode, btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const { btAssert(m_useQuantization); - + bool isLeafNode; //PCK: unsigned instead of bool unsigned aabbOverlap; //PCK: unsigned instead of bool - aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax); + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, currentNode->m_quantizedAabbMin, currentNode->m_quantizedAabbMax); isLeafNode = currentNode->isLeafNode(); - + //PCK: unsigned instead of bool if (aabbOverlap != 0) { if (isLeafNode) { - nodeCallback->processNode(currentNode->getPartId(),currentNode->getTriangleIndex()); - } else + nodeCallback->processNode(currentNode->getPartId(), currentNode->getTriangleIndex()); + } + else { //process left and right children - const btQuantizedBvhNode* leftChildNode = currentNode+1; - walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + const btQuantizedBvhNode* leftChildNode = currentNode + 1; + walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax); - const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex(); - walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode + 1 : leftChildNode + leftChildNode->getEscapeIndex(); + walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax); } - } + } } - - -void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const +void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const { btAssert(!m_useQuantization); @@ -475,11 +452,11 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall int walkIterations = 0; bool isLeafNode; //PCK: unsigned instead of bool - unsigned aabbOverlap=0; - unsigned rayBoxOverlap=0; + unsigned aabbOverlap = 0; + unsigned rayBoxOverlap = 0; btScalar lambda_max = 1.0; - - /* Quick pruning by quantized box */ + + /* Quick pruning by quantized box */ btVector3 rayAabbMin = raySource; btVector3 rayAabbMax = raySource; rayAabbMin.setMin(rayTarget); @@ -490,15 +467,15 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall rayAabbMax += aabbMax; #ifdef RAYAABB2 - btVector3 rayDir = (rayTarget-raySource); - rayDir.normalize (); - lambda_max = rayDir.dot(rayTarget-raySource); + btVector3 rayDir = (rayTarget - raySource); + rayDir.normalize(); + lambda_max = rayDir.dot(rayTarget - raySource); ///what about division by zero? --> just set rayDirection[i] to 1.0 btVector3 rayDirectionInverse; rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; - unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + unsigned int sign[3] = {rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; #endif btVector3 bounds[2]; @@ -507,7 +484,7 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall { btScalar param = 1.0; //catch bugs in tree data - btAssert (walkIterations < m_curNodeIndex); + btAssert(walkIterations < m_curNodeIndex); walkIterations++; @@ -517,34 +494,35 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall bounds[0] -= aabbMax; bounds[1] -= aabbMin; - aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg); + aabbOverlap = TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, rootNode->m_aabbMinOrg, rootNode->m_aabbMaxOrg); //perhaps profile if it is worth doing the aabbOverlap test first #ifdef RAYAABB2 - ///careful with this check: need to check division by zero (above) and fix the unQuantize method - ///thanks Joerg/hiker for the reproduction case! - ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 - rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false; + ///careful with this check: need to check division by zero (above) and fix the unQuantize method + ///thanks Joerg/hiker for the reproduction case! + ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 + rayBoxOverlap = aabbOverlap ? btRayAabb2(raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false; #else btVector3 normal; - rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal); + rayBoxOverlap = btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); #endif isLeafNode = rootNode->m_escapeIndex == -1; - + //PCK: unsigned instead of bool if (isLeafNode && (rayBoxOverlap != 0)) { - nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); - } - + nodeCallback->processNode(rootNode->m_subPart, rootNode->m_triangleIndex); + } + //PCK: unsigned instead of bool if ((rayBoxOverlap != 0) || isLeafNode) { rootNode++; curIndex++; - } else + } + else { escapeIndex = rootNode->m_escapeIndex; rootNode += escapeIndex; @@ -553,15 +531,12 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall } if (maxIterations < walkIterations) maxIterations = walkIterations; - } - - -void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const +void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const { btAssert(m_useQuantization); - + int curIndex = startNodeIndex; int walkIterations = 0; int subTreeSize = endNodeIndex - startNodeIndex; @@ -569,7 +544,7 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; int escapeIndex; - + bool isLeafNode; //PCK: unsigned instead of bool unsigned boxBoxOverlap = 0; @@ -578,14 +553,14 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* btScalar lambda_max = 1.0; #ifdef RAYAABB2 - btVector3 rayDirection = (rayTarget-raySource); - rayDirection.normalize (); - lambda_max = rayDirection.dot(rayTarget-raySource); + btVector3 rayDirection = (rayTarget - raySource); + rayDirection.normalize(); + lambda_max = rayDirection.dot(rayTarget - raySource); ///what about division by zero? --> just set rayDirection[i] to 1.0 rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0]; rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[1]; rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[2]; - unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; + unsigned int sign[3] = {rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; #endif /* Quick pruning by quantized box */ @@ -600,37 +575,36 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* unsigned short int quantizedQueryAabbMin[3]; unsigned short int quantizedQueryAabbMax[3]; - quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0); - quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1); + quantizeWithClamp(quantizedQueryAabbMin, rayAabbMin, 0); + quantizeWithClamp(quantizedQueryAabbMax, rayAabbMax, 1); while (curIndex < endNodeIndex) { - //#define VISUALLY_ANALYZE_BVH 1 #ifdef VISUALLY_ANALYZE_BVH //some code snippet to debugDraw aabb, to visually analyze bvh structure static int drawPatch = 0; //need some global access to a debugDrawer extern btIDebugDraw* debugDrawerPtr; - if (curIndex==drawPatch) + if (curIndex == drawPatch) { - btVector3 aabbMin,aabbMax; + btVector3 aabbMin, aabbMax; aabbMin = unQuantize(rootNode->m_quantizedAabbMin); aabbMax = unQuantize(rootNode->m_quantizedAabbMax); - btVector3 color(1,0,0); - debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + btVector3 color(1, 0, 0); + debugDrawerPtr->drawAabb(aabbMin, aabbMax, color); } -#endif//VISUALLY_ANALYZE_BVH +#endif //VISUALLY_ANALYZE_BVH //catch bugs in tree data - btAssert (walkIterations < subTreeSize); + btAssert(walkIterations < subTreeSize); walkIterations++; //PCK: unsigned instead of bool // only interested if this is closer than any previous hit btScalar param = 1.0; rayBoxOverlap = 0; - boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode->m_quantizedAabbMin, rootNode->m_quantizedAabbMax); isLeafNode = rootNode->isLeafNode(); if (boxBoxOverlap) { @@ -655,24 +629,25 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 //BT_PROFILE("btRayAabb2"); - rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); - + rayBoxOverlap = btRayAabb2(raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); + #else - rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); + rayBoxOverlap = true; //btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); #endif } - + if (isLeafNode && rayBoxOverlap) { - nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); + nodeCallback->processNode(rootNode->getPartId(), rootNode->getTriangleIndex()); } - + //PCK: unsigned instead of bool if ((rayBoxOverlap != 0) || isLeafNode) { rootNode++; curIndex++; - } else + } + else { escapeIndex = rootNode->getEscapeIndex(); rootNode += escapeIndex; @@ -681,13 +656,12 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* } if (maxIterations < walkIterations) maxIterations = walkIterations; - } -void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const +void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const { btAssert(m_useQuantization); - + int curIndex = startNodeIndex; int walkIterations = 0; int subTreeSize = endNodeIndex - startNodeIndex; @@ -695,49 +669,49 @@ void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; int escapeIndex; - + bool isLeafNode; //PCK: unsigned instead of bool unsigned aabbOverlap; while (curIndex < endNodeIndex) { - //#define VISUALLY_ANALYZE_BVH 1 #ifdef VISUALLY_ANALYZE_BVH //some code snippet to debugDraw aabb, to visually analyze bvh structure static int drawPatch = 0; //need some global access to a debugDrawer extern btIDebugDraw* debugDrawerPtr; - if (curIndex==drawPatch) + if (curIndex == drawPatch) { - btVector3 aabbMin,aabbMax; + btVector3 aabbMin, aabbMax; aabbMin = unQuantize(rootNode->m_quantizedAabbMin); aabbMax = unQuantize(rootNode->m_quantizedAabbMax); - btVector3 color(1,0,0); - debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + btVector3 color(1, 0, 0); + debugDrawerPtr->drawAabb(aabbMin, aabbMax, color); } -#endif//VISUALLY_ANALYZE_BVH +#endif //VISUALLY_ANALYZE_BVH //catch bugs in tree data - btAssert (walkIterations < subTreeSize); + btAssert(walkIterations < subTreeSize); walkIterations++; //PCK: unsigned instead of bool - aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode->m_quantizedAabbMin, rootNode->m_quantizedAabbMax); isLeafNode = rootNode->isLeafNode(); - + if (isLeafNode && aabbOverlap) { - nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); - } - + nodeCallback->processNode(rootNode->getPartId(), rootNode->getTriangleIndex()); + } + //PCK: unsigned instead of bool if ((aabbOverlap != 0) || isLeafNode) { rootNode++; curIndex++; - } else + } + else { escapeIndex = rootNode->getEscapeIndex(); rootNode += escapeIndex; @@ -746,40 +720,36 @@ void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb } if (maxIterations < walkIterations) maxIterations = walkIterations; - } //This traversal can be called from Playstation 3 SPU -void btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +void btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const { btAssert(m_useQuantization); int i; - - for (i=0;im_SubtreeHeaders.size();i++) + for (i = 0; i < this->m_SubtreeHeaders.size(); i++) { const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; //PCK: unsigned instead of bool - unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax); if (overlap != 0) { - walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, - subtree.m_rootNodeIndex, - subtree.m_rootNodeIndex+subtree.m_subtreeSize); + walkStacklessQuantizedTree(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax, + subtree.m_rootNodeIndex, + subtree.m_rootNodeIndex + subtree.m_subtreeSize); } } } - -void btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const +void btQuantizedBvh::reportRayOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const { - reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0)); + reportBoxCastOverlappingNodex(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0)); } - -void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const +void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) const { //always use stackless @@ -803,31 +773,31 @@ void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCa reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); } */ - } - -void btQuantizedBvh::swapLeafNodes(int i,int splitIndex) +void btQuantizedBvh::swapLeafNodes(int i, int splitIndex) { if (m_useQuantization) { - btQuantizedBvhNode tmp = m_quantizedLeafNodes[i]; - m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex]; - m_quantizedLeafNodes[splitIndex] = tmp; - } else + btQuantizedBvhNode tmp = m_quantizedLeafNodes[i]; + m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex]; + m_quantizedLeafNodes[splitIndex] = tmp; + } + else { - btOptimizedBvhNode tmp = m_leafNodes[i]; - m_leafNodes[i] = m_leafNodes[splitIndex]; - m_leafNodes[splitIndex] = tmp; + btOptimizedBvhNode tmp = m_leafNodes[i]; + m_leafNodes[i] = m_leafNodes[splitIndex]; + m_leafNodes[splitIndex] = tmp; } } -void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex) +void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex) { if (m_useQuantization) { m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex]; - } else + } + else { m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex]; } @@ -844,11 +814,10 @@ static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1; static const unsigned BVH_ALIGNMENT_BLOCKS = 2; #endif - unsigned int btQuantizedBvh::getAlignmentSerializationPadding() { // I changed this to 0 since the extra padding is not needed or used. - return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; + return 0; //BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; } unsigned btQuantizedBvh::calculateSerializeBufferSize() const @@ -862,12 +831,12 @@ unsigned btQuantizedBvh::calculateSerializeBufferSize() const return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode); } -bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) const +bool btQuantizedBvh::serialize(void* o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) const { btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); m_subtreeHeaderCount = m_SubtreeHeaders.size(); -/* if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + /* if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) { ///check alignedment for buffer? btAssert(0); @@ -875,7 +844,7 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe } */ - btQuantizedBvh *targetBvh = (btQuantizedBvh *)o_alignedDataBuffer; + btQuantizedBvh* targetBvh = (btQuantizedBvh*)o_alignedDataBuffer; // construct the class so the virtual function table, etc will be set up // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor @@ -885,10 +854,9 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe { targetBvh->m_curNodeIndex = static_cast(btSwapEndian(m_curNodeIndex)); - - btSwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin); - btSwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax); - btSwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization); + btSwapVector3Endian(m_bvhAabbMin, targetBvh->m_bvhAabbMin); + btSwapVector3Endian(m_bvhAabbMax, targetBvh->m_bvhAabbMax); + btSwapVector3Endian(m_bvhQuantization, targetBvh->m_bvhQuantization); targetBvh->m_traversalMode = (btTraversalMode)btSwapEndian(m_traversalMode); targetBvh->m_subtreeHeaderCount = static_cast(btSwapEndian(m_subtreeHeaderCount)); @@ -905,12 +873,12 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe targetBvh->m_useQuantization = m_useQuantization; - unsigned char *nodeData = (unsigned char *)targetBvh; + unsigned char* nodeData = (unsigned char*)targetBvh; nodeData += sizeof(btQuantizedBvh); - - unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + + unsigned sizeToAdd = 0; //(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; nodeData += sizeToAdd; - + int nodeCount = m_curNodeIndex; if (m_useQuantization) @@ -936,7 +904,6 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe { 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]; @@ -946,8 +913,6 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]; targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex; - - } } nodeData += sizeof(btQuantizedBvhNode) * nodeCount; @@ -993,7 +958,7 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe targetBvh->m_contiguousNodes.initializeFromBuffer(NULL, 0, 0); } - sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + sizeToAdd = 0; //(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; nodeData += sizeToAdd; // Now serialize the subtree headers @@ -1048,14 +1013,13 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe return true; } -btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) +btQuantizedBvh* btQuantizedBvh::deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) { - - if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + if (i_alignedDataBuffer == NULL) // || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) { return NULL; } - btQuantizedBvh *bvh = (btQuantizedBvh *)i_alignedDataBuffer; + btQuantizedBvh* bvh = (btQuantizedBvh*)i_alignedDataBuffer; if (i_swapEndian) { @@ -1077,12 +1041,12 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un return NULL; } - unsigned char *nodeData = (unsigned char *)bvh; + unsigned char* nodeData = (unsigned char*)bvh; nodeData += sizeof(btQuantizedBvh); - - unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + + 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 @@ -1120,7 +1084,7 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un { btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); - + bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_escapeIndex)); bvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_subPart)); bvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_triangleIndex)); @@ -1129,7 +1093,7 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un nodeData += sizeof(btOptimizedBvhNode) * nodeCount; } - sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + sizeToAdd = 0; //(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; nodeData += sizeToAdd; // Now serialize the subtree headers @@ -1155,13 +1119,11 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un } // Constructor that prevents btVector3's default constructor from being called -btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) : -m_bvhAabbMin(self.m_bvhAabbMin), -m_bvhAabbMax(self.m_bvhAabbMax), -m_bvhQuantization(self.m_bvhQuantization), -m_bulletVersion(BT_BULLET_VERSION) +btQuantizedBvh::btQuantizedBvh(btQuantizedBvh& self, bool /* ownsMemory */) : m_bvhAabbMin(self.m_bvhAabbMin), + m_bvhAabbMax(self.m_bvhAabbMax), + m_bvhQuantization(self.m_bvhQuantization), + m_bulletVersion(BT_BULLET_VERSION) { - } void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData) @@ -1171,8 +1133,8 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB m_bvhQuantization.deSerializeFloat(quantizedBvhFloatData.m_bvhQuantization); m_curNodeIndex = quantizedBvhFloatData.m_curNodeIndex; - m_useQuantization = quantizedBvhFloatData.m_useQuantization!=0; - + m_useQuantization = quantizedBvhFloatData.m_useQuantization != 0; + { int numElem = quantizedBvhFloatData.m_numContiguousLeafNodes; m_contiguousNodes.resize(numElem); @@ -1181,7 +1143,7 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB { btOptimizedBvhNodeFloatData* memPtr = quantizedBvhFloatData.m_contiguousNodesPtr; - for (int i=0;im_aabbMaxOrg); m_contiguousNodes[i].m_aabbMinOrg.deSerializeFloat(memPtr->m_aabbMinOrg); @@ -1195,11 +1157,11 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB { int numElem = quantizedBvhFloatData.m_numQuantizedContiguousNodes; m_quantizedContiguousNodes.resize(numElem); - + if (numElem) { btQuantizedBvhNodeData* memPtr = quantizedBvhFloatData.m_quantizedContiguousNodesPtr; - for (int i=0;im_escapeIndexOrTriangleIndex; m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; @@ -1213,16 +1175,16 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB } m_traversalMode = btTraversalMode(quantizedBvhFloatData.m_traversalMode); - + { int numElem = quantizedBvhFloatData.m_numSubtreeHeaders; m_SubtreeHeaders.resize(numElem); if (numElem) { btBvhSubtreeInfoData* memPtr = quantizedBvhFloatData.m_subTreeInfoPtr; - for (int i=0;im_quantizedAabbMax[0] ; + m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_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]; @@ -1242,8 +1204,8 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize m_bvhQuantization.deSerializeDouble(quantizedBvhDoubleData.m_bvhQuantization); m_curNodeIndex = quantizedBvhDoubleData.m_curNodeIndex; - m_useQuantization = quantizedBvhDoubleData.m_useQuantization!=0; - + m_useQuantization = quantizedBvhDoubleData.m_useQuantization != 0; + { int numElem = quantizedBvhDoubleData.m_numContiguousLeafNodes; m_contiguousNodes.resize(numElem); @@ -1252,7 +1214,7 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize { btOptimizedBvhNodeDoubleData* memPtr = quantizedBvhDoubleData.m_contiguousNodesPtr; - for (int i=0;im_aabbMaxOrg); m_contiguousNodes[i].m_aabbMinOrg.deSerializeDouble(memPtr->m_aabbMinOrg); @@ -1266,11 +1228,11 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize { int numElem = quantizedBvhDoubleData.m_numQuantizedContiguousNodes; m_quantizedContiguousNodes.resize(numElem); - + if (numElem) { btQuantizedBvhNodeData* memPtr = quantizedBvhDoubleData.m_quantizedContiguousNodesPtr; - for (int i=0;im_escapeIndexOrTriangleIndex; m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; @@ -1284,16 +1246,16 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize } m_traversalMode = btTraversalMode(quantizedBvhDoubleData.m_traversalMode); - + { int numElem = quantizedBvhDoubleData.m_numSubtreeHeaders; m_SubtreeHeaders.resize(numElem); if (numElem) { btBvhSubtreeInfoData* memPtr = quantizedBvhDoubleData.m_subTreeInfoPtr; - for (int i=0;im_quantizedAabbMax[0] ; + m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_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]; @@ -1304,32 +1266,29 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize } } } - } - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer) const { btQuantizedBvhData* quantizedData = (btQuantizedBvhData*)dataBuffer; - + m_bvhAabbMax.serialize(quantizedData->m_bvhAabbMax); m_bvhAabbMin.serialize(quantizedData->m_bvhAabbMin); m_bvhQuantization.serialize(quantizedData->m_bvhQuantization); quantizedData->m_curNodeIndex = m_curNodeIndex; quantizedData->m_useQuantization = m_useQuantization; - + quantizedData->m_numContiguousLeafNodes = m_contiguousNodes.size(); - quantizedData->m_contiguousNodesPtr = (btOptimizedBvhNodeData*) (m_contiguousNodes.size() ? serializer->getUniquePointer((void*)&m_contiguousNodes[0]) : 0); + quantizedData->m_contiguousNodesPtr = (btOptimizedBvhNodeData*)(m_contiguousNodes.size() ? serializer->getUniquePointer((void*)&m_contiguousNodes[0]) : 0); if (quantizedData->m_contiguousNodesPtr) { int sz = sizeof(btOptimizedBvhNodeData); int numElem = m_contiguousNodes.size(); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btOptimizedBvhNodeData* memPtr = (btOptimizedBvhNodeData*)chunk->m_oldPtr; - for (int i=0;im_aabbMaxOrg); m_contiguousNodes[i].m_aabbMinOrg.serialize(memPtr->m_aabbMinOrg); @@ -1339,19 +1298,19 @@ const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer // 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]); + serializer->finalizeChunk(chunk, "btOptimizedBvhNodeData", BT_ARRAY_CODE, (void*)&m_contiguousNodes[0]); } quantizedData->m_numQuantizedContiguousNodes = m_quantizedContiguousNodes.size(); -// printf("quantizedData->m_numQuantizedContiguousNodes=%d\n",quantizedData->m_numQuantizedContiguousNodes); - quantizedData->m_quantizedContiguousNodesPtr =(btQuantizedBvhNodeData*) (m_quantizedContiguousNodes.size() ? serializer->getUniquePointer((void*)&m_quantizedContiguousNodes[0]) : 0); + // printf("quantizedData->m_numQuantizedContiguousNodes=%d\n",quantizedData->m_numQuantizedContiguousNodes); + quantizedData->m_quantizedContiguousNodesPtr = (btQuantizedBvhNodeData*)(m_quantizedContiguousNodes.size() ? serializer->getUniquePointer((void*)&m_quantizedContiguousNodes[0]) : 0); if (quantizedData->m_quantizedContiguousNodesPtr) { int sz = sizeof(btQuantizedBvhNodeData); int numElem = m_quantizedContiguousNodes.size(); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btQuantizedBvhNodeData* memPtr = (btQuantizedBvhNodeData*)chunk->m_oldPtr; - for (int i=0;im_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex; memPtr->m_quantizedAabbMax[0] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[0]; @@ -1361,20 +1320,20 @@ const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer memPtr->m_quantizedAabbMin[1] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[1]; memPtr->m_quantizedAabbMin[2] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[2]; } - serializer->finalizeChunk(chunk,"btQuantizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_quantizedContiguousNodes[0]); + serializer->finalizeChunk(chunk, "btQuantizedBvhNodeData", BT_ARRAY_CODE, (void*)&m_quantizedContiguousNodes[0]); } quantizedData->m_traversalMode = int(m_traversalMode); quantizedData->m_numSubtreeHeaders = m_SubtreeHeaders.size(); - quantizedData->m_subTreeInfoPtr = (btBvhSubtreeInfoData*) (m_SubtreeHeaders.size() ? serializer->getUniquePointer((void*)&m_SubtreeHeaders[0]) : 0); + quantizedData->m_subTreeInfoPtr = (btBvhSubtreeInfoData*)(m_SubtreeHeaders.size() ? serializer->getUniquePointer((void*)&m_SubtreeHeaders[0]) : 0); if (quantizedData->m_subTreeInfoPtr) { int sz = sizeof(btBvhSubtreeInfoData); int numElem = m_SubtreeHeaders.size(); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btBvhSubtreeInfoData* memPtr = (btBvhSubtreeInfoData*)chunk->m_oldPtr; - for (int i=0;im_quantizedAabbMax[0] = m_SubtreeHeaders[i].m_quantizedAabbMax[0]; memPtr->m_quantizedAabbMax[1] = m_SubtreeHeaders[i].m_quantizedAabbMax[1]; @@ -1386,12 +1345,7 @@ const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer memPtr->m_rootNodeIndex = m_SubtreeHeaders[i].m_rootNodeIndex; memPtr->m_subtreeSize = m_SubtreeHeaders[i].m_subtreeSize; } - serializer->finalizeChunk(chunk,"btBvhSubtreeInfoData",BT_ARRAY_CODE,(void*)&m_SubtreeHeaders[0]); + serializer->finalizeChunk(chunk, "btBvhSubtreeInfoData", BT_ARRAY_CODE, (void*)&m_SubtreeHeaders[0]); } return btQuantizedBvhDataName; } - - - - - diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h index 3dd5ac9bb..1c47b9ccf 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -22,11 +22,11 @@ class btSerializer; #ifdef DEBUG_CHECK_DEQUANTIZATION #ifdef __SPU__ #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #include #include -#endif //DEBUG_CHECK_DEQUANTIZATION +#endif //DEBUG_CHECK_DEQUANTIZATION #include "LinearMath/btVector3.h" #include "LinearMath/btAlignedAllocator.h" @@ -41,13 +41,10 @@ class btSerializer; #define btQuantizedBvhDataName "btQuantizedBvhFloatData" #endif - - //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 +#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 @@ -55,15 +52,16 @@ class btSerializer; ///btQuantizedBvhNode is a compressed aabb node, 16 bytes. ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). -ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode +ATTRIBUTE_ALIGNED16(struct) +btQuantizedBvhNode { BT_DECLARE_ALIGNED_ALLOCATOR(); //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; //4 bytes - int m_escapeIndexOrTriangleIndex; + int m_escapeIndexOrTriangleIndex; bool isLeafNode() const { @@ -75,68 +73,67 @@ ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode btAssert(!isLeafNode()); return -m_escapeIndexOrTriangleIndex; } - int getTriangleIndex() const + int getTriangleIndex() const { btAssert(isLeafNode()); - unsigned int x=0; - unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); + 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)); + return (m_escapeIndexOrTriangleIndex & ~(y)); } - int getPartId() const + int getPartId() const { btAssert(isLeafNode()); // Get only the highest bits where the part index is stored - return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); + return (m_escapeIndexOrTriangleIndex >> (31 - MAX_NUM_PARTS_IN_BITS)); } -} -; +}; /// btOptimizedBvhNode contains both internal and leaf node information. /// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. -ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode +ATTRIBUTE_ALIGNED16(struct) +btOptimizedBvhNode { BT_DECLARE_ALIGNED_ALLOCATOR(); //32 bytes - btVector3 m_aabbMinOrg; - btVector3 m_aabbMaxOrg; + btVector3 m_aabbMinOrg; + btVector3 m_aabbMaxOrg; //4 - int m_escapeIndex; + int m_escapeIndex; //8 //for child nodes - int m_subPart; - int m_triangleIndex; + int m_subPart; + int m_triangleIndex; -//pad the size to 64 bytes - char m_padding[20]; + //pad the size to 64 bytes + char m_padding[20]; }; - ///btBvhSubtreeInfo provides info to gather a subtree of limited size -ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo +ATTRIBUTE_ALIGNED16(class) +btBvhSubtreeInfo { public: BT_DECLARE_ALIGNED_ALLOCATOR(); //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; //4 bytes, points to the root of the subtree - int m_rootNodeIndex; + int m_rootNodeIndex; //4 bytes - int m_subtreeSize; - int m_padding[3]; + int m_subtreeSize; + int m_padding[3]; btBvhSubtreeInfo() { //memset(&m_padding[0], 0, sizeof(m_padding)); } - - void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) + void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) { m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0]; m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1]; @@ -145,14 +142,12 @@ public: m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1]; m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2]; } -} -; - +}; class btNodeOverlapCallback { public: - virtual ~btNodeOverlapCallback() {}; + virtual ~btNodeOverlapCallback(){}; virtual void processNode(int subPart, int triangleIndex) = 0; }; @@ -160,18 +155,16 @@ public: #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedObjectArray.h" - - ///for code readability: -typedef btAlignedObjectArray NodeArray; -typedef btAlignedObjectArray QuantizedNodeArray; -typedef btAlignedObjectArray BvhSubtreeInfoArray; - +typedef btAlignedObjectArray NodeArray; +typedef btAlignedObjectArray QuantizedNodeArray; +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. ///It is recommended to use quantization for better performance and lower memory requirements. -ATTRIBUTE_ALIGNED16(class) btQuantizedBvh +ATTRIBUTE_ALIGNED16(class) +btQuantizedBvh { public: enum btTraversalMode @@ -182,54 +175,47 @@ public: }; protected: + btVector3 m_bvhAabbMin; + btVector3 m_bvhAabbMax; + btVector3 m_bvhQuantization; + int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess. - btVector3 m_bvhAabbMin; - btVector3 m_bvhAabbMax; - btVector3 m_bvhQuantization; - - int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess. - - int m_curNodeIndex; + int m_curNodeIndex; //quantization data - bool m_useQuantization; + bool m_useQuantization; + NodeArray m_leafNodes; + NodeArray m_contiguousNodes; + QuantizedNodeArray m_quantizedLeafNodes; + QuantizedNodeArray m_quantizedContiguousNodes; - - NodeArray m_leafNodes; - NodeArray m_contiguousNodes; - QuantizedNodeArray m_quantizedLeafNodes; - QuantizedNodeArray m_quantizedContiguousNodes; - - btTraversalMode m_traversalMode; - BvhSubtreeInfoArray m_SubtreeHeaders; + btTraversalMode m_traversalMode; + BvhSubtreeInfoArray m_SubtreeHeaders; //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray 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 btVector3& aabbMin) + void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin) { if (m_useQuantization) { - quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0); - } else + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0], aabbMin, 0); + } + else { m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; - } } - void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) + void setInternalNodeAabbMax(int nodeIndex, const btVector3& aabbMax) { if (m_useQuantization) { - quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1); - } else + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0], aabbMax, 1); + } + else { m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; } @@ -243,115 +229,102 @@ protected: } //non-quantized return m_leafNodes[nodeIndex].m_aabbMinOrg; - } btVector3 getAabbMax(int nodeIndex) const { if (m_useQuantization) { return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]); - } + } //non-quantized return m_leafNodes[nodeIndex].m_aabbMaxOrg; - } - - void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) + void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) { if (m_useQuantization) { m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex; - } + } else { m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex; } - } - void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) + void mergeInternalNodeAabb(int nodeIndex, const btVector3& newAabbMin, const btVector3& 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++) + 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 + } + else { //non-quantized m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin); - m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); + m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); } } - void swapLeafNodes(int firstIndex,int secondIndex); + void swapLeafNodes(int firstIndex, int secondIndex); - void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex); + void assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex); protected: + void buildTree(int startIndex, int endIndex); - + int calcSplittingAxis(int startIndex, int endIndex); - void buildTree (int startIndex,int endIndex); + int sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis); - int calcSplittingAxis(int startIndex,int endIndex); + void walkStacklessTree(btNodeOverlapCallback * nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const; - int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); - - void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; - void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; - void walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; + void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const; + void walkStacklessQuantizedTree(btNodeOverlapCallback * nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const; + void walkStacklessTreeAgainstRay(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const; ///tree traversal designed for small-memory processors like PS3 SPU - void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback * nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const; ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal - void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode, btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const; ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal - void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const; - + void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA, const btQuantizedBvhNode* treeNodeB, btNodeOverlapCallback* nodeCallback) const; - - - void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); + void updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); btQuantizedBvh(); virtual ~btQuantizedBvh(); - ///***************************************** expert/internal use only ************************* - void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0)); - QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } + void setQuantizationValues(const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, btScalar quantizationMargin = btScalar(1.0)); + QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized - void buildInternal(); + void buildInternal(); ///***************************************** expert/internal use only ************************* - void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; - void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const; - void reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const; + void reportAabbOverlappingNodex(btNodeOverlapCallback * nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const; + void reportRayOverlappingNodex(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const; + void reportBoxCastOverlappingNodex(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) const; - SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const + SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point, int isMax) const { - btAssert(m_useQuantization); btAssert(point.getX() <= m_bvhAabbMax.getX()); @@ -368,16 +341,16 @@ public: ///@todo: double-check this if (isMax) { - out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1)); - out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1)); - out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(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)); + out[0] = (unsigned short)(((unsigned short)(v.getX() + btScalar(1.)) | 1)); + out[1] = (unsigned short)(((unsigned short)(v.getY() + btScalar(1.)) | 1)); + out[2] = (unsigned short)(((unsigned short)(v.getZ() + btScalar(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 btVector3 newPoint = unQuantize(out); @@ -385,105 +358,97 @@ public: { if (newPoint.getX() < point.getX()) { - printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), 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()); + 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()); + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ()); } - } else + } + else { if (newPoint.getX() > point.getX()) { - printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), 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()); + 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()); + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ()); } } -#endif //DEBUG_CHECK_DEQUANTIZATION - +#endif //DEBUG_CHECK_DEQUANTIZATION } - - SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const + SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2, int isMax) const { - btAssert(m_useQuantization); btVector3 clampedPoint(point2); clampedPoint.setMax(m_bvhAabbMin); clampedPoint.setMin(m_bvhAabbMax); - quantize(out,clampedPoint,isMax); - + quantize(out, clampedPoint, isMax); } - - SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const + + SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const { - btVector3 vecOut; - vecOut.setValue( + btVector3 vecOut; + vecOut.setValue( (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()), (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()), (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ())); - vecOut += m_bvhAabbMin; - return vecOut; + 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(btTraversalMode traversalMode) + void setTraversalMode(btTraversalMode traversalMode) { m_traversalMode = traversalMode; } - - SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() - { - return m_quantizedContiguousNodes; + SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() + { + return m_quantizedContiguousNodes; } - - SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray() + SIMD_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; + 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 btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); + static btQuantizedBvh* deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); static unsigned int getAlignmentSerializationPadding(); -////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////// - - virtual int calculateSerializeBufferSizeNew() const; + virtual int calculateSerializeBufferSizeNew() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - virtual void deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData); + virtual void deSerializeFloat(struct btQuantizedBvhFloatData & quantizedBvhFloatData); - virtual void deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData); + virtual void deSerializeDouble(struct btQuantizedBvhDoubleData & quantizedBvhDoubleData); - -//////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////// SIMD_FORCE_INLINE bool isQuantized() { @@ -494,38 +459,37 @@ private: // Special "copy" constructor that allows for in-place deserialization // Prevents btVector3'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) - btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory); + btQuantizedBvh(btQuantizedBvh & other, bool ownsMemory); +}; -} -; - - -struct btBvhSubtreeInfoData +// clang-format off +// parser needs * with the name +struct btBvhSubtreeInfoData { - int m_rootNodeIndex; - int m_subtreeSize; + int m_rootNodeIndex; + int m_subtreeSize; unsigned short m_quantizedAabbMin[3]; unsigned short m_quantizedAabbMax[3]; }; struct btOptimizedBvhNodeFloatData { - btVector3FloatData m_aabbMinOrg; - btVector3FloatData m_aabbMaxOrg; - int m_escapeIndex; - int m_subPart; - int m_triangleIndex; + btVector3FloatData m_aabbMinOrg; + btVector3FloatData m_aabbMaxOrg; + int m_escapeIndex; + int m_subPart; + int m_triangleIndex; char m_pad[4]; }; struct btOptimizedBvhNodeDoubleData { - btVector3DoubleData m_aabbMinOrg; - btVector3DoubleData m_aabbMaxOrg; - int m_escapeIndex; - int m_subPart; - int m_triangleIndex; - char m_pad[4]; + btVector3DoubleData m_aabbMinOrg; + btVector3DoubleData m_aabbMaxOrg; + int m_escapeIndex; + int m_subPart; + int m_triangleIndex; + char m_pad[4]; }; @@ -569,13 +533,11 @@ struct btQuantizedBvhDoubleData int m_numSubtreeHeaders; btBvhSubtreeInfoData *m_subTreeInfoPtr; }; +// clang-format on - -SIMD_FORCE_INLINE int btQuantizedBvh::calculateSerializeBufferSizeNew() const +SIMD_FORCE_INLINE int btQuantizedBvh::calculateSerializeBufferSizeNew() const { return sizeof(btQuantizedBvhData); } - - -#endif //BT_QUANTIZED_BVH_H +#endif //BT_QUANTIZED_BVH_H diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index f1d5f5476..b7fe0a1f3 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -24,52 +24,45 @@ subject to the following restrictions: #include -extern int gOverlappingPairs; - -void btSimpleBroadphase::validate() +void btSimpleBroadphase::validate() { - for (int i=0;i= m_maxHandles) { btAssert(0); - return 0; //should never happen, but don't let the game crash ;-) + return 0; //should never happen, but don't let the game crash ;-) } - btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]); + btAssert(aabbMin[0] <= aabbMax[0] && aabbMin[1] <= aabbMax[1] && aabbMin[2] <= aabbMax[2]); int newHandleIndex = allocHandle(); - btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask); + btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex]) btSimpleBroadphaseProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask); return proxy; } -class RemovingOverlapCallback : public btOverlapCallback +class RemovingOverlapCallback : public btOverlapCallback { protected: - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { (void)pair; btAssert(0); @@ -112,12 +104,13 @@ protected: class RemovePairContainingProxy { + btBroadphaseProxy* m_targetProxy; - btBroadphaseProxy* m_targetProxy; - public: +public: virtual ~RemovePairContainingProxy() { } + protected: virtual bool processOverlap(btBroadphasePair& pair) { @@ -128,38 +121,36 @@ protected: }; }; -void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher) +void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg, btDispatcher* dispatcher) { - - btSimpleBroadphaseProxy* proxy0 = static_cast(proxyOrg); - freeHandle(proxy0); + m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher); - m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher); + btSimpleBroadphaseProxy* proxy0 = static_cast(proxyOrg); + freeHandle(proxy0); - //validate(); - + //validate(); } -void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const { const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); aabbMin = sbp->m_aabbMin; aabbMax = sbp->m_aabbMax; } -void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/) +void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/) { btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); sbp->m_aabbMin = aabbMin; sbp->m_aabbMax = aabbMax; } -void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) +void btSimpleBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax) { - for (int i=0; i <= m_LastHandleIndex; i++) + for (int i = 0; i <= m_LastHandleIndex; i++) { btSimpleBroadphaseProxy* proxy = &m_pHandles[i]; - if(!proxy->m_clientObject) + if (!proxy->m_clientObject) { continue; } @@ -167,69 +158,59 @@ void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo } } - -void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) +void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) { - for (int i=0; i <= m_LastHandleIndex; i++) + for (int i = 0; i <= m_LastHandleIndex; i++) { btSimpleBroadphaseProxy* proxy = &m_pHandles[i]; - if(!proxy->m_clientObject) + if (!proxy->m_clientObject) { continue; } - if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax)) + if (TestAabbAgainstAabb2(aabbMin, aabbMax, proxy->m_aabbMin, proxy->m_aabbMax)) { callback.process(proxy); } } } - - - - - - -bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1) +bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0, btSimpleBroadphaseProxy* proxy1) { - return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && + return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] && proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2]; - } - - //then remove non-overlapping ones class CheckOverlapCallback : public btOverlapCallback { public: virtual bool processOverlap(btBroadphasePair& pair) { - return (!btSimpleBroadphase::aabbOverlap(static_cast(pair.m_pProxy0),static_cast(pair.m_pProxy1))); + return (!btSimpleBroadphase::aabbOverlap(static_cast(pair.m_pProxy0), static_cast(pair.m_pProxy1))); } }; -void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { //first check for new overlapping pairs - int i,j; + int i, j; if (m_numHandles >= 0) { int new_largest_index = -1; - for (i=0; i <= m_LastHandleIndex; i++) + for (i = 0; i <= m_LastHandleIndex; i++) { btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i]; - if(!proxy0->m_clientObject) + if (!proxy0->m_clientObject) { continue; } new_largest_index = i; - for (j=i+1; j <= m_LastHandleIndex; j++) + for (j = i + 1; j <= m_LastHandleIndex; j++) { btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j]; btAssert(proxy0 != proxy1); - if(!proxy1->m_clientObject) + if (!proxy1->m_clientObject) { continue; } @@ -237,19 +218,20 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); - if (aabbOverlap(p0,p1)) + if (aabbOverlap(p0, p1)) { - if ( !m_pairCache->findPair(proxy0,proxy1)) + if (!m_pairCache->findPair(proxy0, proxy1)) { - m_pairCache->addOverlappingPair(proxy0,proxy1); + m_pairCache->addOverlappingPair(proxy0, proxy1); } - } else + } + else { if (!m_pairCache->hasDeferredRemoval()) { - if ( m_pairCache->findPair(proxy0,proxy1)) + if (m_pairCache->findPair(proxy0, proxy1)) { - m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher); + m_pairCache->removeOverlappingPair(proxy0, proxy1, dispatcher); } } } @@ -260,8 +242,7 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) if (m_ownsPairCache && m_pairCache->hasDeferredRemoval()) { - - btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); + btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); //perform a sort, to find duplicates and to sort 'invalid' pairs to the end overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); @@ -269,16 +250,13 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); m_invalidPair = 0; - btBroadphasePair previousPair; previousPair.m_pProxy0 = 0; previousPair.m_pProxy1 = 0; previousPair.m_algorithm = 0; - - for (i=0;iprocessOverlap(pair); - } else + needsRemoval = false; //callback->processOverlap(pair); + } + else { needsRemoval = true; } - } else + } + else { //remove duplicate needsRemoval = true; @@ -308,16 +288,14 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) if (needsRemoval) { - m_pairCache->cleanOverlappingPair(pair,dispatcher); + m_pairCache->cleanOverlappingPair(pair, dispatcher); // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); // m_overlappingPairArray.pop_back(); pair.m_pProxy0 = 0; pair.m_pProxy1 = 0; m_invalidPair++; - gOverlappingPairs--; - } - + } } ///if you don't like to skip the invalid pairs in the array, execute following code: @@ -329,21 +307,19 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); m_invalidPair = 0; -#endif//CLEAN_INVALID_PAIRS - +#endif //CLEAN_INVALID_PAIRS } } } - -bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); - return aabbOverlap(p0,p1); + return aabbOverlap(p0, p1); } -void btSimpleBroadphase::resetPool(btDispatcher* dispatcher) +void btSimpleBroadphase::resetPool(btDispatcher* dispatcher) { //not yet } diff --git a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index d7a18e400..3e02fdc00 100644 --- a/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -16,57 +16,47 @@ subject to the following restrictions: #ifndef BT_SIMPLE_BROADPHASE_H #define BT_SIMPLE_BROADPHASE_H - #include "btOverlappingPairCache.h" - struct btSimpleBroadphaseProxy : public btBroadphaseProxy { - int m_nextFree; - -// int m_handleId; + int m_nextFree; - - btSimpleBroadphaseProxy() {}; + // int m_handleId; - btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask) - :btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask) + btSimpleBroadphaseProxy(){}; + + btSimpleBroadphaseProxy(const btVector3& minpt, const btVector3& maxpt, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask) + : btBroadphaseProxy(minpt, maxpt, userPtr, collisionFilterGroup, collisionFilterMask) { (void)shapeType; } - - - SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;} - SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;} - - - + SIMD_FORCE_INLINE void SetNextFree(int next) { m_nextFree = next; } + SIMD_FORCE_INLINE int GetNextFree() const { return m_nextFree; } }; ///The SimpleBroadphase is just a unit-test for btAxisSweep3, bt32BitAxisSweep3, or btDbvtBroadphase, so use those classes instead. ///It is a brute force aabb culling broadphase based on O(n^2) aabb checks class btSimpleBroadphase : public btBroadphaseInterface { - protected: + int m_numHandles; // number of active handles + int m_maxHandles; // max number of handles + int m_LastHandleIndex; - int m_numHandles; // number of active handles - int m_maxHandles; // max number of handles - int m_LastHandleIndex; - - btSimpleBroadphaseProxy* m_pHandles; // handles pool + btSimpleBroadphaseProxy* m_pHandles; // handles pool void* m_pHandlesRawPtr; - int m_firstFreeHandle; // free handles list - + int m_firstFreeHandle; // free handles list + int allocHandle() { btAssert(m_numHandles < m_maxHandles); int freeHandle = m_firstFreeHandle; m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree(); m_numHandles++; - if(freeHandle > m_LastHandleIndex) + if (freeHandle > m_LastHandleIndex) { m_LastHandleIndex = freeHandle; } @@ -75,9 +65,9 @@ protected: void freeHandle(btSimpleBroadphaseProxy* proxy) { - int handle = int(proxy-m_pHandles); + int handle = int(proxy - m_pHandles); btAssert(handle >= 0 && handle < m_maxHandles); - if(handle == m_LastHandleIndex) + if (handle == m_LastHandleIndex) { m_LastHandleIndex--; } @@ -89,20 +79,18 @@ protected: m_numHandles--; } - btOverlappingPairCache* m_pairCache; - bool m_ownsPairCache; + btOverlappingPairCache* m_pairCache; + bool m_ownsPairCache; - int m_invalidPair; + int m_invalidPair; - - - inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) + inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) { btSimpleBroadphaseProxy* proxy0 = static_cast(proxy); return proxy0; } - inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const + inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const { const btSimpleBroadphaseProxy* proxy0 = static_cast(proxy); return proxy0; @@ -111,61 +99,50 @@ protected: ///reset broadphase internal structures, to ensure determinism/reproducability virtual void resetPool(btDispatcher* dispatcher); - - void validate(); + void validate(); protected: - - - - public: - btSimpleBroadphase(int maxProxies=16384,btOverlappingPairCache* overlappingPairCache=0); + btSimpleBroadphase(int maxProxies = 16384, btOverlappingPairCache* overlappingPairCache = 0); virtual ~btSimpleBroadphase(); + static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0, btSimpleBroadphaseProxy* proxy1); - static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1); + virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher); + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - 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; - virtual void calculateOverlappingPairs(btDispatcher* dispatcher); + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); - 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)); - virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); - - btOverlappingPairCache* getOverlappingPairCache() + btOverlappingPairCache* getOverlappingPairCache() { return m_pairCache; } - const btOverlappingPairCache* getOverlappingPairCache() const + const btOverlappingPairCache* getOverlappingPairCache() const { return m_pairCache; } - bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - + bool testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); ///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 + 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); + aabbMin.setValue(-BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT); + aabbMax.setValue(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); } - virtual void printStats() + virtual void printStats() { -// printf("btSimpleBroadphase.h\n"); -// printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); + // printf("btSimpleBroadphase.h\n"); + // printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); } }; - - -#endif //BT_SIMPLE_BROADPHASE_H - +#endif //BT_SIMPLE_BROADPHASE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CMakeLists.txt b/Engine/lib/bullet/src/BulletCollision/CMakeLists.txt index 85c5fc8b6..f5d725562 100644 --- a/Engine/lib/bullet/src/BulletCollision/CMakeLists.txt +++ b/Engine/lib/bullet/src/BulletCollision/CMakeLists.txt @@ -56,12 +56,14 @@ SET(BulletCollision_SRCS 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 diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index c81af9567..7647f6736 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -18,94 +18,95 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" - -SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) -:m_sphere(sphere), -m_triangle(triangle), -m_contactBreakingThreshold(contactBreakingThreshold) +SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold) + : m_sphere(sphere), + m_triangle(triangle), + m_contactBreakingThreshold(contactBreakingThreshold) { - } -void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults) { - (void)debugDraw; const btTransform& transformA = input.m_transformA; const btTransform& transformB = input.m_transformB; - btVector3 point,normal; + btVector3 point, normal; btScalar timeOfImpact = btScalar(1.); btScalar depth = btScalar(0.); -// output.m_distance = btScalar(BT_LARGE_FLOAT); + // output.m_distance = btScalar(BT_LARGE_FLOAT); //move sphere into triangle space - btTransform sphereInTr = transformB.inverseTimes(transformA); + btTransform sphereInTr = transformB.inverseTimes(transformA); - if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) + if (collide(sphereInTr.getOrigin(), point, normal, depth, timeOfImpact, m_contactBreakingThreshold)) { if (swapResults) { - btVector3 normalOnB = transformB.getBasis()*normal; + btVector3 normalOnB = transformB.getBasis() * normal; btVector3 normalOnA = -normalOnB; - btVector3 pointOnA = transformB*point+normalOnB*depth; - output.addContactPoint(normalOnA,pointOnA,depth); - } else + btVector3 pointOnA = transformB * point + normalOnB * depth; + output.addContactPoint(normalOnA, pointOnA, depth); + } + else { - output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); + output.addContactPoint(transformB.getBasis() * normal, transformB * point, depth); } } - } - - // See also geometrictools.com // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv -btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest); +btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest); -btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { +btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest) +{ btVector3 diff = p - from; btVector3 v = to - from; btScalar t = v.dot(diff); - - if (t > 0) { + + if (t > 0) + { btScalar dotVV = v.dot(v); - if (t < dotVV) { + if (t < dotVV) + { t /= dotVV; - diff -= t*v; - } else { + diff -= t * v; + } + else + { t = 1; diff -= v; } - } else + } + else t = 0; - nearest = from + t*v; - return diff.dot(diff); + nearest = from + t * v; + return diff.dot(diff); } -bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) { +bool SphereTriangleDetector::facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal) +{ btVector3 lp(p); btVector3 lnormal(normal); - + return pointInTriangle(vertices, lnormal, &lp); } -bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) +bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold) { - const btVector3* vertices = &m_triangle->getVertexPtr(0); - + btScalar radius = m_sphere->getRadius(); btScalar radiusWithThreshold = radius + contactBreakingThreshold; - btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); + btVector3 normal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]); btScalar l2 = normal.length2(); bool hasContact = false; btVector3 contactPoint; - if (l2 >= SIMD_EPSILON*SIMD_EPSILON) + if (l2 >= SIMD_EPSILON * SIMD_EPSILON) { normal /= btSqrt(l2); @@ -120,52 +121,59 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po } bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; - + // Check for contact / intersection - - if (isInsideContactPlane) { - if (facecontains(sphereCenter, vertices, normal)) { + + if (isInsideContactPlane) + { + if (facecontains(sphereCenter, vertices, normal)) + { // Inside the contact wedge - touches a point on the shell plane hasContact = true; - contactPoint = sphereCenter - normal*distanceFromPlane; + contactPoint = sphereCenter - normal * distanceFromPlane; } - else { + else + { // Could be inside one of the contact capsules - btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; + btScalar contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold; + btScalar minDistSqr = contactCapsuleRadiusSqr; btVector3 nearestOnEdge; - for (int i = 0; i < m_triangle->getNumEdges(); i++) { - + 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 + if (distanceSqr < minDistSqr) + { + // Yep, we're inside a capsule, and record the capsule with smallest distance + minDistSqr = distanceSqr; hasContact = true; contactPoint = nearestOnEdge; } - } } } } - if (hasContact) { + if (hasContact) + { btVector3 contactToCentre = sphereCenter - contactPoint; btScalar distanceSqr = contactToCentre.length2(); - if (distanceSqr < radiusWithThreshold*radiusWithThreshold) + if (distanceSqr < radiusWithThreshold * radiusWithThreshold) { - if (distanceSqr>SIMD_EPSILON) + if (distanceSqr > SIMD_EPSILON) { btScalar distance = btSqrt(distanceSqr); resultNormal = contactToCentre; resultNormal.normalize(); point = contactPoint; - depth = -(radius-distance); - } else + depth = -(radius - distance); + } + else { resultNormal = normal; point = contactPoint; @@ -174,36 +182,34 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po return true; } } - + return false; } - -bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ) +bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p) { const btVector3* p1 = &vertices[0]; const btVector3* p2 = &vertices[1]; const btVector3* p3 = &vertices[2]; - btVector3 edge1( *p2 - *p1 ); - btVector3 edge2( *p3 - *p2 ); - btVector3 edge3( *p1 - *p3 ); + btVector3 edge1(*p2 - *p1); + btVector3 edge2(*p3 - *p2); + btVector3 edge3(*p1 - *p3); - btVector3 p1_to_p( *p - *p1 ); - btVector3 p2_to_p( *p - *p2 ); - btVector3 p3_to_p( *p - *p3 ); + btVector3 p1_to_p(*p - *p1); + btVector3 p2_to_p(*p - *p2); + btVector3 p3_to_p(*p - *p3); + + btVector3 edge1_normal(edge1.cross(normal)); + btVector3 edge2_normal(edge2.cross(normal)); + btVector3 edge3_normal(edge3.cross(normal)); - btVector3 edge1_normal( edge1.cross(normal)); - btVector3 edge2_normal( edge2.cross(normal)); - btVector3 edge3_normal( edge3.cross(normal)); - btScalar r1, r2, r3; - r1 = edge1_normal.dot( p1_to_p ); - r2 = edge2_normal.dot( p2_to_p ); - r3 = edge3_normal.dot( p3_to_p ); - if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || - ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) + r1 = edge1_normal.dot(p1_to_p); + r2 = edge2_normal.dot(p2_to_p); + r3 = edge3_normal.dot(p3_to_p); + if ((r1 > 0 && r2 > 0 && r3 > 0) || + (r1 <= 0 && r2 <= 0 && r3 <= 0)) return true; return false; - } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 22953af43..d47e47530 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -18,34 +18,26 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" - - class btSphereShape; class btTriangleShape; - - /// sphere-triangle to match the btDiscreteCollisionDetectorInterface struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface { - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false); - SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold); + SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold); - virtual ~SphereTriangleDetector() {}; + virtual ~SphereTriangleDetector(){}; - bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold); + bool collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold); private: - - - bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ); - bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal); + bool pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p); + bool facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal); btSphereShape* m_sphere; btTriangleShape* m_triangle; - btScalar m_contactBreakingThreshold; - + btScalar m_contactBreakingThreshold; }; -#endif //BT_SPHERE_TRIANGLE_DETECTOR_H - +#endif //BT_SPHERE_TRIANGLE_DETECTOR_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp index 57f146493..ac5de45d2 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp @@ -17,31 +17,31 @@ subject to the following restrictions: #include "btCollisionDispatcher.h" #include "btCollisionObject.h" -btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) -:btCollisionAlgorithm(ci) +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) //, //m_colObj0(0), //m_colObj1(0) { } -btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ) -:btCollisionAlgorithm(ci) +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper*, const btCollisionObjectWrapper*) + : btCollisionAlgorithm(ci) //, //m_colObj0(0), //m_colObj1(0) { -// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) -// { -// m_colObj0 = colObj0; -// m_colObj1 = colObj1; -// -// m_colObj0->activate(); -// m_colObj1->activate(); -// } + // if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) + // { + // m_colObj0 = colObj0; + // m_colObj1 = colObj1; + // + // m_colObj0->activate(); + // m_colObj1->activate(); + // } } btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm() { -// m_colObj0->activate(); -// m_colObj1->activate(); + // m_colObj0->activate(); + // m_colObj1->activate(); } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h index 0e19f1ea3..862060571 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -21,17 +21,15 @@ subject to the following restrictions: ///This class is not enabled yet (work-in-progress) to more aggressively activate objects. class btActivatingCollisionAlgorithm : public btCollisionAlgorithm { -// btCollisionObject* m_colObj0; -// btCollisionObject* m_colObj1; + // btCollisionObject* m_colObj0; + // btCollisionObject* m_colObj1; protected: + btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); - btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); - - btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); public: virtual ~btActivatingCollisionAlgorithm(); - }; -#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H +#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp index 2c3627782..6873a95d9 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -26,61 +26,55 @@ subject to the following restrictions: #define USE_PERSISTENT_CONTACTS 1 -btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap) -: btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf) +btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap) + : btActivatingCollisionAlgorithm(ci, obj0Wrap, obj1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf) { - if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject())) + if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject()); m_ownManifold = true; } } btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm() { - if (m_ownManifold) { if (m_manifoldPtr) m_dispatcher->releaseManifold(m_manifoldPtr); } - } - -void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); +void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); //#include -void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btBox2dBox2dCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) return; - const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape(); const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape(); resultOut->setPersistentManifold(m_manifoldPtr); - b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform()); + b2CollidePolygons(resultOut, box0, body0Wrap->getWorldTransform(), box1, body1Wrap->getWorldTransform()); // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added if (m_ownManifold) { resultOut->refreshContactPoints(); } - } -btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/) { //not yet return 1.f; } - struct ClipVertex { btVector3 v; @@ -89,16 +83,16 @@ struct ClipVertex //b2ContactID id; }; -#define b2Dot(a,b) (a).dot(b) -#define b2Mul(a,b) (a)*(b) -#define b2MulT(a,b) (a).transpose()*(b) -#define b2Cross(a,b) (a).cross(b) -#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f) +#define b2Dot(a, b) (a).dot(b) +#define b2Mul(a, b) (a) * (b) +#define b2MulT(a, b) (a).transpose() * (b) +#define b2Cross(a, b) (a).cross(b) +#define btCrossS(a, s) btVector3(s* a.getY(), -s* a.getX(), 0.f) -int b2_maxManifoldPoints =2; +int b2_maxManifoldPoints = 2; static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], - const btVector3& normal, btScalar offset) + const btVector3& normal, btScalar offset) { // Start with no output points int numOut = 0; @@ -133,7 +127,7 @@ static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], // Find the separation between poly1 and poly2 for a give edge normal on poly1. static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1, - const btBox2dShape* poly2, const btTransform& xf2) + const btBox2dShape* poly2, const btTransform& xf2) { const btVector3* vertices1 = poly1->getVertices(); const btVector3* normals1 = poly1->getNormals(); @@ -151,8 +145,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1 int index = 0; btScalar minDot = BT_LARGE_FLOAT; - if( count2 > 0 ) - index = (int) normal1.minDot( vertices2, count2, minDot); + if (count2 > 0) + index = (int)normal1.minDot(vertices2, count2, minDot); btVector3 v1 = b2Mul(xf1, vertices1[edge1]); btVector3 v2 = b2Mul(xf2, vertices2[index]); @@ -162,8 +156,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1 // Find the max separation between poly1 and poly2 using edge normals from poly1. static btScalar FindMaxSeparation(int* edgeIndex, - const btBox2dShape* poly1, const btTransform& xf1, - const btBox2dShape* poly2, const btTransform& xf2) + const btBox2dShape* poly1, const btTransform& xf1, + const btBox2dShape* poly2, const btTransform& xf2) { int count1 = poly1->getVertexCount(); const btVector3* normals1 = poly1->getNormals(); @@ -174,9 +168,9 @@ static btScalar FindMaxSeparation(int* edgeIndex, // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; - btScalar maxDot; - if( count1 > 0 ) - edge = (int) dLocal1.maxDot( normals1, count1, maxDot); + btScalar maxDot; + if (count1 > 0) + edge = (int)dLocal1.maxDot(normals1, count1, maxDot); // Get the separation for the edge normal. btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); @@ -224,7 +218,7 @@ static btScalar FindMaxSeparation(int* edgeIndex, } // Perform a local search for the best edge normal. - for ( ; ; ) + for (;;) { if (increment == -1) edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; @@ -285,14 +279,14 @@ static void FindIncidentEdge(ClipVertex c[2], int i2 = i1 + 1 < count2 ? i1 + 1 : 0; c[0].v = b2Mul(xf2, vertices2[i1]); -// c[0].id.features.referenceEdge = (unsigned char)edge1; -// c[0].id.features.incidentEdge = (unsigned char)i1; -// c[0].id.features.incidentVertex = 0; + // c[0].id.features.referenceEdge = (unsigned char)edge1; + // c[0].id.features.incidentEdge = (unsigned char)i1; + // c[0].id.features.incidentVertex = 0; c[1].v = b2Mul(xf2, vertices2[i2]); -// c[1].id.features.referenceEdge = (unsigned char)edge1; -// c[1].id.features.incidentEdge = (unsigned char)i2; -// c[1].id.features.incidentVertex = 1; + // c[1].id.features.referenceEdge = (unsigned char)edge1; + // c[1].id.features.incidentEdge = (unsigned char)i2; + // c[1].id.features.incidentVertex = 1; } // Find edge normal of max separation on A - return if separating axis is found @@ -303,10 +297,9 @@ static void FindIncidentEdge(ClipVertex c[2], // The normal points from 1 to 2 void b2CollidePolygons(btManifoldResult* manifold, - const btBox2dShape* polyA, const btTransform& xfA, - const btBox2dShape* polyB, const btTransform& xfB) + const btBox2dShape* polyA, const btTransform& xfA, + const btBox2dShape* polyB, const btTransform& xfB) { - int edgeA = 0; btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); if (separationA > 0.0f) @@ -317,10 +310,10 @@ void b2CollidePolygons(btManifoldResult* manifold, if (separationB > 0.0f) return; - const btBox2dShape* poly1; // reference poly - const btBox2dShape* poly2; // incident poly + const btBox2dShape* poly1; // reference poly + const btBox2dShape* poly2; // incident poly btTransform xf1, xf2; - int edge1; // reference edge + int edge1; // reference edge unsigned char flip; const btScalar k_relativeTol = 0.98f; const btScalar k_absoluteTol = 0.001f; @@ -352,14 +345,13 @@ void b2CollidePolygons(btManifoldResult* manifold, const btVector3* vertices1 = poly1->getVertices(); btVector3 v11 = vertices1[edge1]; - btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; + btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]; //btVector3 dv = v12 - v11; btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11); sideNormal.normalize(); btVector3 frontNormal = btCrossS(sideNormal, 1.0f); - - + v11 = b2Mul(xf1, v11); v12 = b2Mul(xf1, v12); @@ -369,13 +361,12 @@ void b2CollidePolygons(btManifoldResult* manifold, // Clip incident edge against extruded edge1 side edges. ClipVertex clipPoints1[2]; - clipPoints1[0].v.setValue(0,0,0); - clipPoints1[1].v.setValue(0,0,0); + clipPoints1[0].v.setValue(0, 0, 0); + clipPoints1[1].v.setValue(0, 0, 0); ClipVertex clipPoints2[2]; - clipPoints2[0].v.setValue(0,0,0); - clipPoints2[1].v.setValue(0,0,0); - + clipPoints2[0].v.setValue(0, 0, 0); + clipPoints2[1].v.setValue(0, 0, 0); int np; @@ -386,7 +377,7 @@ void b2CollidePolygons(btManifoldResult* manifold, return; // Clip to negative box side 1 - np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); + np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); if (np < 2) { @@ -403,19 +394,18 @@ void b2CollidePolygons(btManifoldResult* manifold, if (separation <= 0.0f) { - //b2ManifoldPoint* cp = manifold->points + pointCount; //btScalar separation = separation; //cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v); //cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v); - manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation); + manifold->addContactPoint(-manifoldNormal, clipPoints2[i].v, separation); -// cp->id = clipPoints2[i].id; -// cp->id.features.flip = flip; + // cp->id = clipPoints2[i].id; + // cp->id.features.flip = flip; ++pointCount; } } -// manifold->pointCount = pointCount;} + // manifold->pointCount = pointCount;} } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h index 6ea6e89bd..3b66d1fd0 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h @@ -26,22 +26,22 @@ class btPersistentManifold; ///box-box collision detection class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + public: btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btBox2dBox2dCollisionAlgorithm(); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -49,18 +49,15 @@ public: } } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm); void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0Wrap,body1Wrap); + return new (ptr) btBox2dBox2dCollisionAlgorithm(0, ci, body0Wrap, body1Wrap); } }; - }; -#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H - +#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp index ac68968f5..7a391e059 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -21,14 +21,14 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" #define USE_PERSISTENT_CONTACTS 1 -btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf) +btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf) { - if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject())) + if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } } @@ -42,30 +42,27 @@ btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm() } } -void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btBoxBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) return; - const btBoxShape* box0 = (btBoxShape*)body0Wrap->getCollisionShape(); const btBoxShape* box1 = (btBoxShape*)body1Wrap->getCollisionShape(); - - /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); -#ifndef USE_PERSISTENT_CONTACTS +#ifndef USE_PERSISTENT_CONTACTS m_manifoldPtr->clearManifold(); -#endif //USE_PERSISTENT_CONTACTS +#endif //USE_PERSISTENT_CONTACTS btDiscreteCollisionDetectorInterface::ClosestPointInput input; input.m_maximumDistanceSquared = BT_LARGE_FLOAT; input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB = body1Wrap->getWorldTransform(); - btBoxBoxDetector detector(box0,box1); - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + btBoxBoxDetector detector(box0, box1); + detector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); #ifdef USE_PERSISTENT_CONTACTS // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added @@ -73,11 +70,10 @@ void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrappe { resultOut->refreshContactPoints(); } -#endif //USE_PERSISTENT_CONTACTS - +#endif //USE_PERSISTENT_CONTACTS } -btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/) { //not yet return 1.f; diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h index 59808df5a..eb2106576 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -26,22 +26,22 @@ class btPersistentManifold; ///box-box collision detection class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + public: btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btBoxBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btBoxBoxCollisionAlgorithm(); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -49,18 +49,15 @@ public: } } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { int bbsize = sizeof(btBoxBoxCollisionAlgorithm); void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap); + return new (ptr) btBoxBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap); } }; - }; -#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H - +#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index 7043bde34..202039956 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -24,14 +24,12 @@ subject to the following restrictions: #include #include -btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2) -: m_box1(box1), -m_box2(box2) +btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2) + : m_box1(box1), + m_box2(box2) { - } - // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and // generate contact points. this returns 0 if there is no contact otherwise // it returns the number of contacts generated. @@ -48,67 +46,66 @@ m_box2(box2) // collision functions. this function only fills in the position and depth // fields. struct dContactGeom; -#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) +#define dDOTpq(a, b, p, q) ((a)[0] * (b)[0] + (a)[p] * (b)[q] + (a)[2 * (p)] * (b)[2 * (q)]) #define dInfinity FLT_MAX - /*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); } PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); } PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); } */ -static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } -static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); } -static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); } -static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); } -#define dMULTIPLYOP1_331(A,op,B,C) \ -{\ - (A)[0] op dDOT41((B),(C)); \ - (A)[1] op dDOT41((B+1),(C)); \ - (A)[2] op dDOT41((B+2),(C)); \ -} +static btScalar dDOT(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 1); } +static btScalar dDOT44(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 4); } +static btScalar dDOT41(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 1); } +static btScalar dDOT14(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 4); } +#define dMULTIPLYOP1_331(A, op, B, C) \ + { \ + (A)[0] op dDOT41((B), (C)); \ + (A)[1] op dDOT41((B + 1), (C)); \ + (A)[2] op dDOT41((B + 2), (C)); \ + } -#define dMULTIPLYOP0_331(A,op,B,C) \ -{ \ - (A)[0] op dDOT((B),(C)); \ - (A)[1] op dDOT((B+4),(C)); \ - (A)[2] op dDOT((B+8),(C)); \ -} +#define dMULTIPLYOP0_331(A, op, B, C) \ + { \ + (A)[0] op dDOT((B), (C)); \ + (A)[1] op dDOT((B + 4), (C)); \ + (A)[2] op dDOT((B + 8), (C)); \ + } -#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) -#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) +#define dMULTIPLY1_331(A, B, C) dMULTIPLYOP1_331(A, =, B, C) +#define dMULTIPLY0_331(A, B, C) dMULTIPLYOP0_331(A, =, B, C) -typedef btScalar dMatrix3[4*3]; +typedef btScalar dMatrix3[4 * 3]; -void dLineClosestApproach (const btVector3& pa, const btVector3& ua, - const btVector3& pb, const btVector3& ub, - btScalar *alpha, btScalar *beta); -void dLineClosestApproach (const btVector3& pa, const btVector3& ua, - const btVector3& pb, const btVector3& ub, - btScalar *alpha, btScalar *beta) +void dLineClosestApproach(const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar* alpha, btScalar* beta); +void dLineClosestApproach(const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar* alpha, btScalar* beta) { - btVector3 p; - p[0] = pb[0] - pa[0]; - p[1] = pb[1] - pa[1]; - p[2] = pb[2] - pa[2]; - btScalar uaub = dDOT(ua,ub); - btScalar q1 = dDOT(ua,p); - btScalar q2 = -dDOT(ub,p); - btScalar d = 1-uaub*uaub; - if (d <= btScalar(0.0001f)) { - // @@@ this needs to be made more robust - *alpha = 0; - *beta = 0; - } - else { - d = 1.f/d; - *alpha = (q1 + uaub*q2)*d; - *beta = (uaub*q1 + q2)*d; - } + btVector3 p; + p[0] = pb[0] - pa[0]; + p[1] = pb[1] - pa[1]; + p[2] = pb[2] - pa[2]; + btScalar uaub = dDOT(ua, ub); + btScalar q1 = dDOT(ua, p); + btScalar q2 = -dDOT(ub, p); + btScalar d = 1 - uaub * uaub; + if (d <= btScalar(0.0001f)) + { + // @@@ this needs to be made more robust + *alpha = 0; + *beta = 0; + } + else + { + d = 1.f / d; + *alpha = (q1 + uaub * q2) * d; + *beta = (uaub * q1 + q2) * d; + } } - - // find all the intersection points between the 2D rectangle with vertices // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). @@ -117,60 +114,66 @@ void dLineClosestApproach (const btVector3& pa, const btVector3& ua, // the number of intersection points is returned by the function (this will // be in the range 0 to 8). -static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) +static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16]) { - // q (and r) contain nq (and nr) coordinate points for the current (and - // chopped) polygons - int nq=4,nr=0; - btScalar buffer[16]; - btScalar *q = p; - btScalar *r = ret; - for (int dir=0; dir <= 1; dir++) { - // direction notation: xy[0] = x axis, xy[1] = y axis - for (int sign=-1; sign <= 1; sign += 2) { - // chop q along the line xy[dir] = sign*h[dir] - btScalar *pq = q; - btScalar *pr = r; - nr = 0; - for (int i=nq; i > 0; i--) { - // go through all points in q and all lines between adjacent points - if (sign*pq[dir] < h[dir]) { - // this point is inside the chopping line - pr[0] = pq[0]; - pr[1] = pq[1]; - pr += 2; - nr++; - if (nr & 8) { - q = r; - goto done; - } + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + int nq = 4, nr = 0; + btScalar buffer[16]; + btScalar* q = p; + btScalar* r = ret; + for (int dir = 0; dir <= 1; dir++) + { + // direction notation: xy[0] = x axis, xy[1] = y axis + for (int sign = -1; sign <= 1; sign += 2) + { + // chop q along the line xy[dir] = sign*h[dir] + btScalar* pq = q; + btScalar* pr = r; + nr = 0; + for (int i = nq; i > 0; i--) + { + // go through all points in q and all lines between adjacent points + if (sign * pq[dir] < h[dir]) + { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if (nr & 8) + { + q = r; + goto done; + } + } + btScalar* nextq = (i > 1) ? pq + 2 : q; + if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir])) + { + // this line crosses the chopping line + pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) / + (nextq[dir] - pq[dir]) * (sign * h[dir] - pq[dir]); + pr[dir] = sign * h[dir]; + pr += 2; + nr++; + if (nr & 8) + { + q = r; + goto done; + } + } + pq += 2; + } + q = r; + r = (q == ret) ? buffer : ret; + nq = nr; + } } - btScalar *nextq = (i > 1) ? pq+2 : q; - if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { - // this line crosses the chopping line - pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / - (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); - pr[dir] = sign*h[dir]; - pr += 2; - nr++; - if (nr & 8) { - q = r; - goto done; - } - } - pq += 2; - } - q = r; - r = (q==ret) ? buffer : ret; - nq = nr; - } - } - done: - if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar)); - return nr; +done: + if (q != ret) memcpy(ret, q, nr * 2 * sizeof(btScalar)); + return nr; } - #define M__PI 3.14159265f // given n points in the plane (array p, of size 2*n), generate m points that @@ -181,538 +184,584 @@ static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be // in the range [0..n-1]. -void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]); -void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) +void cullPoints2(int n, btScalar p[], int m, int i0, int iret[]); +void cullPoints2(int n, btScalar p[], int m, int i0, int iret[]) { - // compute the centroid of the polygon in cx,cy - int i,j; - btScalar a,cx,cy,q; - if (n==1) { - cx = p[0]; - cy = p[1]; - } - else if (n==2) { - cx = btScalar(0.5)*(p[0] + p[2]); - cy = btScalar(0.5)*(p[1] + p[3]); - } - else { - a = 0; - cx = 0; - cy = 0; - for (i=0; i<(n-1); i++) { - q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; - a += q; - cx += q*(p[i*2]+p[i*2+2]); - cy += q*(p[i*2+1]+p[i*2+3]); - } - q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; - if (btFabs(a+q) > SIMD_EPSILON) + // compute the centroid of the polygon in cx,cy + int i, j; + btScalar a, cx, cy, q; + if (n == 1) { - a = 1.f/(btScalar(3.0)*(a+q)); - } else + cx = p[0]; + cy = p[1]; + } + else if (n == 2) { - a=BT_LARGE_FLOAT; + cx = btScalar(0.5) * (p[0] + p[2]); + cy = btScalar(0.5) * (p[1] + p[3]); } - cx = a*(cx + q*(p[n*2-2]+p[0])); - cy = a*(cy + q*(p[n*2-1]+p[1])); - } - - // compute the angle of each point w.r.t. the centroid - btScalar A[8]; - for (i=0; i M__PI) a -= 2*M__PI; - btScalar maxdiff=1e9,diff; - - *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 - - for (i=0; i M__PI) diff = 2*M__PI - diff; - if (diff < maxdiff) { - maxdiff = diff; - *iret = i; + else + { + a = 0; + cx = 0; + cy = 0; + for (i = 0; i < (n - 1); i++) + { + q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1]; + a += q; + cx += q * (p[i * 2] + p[i * 2 + 2]); + cy += q * (p[i * 2 + 1] + p[i * 2 + 3]); + } + q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1]; + if (btFabs(a + q) > SIMD_EPSILON) + { + a = 1.f / (btScalar(3.0) * (a + q)); + } + else + { + a = BT_LARGE_FLOAT; + } + cx = a * (cx + q * (p[n * 2 - 2] + p[0])); + cy = a * (cy + q * (p[n * 2 - 1] + p[1])); } - } - } -#if defined(DEBUG) || defined (_DEBUG) - btAssert (*iret != i0); // ensure iret got set + + // compute the angle of each point w.r.t. the centroid + btScalar A[8]; + for (i = 0; i < n; i++) A[i] = btAtan2(p[i * 2 + 1] - cy, p[i * 2] - cx); + + // search for points that have angles closest to A[i0] + i*(2*pi/m). + int avail[8]; + for (i = 0; i < n; i++) avail[i] = 1; + avail[i0] = 0; + iret[0] = i0; + iret++; + for (j = 1; j < m; j++) + { + a = btScalar(j) * (2 * M__PI / m) + A[i0]; + if (a > M__PI) a -= 2 * M__PI; + btScalar maxdiff = 1e9, diff; + + *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 + + for (i = 0; i < n; i++) + { + if (avail[i]) + { + diff = btFabs(A[i] - a); + if (diff > M__PI) diff = 2 * M__PI - diff; + if (diff < maxdiff) + { + maxdiff = diff; + *iret = i; + } + } + } +#if defined(DEBUG) || defined(_DEBUG) + btAssert(*iret != i0); // ensure iret got set #endif - avail[*iret] = 0; - iret++; - } + avail[*iret] = 0; + iret++; + } } - - -int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, - const btVector3& side1, const btVector3& p2, - const dMatrix3 R2, const btVector3& side2, - btVector3& normal, btScalar *depth, int *return_code, - int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output); -int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, - const btVector3& side1, const btVector3& p2, - const dMatrix3 R2, const btVector3& side2, - btVector3& normal, btScalar *depth, int *return_code, - int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) +int dBoxBox2(const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar* depth, int* return_code, + int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output); +int dBoxBox2(const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar* depth, int* return_code, + int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output) { - const btScalar fudge_factor = btScalar(1.05); - btVector3 p,pp,normalC(0.f,0.f,0.f); - const btScalar *normalR = 0; - btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, - Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; - int i,j,invert_normal,code; + const btScalar fudge_factor = btScalar(1.05); + btVector3 p, pp, normalC(0.f, 0.f, 0.f); + const btScalar* normalR = 0; + btScalar A[3], B[3], R11, R12, R13, R21, R22, R23, R31, R32, R33, + Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33, s, s2, l; + int i, j, invert_normal, code; - // get vector from centers of box 1 to box 2, relative to box 1 - p = p2 - p1; - dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 + // get vector from centers of box 1 to box 2, relative to box 1 + p = p2 - p1; + dMULTIPLY1_331(pp, R1, p); // get pp = p relative to body 1 - // get side lengths / 2 - A[0] = side1[0]*btScalar(0.5); - A[1] = side1[1]*btScalar(0.5); - A[2] = side1[2]*btScalar(0.5); - B[0] = side2[0]*btScalar(0.5); - B[1] = side2[1]*btScalar(0.5); - B[2] = side2[2]*btScalar(0.5); + // get side lengths / 2 + A[0] = side1[0] * btScalar(0.5); + A[1] = side1[1] * btScalar(0.5); + A[2] = side1[2] * btScalar(0.5); + B[0] = side2[0] * btScalar(0.5); + B[1] = side2[1] * btScalar(0.5); + B[2] = side2[2] * btScalar(0.5); - // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 - R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); - R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); - R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + R11 = dDOT44(R1 + 0, R2 + 0); + R12 = dDOT44(R1 + 0, R2 + 1); + R13 = dDOT44(R1 + 0, R2 + 2); + R21 = dDOT44(R1 + 1, R2 + 0); + R22 = dDOT44(R1 + 1, R2 + 1); + R23 = dDOT44(R1 + 1, R2 + 2); + R31 = dDOT44(R1 + 2, R2 + 0); + R32 = dDOT44(R1 + 2, R2 + 1); + R33 = dDOT44(R1 + 2, R2 + 2); - Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); - Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); - Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33); + Q11 = btFabs(R11); + Q12 = btFabs(R12); + Q13 = btFabs(R13); + Q21 = btFabs(R21); + Q22 = btFabs(R22); + Q23 = btFabs(R23); + Q31 = btFabs(R31); + Q32 = btFabs(R32); + Q33 = btFabs(R33); - // for all 15 possible separating axes: - // * see if the axis separates the boxes. if so, return 0. - // * find the depth of the penetration along the separating axis (s2) - // * if this is the largest depth so far, record it. - // the normal vector will be set to the separating axis with the smallest - // depth. note: normalR is set to point to a column of R1 or R2 if that is - // the smallest depth normal so far. otherwise normalR is 0 and normalC is - // set to a vector relative to body 1. invert_normal is 1 if the sign of - // the normal should be flipped. + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. -#define TST(expr1,expr2,norm,cc) \ - s2 = btFabs(expr1) - (expr2); \ - if (s2 > 0) return 0; \ - if (s2 > s) { \ - s = s2; \ - normalR = norm; \ - invert_normal = ((expr1) < 0); \ - code = (cc); \ - } +#define TST(expr1, expr2, norm, cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) \ + { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } - s = -dInfinity; - invert_normal = 0; - code = 0; + s = -dInfinity; + invert_normal = 0; + code = 0; - // separating axis = u1,u2,u3 - TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); - TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); - TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); + // separating axis = u1,u2,u3 + TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1 + 0, 1); + TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1 + 1, 2); + TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1 + 2, 3); - // separating axis = v1,v2,v3 - TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); - TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); - TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); + // separating axis = v1,v2,v3 + TST(dDOT41(R2 + 0, p), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2 + 0, 4); + TST(dDOT41(R2 + 1, p), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2 + 1, 5); + TST(dDOT41(R2 + 2, p), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2 + 2, 6); - // note: cross product axes need to be scaled when s is computed. - // normal (n1,n2,n3) is relative to box 1. + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to box 1. #undef TST -#define TST(expr1,expr2,n1,n2,n3,cc) \ - s2 = btFabs(expr1) - (expr2); \ - if (s2 > SIMD_EPSILON) return 0; \ - l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ - if (l > SIMD_EPSILON) { \ - s2 /= l; \ - if (s2*fudge_factor > s) { \ - s = s2; \ - normalR = 0; \ - normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ - invert_normal = ((expr1) < 0); \ - code = (cc); \ - } \ - } +#define TST(expr1, expr2, n1, n2, n3, cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > SIMD_EPSILON) return 0; \ + l = btSqrt((n1) * (n1) + (n2) * (n2) + (n3) * (n3)); \ + if (l > SIMD_EPSILON) \ + { \ + s2 /= l; \ + if (s2 * fudge_factor > s) \ + { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1) / l; \ + normalC[1] = (n2) / l; \ + normalC[2] = (n3) / l; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } \ + } - btScalar fudge2 (1.0e-5f); + btScalar fudge2(1.0e-5f); - Q11 += fudge2; - Q12 += fudge2; - Q13 += fudge2; + Q11 += fudge2; + Q12 += fudge2; + Q13 += fudge2; - Q21 += fudge2; - Q22 += fudge2; - Q23 += fudge2; + Q21 += fudge2; + Q22 += fudge2; + Q23 += fudge2; - Q31 += fudge2; - Q32 += fudge2; - Q33 += fudge2; + Q31 += fudge2; + Q32 += fudge2; + Q33 += fudge2; - // separating axis = u1 x (v1,v2,v3) - TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); - TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); - TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); + // separating axis = u1 x (v1,v2,v3) + TST(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, 7); + TST(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, 8); + TST(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, 9); - // separating axis = u2 x (v1,v2,v3) - TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); - TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); - TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); + // separating axis = u2 x (v1,v2,v3) + TST(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, 10); + TST(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, 11); + TST(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, 12); - // separating axis = u3 x (v1,v2,v3) - TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); - TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); - TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); + // separating axis = u3 x (v1,v2,v3) + TST(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, 13); + TST(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, 14); + TST(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, 15); #undef TST - if (!code) return 0; - - // if we get to this point, the boxes interpenetrate. compute the normal - // in global coordinates. - if (normalR) { - normal[0] = normalR[0]; - normal[1] = normalR[4]; - normal[2] = normalR[8]; - } - else { - dMULTIPLY0_331 (normal,R1,normalC); - } - if (invert_normal) { - normal[0] = -normal[0]; - normal[1] = -normal[1]; - normal[2] = -normal[2]; - } - *depth = -s; - - // compute contact point(s) - - if (code > 6) { - // an edge from box 1 touches an edge from box 2. - // find a point pa on the intersecting edge of box 1 - btVector3 pa; - btScalar sign; - for (i=0; i<3; i++) pa[i] = p1[i]; - for (j=0; j<3; j++) { - sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0); - for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; - } - - // find a point pb on the intersecting edge of box 2 - btVector3 pb; - for (i=0; i<3; i++) pb[i] = p2[i]; - for (j=0; j<3; j++) { - sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0); - for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; - } - - btScalar alpha,beta; - btVector3 ua,ub; - for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; - for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; - - dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); - for (i=0; i<3; i++) pa[i] += ua[i]*alpha; - for (i=0; i<3; i++) pb[i] += ub[i]*beta; + if (!code) return 0; + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) { - - //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); - //contact[0].depth = *depth; - btVector3 pointInWorld; + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else + { + dMULTIPLY0_331(normal, R1, normalC); + } + if (invert_normal) + { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (code > 6) + { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + btVector3 pa; + btScalar sign; + for (i = 0; i < 3; i++) pa[i] = p1[i]; + for (j = 0; j < 3; j++) + { + sign = (dDOT14(normal, R1 + j) > 0) ? btScalar(1.0) : btScalar(-1.0); + for (i = 0; i < 3; i++) pa[i] += sign * A[j] * R1[i * 4 + j]; + } + + // find a point pb on the intersecting edge of box 2 + btVector3 pb; + for (i = 0; i < 3; i++) pb[i] = p2[i]; + for (j = 0; j < 3; j++) + { + sign = (dDOT14(normal, R2 + j) > 0) ? btScalar(-1.0) : btScalar(1.0); + for (i = 0; i < 3; i++) pb[i] += sign * B[j] * R2[i * 4 + j]; + } + + btScalar alpha, beta; + btVector3 ua, ub; + for (i = 0; i < 3; i++) ua[i] = R1[((code)-7) / 3 + i * 4]; + for (i = 0; i < 3; i++) ub[i] = R2[((code)-7) % 3 + i * 4]; + + dLineClosestApproach(pa, ua, pb, ub, &alpha, &beta); + for (i = 0; i < 3; i++) pa[i] += ua[i] * alpha; + for (i = 0; i < 3; i++) pb[i] += ub[i] * beta; + + { + //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); + //contact[0].depth = *depth; + btVector3 pointInWorld; #ifdef USE_CENTER_POINT - for (i=0; i<3; i++) - pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5); - output.addContactPoint(-normal,pointInWorld,-*depth); + for (i = 0; i < 3; i++) + pointInWorld[i] = (pa[i] + pb[i]) * btScalar(0.5); + output.addContactPoint(-normal, pointInWorld, -*depth); #else - output.addContactPoint(-normal,pb,-*depth); + output.addContactPoint(-normal, pb, -*depth); -#endif // - *return_code = code; +#endif // + *return_code = code; + } + return 1; } - return 1; - } - // okay, we have a face-something intersection (because the separating - // axis is perpendicular to a face). define face 'a' to be the reference - // face (i.e. the normal vector is perpendicular to this) and face 'b' to be - // the incident face (the closest face of the other box). + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). - const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb; - if (code <= 3) { - Ra = R1; - Rb = R2; - pa = p1; - pb = p2; - Sa = A; - Sb = B; - } - else { - Ra = R2; - Rb = R1; - pa = p2; - pb = p1; - Sa = B; - Sb = A; - } - - // nr = normal vector of reference face dotted with axes of incident box. - // anr = absolute values of nr. - btVector3 normal2,nr,anr; - if (code <= 3) { - normal2[0] = normal[0]; - normal2[1] = normal[1]; - normal2[2] = normal[2]; - } - else { - normal2[0] = -normal[0]; - normal2[1] = -normal[1]; - normal2[2] = -normal[2]; - } - dMULTIPLY1_331 (nr,Rb,normal2); - anr[0] = btFabs (nr[0]); - anr[1] = btFabs (nr[1]); - anr[2] = btFabs (nr[2]); - - // find the largest compontent of anr: this corresponds to the normal - // for the indident face. the other axis numbers of the indicent face - // are stored in a1,a2. - int lanr,a1,a2; - if (anr[1] > anr[0]) { - if (anr[1] > anr[2]) { - a1 = 0; - lanr = 1; - a2 = 2; - } - else { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - else { - if (anr[0] > anr[2]) { - lanr = 0; - a1 = 1; - a2 = 2; - } - else { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - - // compute center point of incident face, in reference-face coordinates - btVector3 center; - if (nr[lanr] < 0) { - for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; - } - else { - for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; - } - - // find the normal and non-normal axis numbers of the reference box - int codeN,code1,code2; - if (code <= 3) codeN = code-1; else codeN = code-4; - if (codeN==0) { - code1 = 1; - code2 = 2; - } - else if (codeN==1) { - code1 = 0; - code2 = 2; - } - else { - code1 = 0; - code2 = 1; - } - - // find the four corners of the incident face, in reference-face coordinates - btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) - btScalar c1,c2,m11,m12,m21,m22; - c1 = dDOT14 (center,Ra+code1); - c2 = dDOT14 (center,Ra+code2); - // optimize this? - we have already computed this data above, but it is not - // stored in an easy-to-index format. for now it's quicker just to recompute - // the four dot products. - m11 = dDOT44 (Ra+code1,Rb+a1); - m12 = dDOT44 (Ra+code1,Rb+a2); - m21 = dDOT44 (Ra+code2,Rb+a1); - m22 = dDOT44 (Ra+code2,Rb+a2); - { - btScalar k1 = m11*Sb[a1]; - btScalar k2 = m21*Sb[a1]; - btScalar k3 = m12*Sb[a2]; - btScalar k4 = m22*Sb[a2]; - quad[0] = c1 - k1 - k3; - quad[1] = c2 - k2 - k4; - quad[2] = c1 - k1 + k3; - quad[3] = c2 - k2 + k4; - quad[4] = c1 + k1 + k3; - quad[5] = c2 + k2 + k4; - quad[6] = c1 + k1 - k3; - quad[7] = c2 + k2 - k4; - } - - // find the size of the reference face - btScalar rect[2]; - rect[0] = Sa[code1]; - rect[1] = Sa[code2]; - - // intersect the incident and reference faces - btScalar ret[16]; - int n = intersectRectQuad2 (rect,quad,ret); - if (n < 1) return 0; // this should never happen - - // convert the intersection points into reference-face coordinates, - // and compute the contact position and depth for each point. only keep - // those points that have a positive (penetrating) depth. delete points in - // the 'ret' array as necessary so that 'point' and 'ret' correspond. - btScalar point[3*8]; // penetrating contact points - btScalar dep[8]; // depths for those points - btScalar det1 = 1.f/(m11*m22 - m12*m21); - m11 *= det1; - m12 *= det1; - m21 *= det1; - m22 *= det1; - int cnum = 0; // number of penetrating contact points found - for (j=0; j < n; j++) { - btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); - btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); - for (i=0; i<3; i++) point[cnum*3+i] = - center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; - dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); - if (dep[cnum] >= 0) { - ret[cnum*2] = ret[j*2]; - ret[cnum*2+1] = ret[j*2+1]; - cnum++; - } - } - if (cnum < 1) return 0; // this should never happen - - // we can't generate more contacts than we actually have - if (maxc > cnum) maxc = cnum; - if (maxc < 1) maxc = 1; - - if (cnum <= maxc) { - - if (code<4) - { - // we have less contacts than we need, so we use them all - for (j=0; j < cnum; j++) + const btScalar *Ra, *Rb, *pa, *pb, *Sa, *Sb; + if (code <= 3) { - btVector3 pointInWorld; - for (i=0; i<3; i++) - pointInWorld[i] = point[j*3+i] + pa[i]; - output.addContactPoint(-normal,pointInWorld,-dep[j]); + Ra = R1; + Rb = R2; + pa = p1; + pb = p2; + Sa = A; + Sb = B; + } + else + { + Ra = R2; + Rb = R1; + pa = p2; + pb = p1; + Sa = B; + Sb = A; + } - } - } else - { - // we have less contacts than we need, so we use them all - for (j=0; j < cnum; j++) + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + btVector3 normal2, nr, anr; + if (code <= 3) + { + normal2[0] = normal[0]; + normal2[1] = normal[1]; + normal2[2] = normal[2]; + } + else + { + normal2[0] = -normal[0]; + normal2[1] = -normal[1]; + normal2[2] = -normal[2]; + } + dMULTIPLY1_331(nr, Rb, normal2); + anr[0] = btFabs(nr[0]); + anr[1] = btFabs(nr[1]); + anr[2] = btFabs(nr[2]); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr, a1, a2; + if (anr[1] > anr[0]) + { + if (anr[1] > anr[2]) { - btVector3 pointInWorld; - for (i=0; i<3; i++) - pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j]; + a1 = 0; + lanr = 1; + a2 = 2; + } + else + { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else + { + if (anr[0] > anr[2]) + { + lanr = 0; + a1 = 1; + a2 = 2; + } + else + { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + btVector3 center; + if (nr[lanr] < 0) + { + for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr]; + } + else + { + for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr]; + } + + // find the normal and non-normal axis numbers of the reference box + int codeN, code1, code2; + if (code <= 3) + codeN = code - 1; + else + codeN = code - 4; + if (codeN == 0) + { + code1 = 1; + code2 = 2; + } + else if (codeN == 1) + { + code1 = 0; + code2 = 2; + } + else + { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) + btScalar c1, c2, m11, m12, m21, m22; + c1 = dDOT14(center, Ra + code1); + c2 = dDOT14(center, Ra + code2); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + m11 = dDOT44(Ra + code1, Rb + a1); + m12 = dDOT44(Ra + code1, Rb + a2); + m21 = dDOT44(Ra + code2, Rb + a1); + m22 = dDOT44(Ra + code2, Rb + a2); + { + btScalar k1 = m11 * Sb[a1]; + btScalar k2 = m21 * Sb[a1]; + btScalar k3 = m12 * Sb[a2]; + btScalar k4 = m22 * Sb[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + } + + // find the size of the reference face + btScalar rect[2]; + rect[0] = Sa[code1]; + rect[1] = Sa[code2]; + + // intersect the incident and reference faces + btScalar ret[16]; + int n = intersectRectQuad2(rect, quad, ret); + if (n < 1) return 0; // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + btScalar point[3 * 8]; // penetrating contact points + btScalar dep[8]; // depths for those points + btScalar det1 = 1.f / (m11 * m22 - m12 * m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for (j = 0; j < n; j++) + { + btScalar k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2); + btScalar k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2); + for (i = 0; i < 3; i++) point[cnum * 3 + i] = + center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2]; + dep[cnum] = Sa[codeN] - dDOT(normal2, point + cnum * 3); + if (dep[cnum] >= 0) + { + ret[cnum * 2] = ret[j * 2]; + ret[cnum * 2 + 1] = ret[j * 2 + 1]; + cnum++; + } + } + if (cnum < 1) return 0; // this should never happen + + // we can't generate more contacts than we actually have + if (maxc > cnum) maxc = cnum; + if (maxc < 1) maxc = 1; + + if (cnum <= maxc) + { + if (code < 4) + { + // we have less contacts than we need, so we use them all + for (j = 0; j < cnum; j++) + { + btVector3 pointInWorld; + for (i = 0; i < 3; i++) + pointInWorld[i] = point[j * 3 + i] + pa[i]; + output.addContactPoint(-normal, pointInWorld, -dep[j]); + } + } + else + { + // we have less contacts than we need, so we use them all + for (j = 0; j < cnum; j++) + { + btVector3 pointInWorld; + for (i = 0; i < 3; i++) + pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j]; //pointInWorld[i] = point[j*3+i] + pa[i]; - output.addContactPoint(-normal,pointInWorld,-dep[j]); + output.addContactPoint(-normal, pointInWorld, -dep[j]); + } } - } - } - else { - // we have more contacts than are wanted, some of them must be culled. - // find the deepest point, it is always the first contact. - int i1 = 0; - btScalar maxdepth = dep[0]; - for (i=1; i maxdepth) { - maxdepth = dep[i]; - i1 = i; - } - } - - int iret[8]; - cullPoints2 (cnum,ret,maxc,i1,iret); - - for (j=0; j < maxc; j++) { -// dContactGeom *con = CONTACT(contact,skip*j); - // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; - // con->depth = dep[iret[j]]; - - btVector3 posInWorld; - for (i=0; i<3; i++) - posInWorld[i] = point[iret[j]*3+i] + pa[i]; - if (code<4) - { - output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); - } else + } + else + { + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + int i1 = 0; + btScalar maxdepth = dep[0]; + for (i = 1; i < cnum; i++) { - output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]); + if (dep[i] > maxdepth) + { + maxdepth = dep[i]; + i1 = i; + } } - } - cnum = maxc; - } - *return_code = code; - return cnum; + int iret[8]; + cullPoints2(cnum, ret, maxc, i1, iret); + + for (j = 0; j < maxc; j++) + { + // dContactGeom *con = CONTACT(contact,skip*j); + // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; + // con->depth = dep[iret[j]]; + + btVector3 posInWorld; + for (i = 0; i < 3; i++) + posInWorld[i] = point[iret[j] * 3 + i] + pa[i]; + if (code < 4) + { + output.addContactPoint(-normal, posInWorld, -dep[iret[j]]); + } + else + { + output.addContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]); + } + } + cnum = maxc; + } + + *return_code = code; + return cnum; } -void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/) +void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* /*debugDraw*/, bool /*swapResults*/) { - const btTransform& transformA = input.m_transformA; const btTransform& transformB = input.m_transformB; - + int skip = 0; - dContactGeom *contact = 0; + dContactGeom* contact = 0; dMatrix3 R1; dMatrix3 R2; - for (int j=0;j<3;j++) + for (int j = 0; j < 3; j++) { - R1[0+4*j] = transformA.getBasis()[j].x(); - R2[0+4*j] = transformB.getBasis()[j].x(); + R1[0 + 4 * j] = transformA.getBasis()[j].x(); + R2[0 + 4 * j] = transformB.getBasis()[j].x(); - R1[1+4*j] = transformA.getBasis()[j].y(); - R2[1+4*j] = transformB.getBasis()[j].y(); - - - R1[2+4*j] = transformA.getBasis()[j].z(); - R2[2+4*j] = transformB.getBasis()[j].z(); + R1[1 + 4 * j] = transformA.getBasis()[j].y(); + R2[1 + 4 * j] = transformB.getBasis()[j].y(); + R1[2 + 4 * j] = transformA.getBasis()[j].z(); + R2[2 + 4 * j] = transformB.getBasis()[j].z(); } - - btVector3 normal; btScalar depth; int return_code; int maxc = 4; - - dBoxBox2 (transformA.getOrigin(), - R1, - 2.f*m_box1->getHalfExtentsWithMargin(), - transformB.getOrigin(), - R2, - 2.f*m_box2->getHalfExtentsWithMargin(), - normal, &depth, &return_code, - maxc, contact, skip, - output - ); - + dBoxBox2(transformA.getOrigin(), + R1, + 2.f * m_box1->getHalfExtentsWithMargin(), + transformB.getOrigin(), + R2, + 2.f * m_box2->getHalfExtentsWithMargin(), + normal, &depth, &return_code, + maxc, contact, skip, + output); } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h index 392437770..9f7d988fc 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h @@ -19,11 +19,9 @@ subject to the following restrictions: #ifndef BT_BOX_BOX_DETECTOR_H #define BT_BOX_BOX_DETECTOR_H - class btBoxShape; #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" - /// btBoxBoxDetector wraps the ODE box-box collision detector /// re-distributed under the Zlib license with permission from Russell L. Smith struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface @@ -32,13 +30,11 @@ struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface const btBoxShape* m_box2; public: + btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2); - btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2); - - virtual ~btBoxBoxDetector() {}; - - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + virtual ~btBoxBoxDetector(){}; + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false); }; -#endif //BT_BOX_BOX_DETECTOR_H +#endif //BT_BOX_BOX_DETECTOR_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index 35f77d4e6..d6e15f555 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -23,11 +23,9 @@ class btPoolAllocator; ///btCollisionConfiguration allows to configure Bullet collision detection ///stack allocator size, default collision algorithms and persistent manifold pool size ///@todo: describe the meaning -class btCollisionConfiguration +class btCollisionConfiguration { - public: - virtual ~btCollisionConfiguration() { } @@ -37,13 +35,9 @@ public: virtual btPoolAllocator* getCollisionAlgorithmPool() = 0; - - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0; virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0; - - }; -#endif //BT_COLLISION_CONFIGURATION - +#endif //BT_COLLISION_CONFIGURATION diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index 62ee66c4e..bd8128493 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -26,20 +26,18 @@ struct btCollisionAlgorithmConstructionInfo; struct btCollisionAlgorithmCreateFunc { bool m_swapped; - + btCollisionAlgorithmCreateFunc() - :m_swapped(false) + : m_swapped(false) { } virtual ~btCollisionAlgorithmCreateFunc(){}; - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo&, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - (void)body0Wrap; (void)body1Wrap; return 0; } }; -#endif //BT_COLLISION_CREATE_FUNC - +#endif //BT_COLLISION_CREATE_FUNC diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 5739a1ef0..25b2b1ea4 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btCollisionDispatcher.h" #include "LinearMath/btQuickprof.h" @@ -27,46 +25,38 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -int gNumManifold = 0; - #ifdef BT_DEBUG #include #endif - -btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): -m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD), - m_collisionConfiguration(collisionConfiguration) +btCollisionDispatcher::btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration) : m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD), + m_collisionConfiguration(collisionConfiguration) { int i; setNearCallback(defaultNearCallback); - + m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool(); m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool(); - for (i=0;igetCollisionAlgorithmCreateFunc(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); - } } - - } - -void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) +void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc) { m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc; } -void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) +void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc) { m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc; } @@ -75,37 +65,33 @@ btCollisionDispatcher::~btCollisionDispatcher() { } -btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) -{ - gNumManifold++; - +btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) +{ //btAssert(gNumManifold < 65535); - - //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) + 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) + //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 + 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); + btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold); manifold->m_index1a = m_manifoldsPtr.size(); m_manifoldsPtr.push_back(manifold); @@ -117,18 +103,14 @@ void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold) manifold->clearManifold(); } - void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) { - - gNumManifold--; - //printf("releaseManifold: gNumManifold %d\n",gNumManifold); clearManifold(manifold); int findIndex = manifold->m_index1a; btAssert(findIndex < m_manifoldsPtr.size()); - m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); + m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1); m_manifoldsPtr[findIndex]->m_index1a = findIndex; m_manifoldsPtr.pop_back(); @@ -136,19 +118,15 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) if (m_persistentManifoldPoolAllocator->validPtr(manifold)) { m_persistentManifoldPoolAllocator->freeMemory(manifold); - } else + } + else { btAlignedFree(manifold); } - } - - - -btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType) +btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType) { - btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = this; @@ -166,21 +144,18 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObje return algo; } - - - -bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0,const btCollisionObject* body1) +bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0, const btCollisionObject* body1) { //here you can do filtering - bool hasResponse = + bool hasResponse = (body0->hasContactResponse() && body1->hasContactResponse()); //no response between two static/kinematic bodies: hasResponse = hasResponse && - ((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject())); + ((!body0->isStaticOrKinematicObject()) || (!body1->isStaticOrKinematicObject())); return hasResponse; } -bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const btCollisionObject* body1) +bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) { btAssert(body0); btAssert(body1); @@ -197,31 +172,27 @@ bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); } } -#endif //BT_DEBUG +#endif //BT_DEBUG if ((!body0->isActive()) && (!body1->isActive())) needsCollision = false; else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0))) needsCollision = false; - - return needsCollision ; + return needsCollision; } - - ///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc) ///this is useful for the collision dispatcher. class btCollisionPairCallback : public btOverlapCallback { const btDispatcherInfo& m_dispatchInfo; - btCollisionDispatcher* m_dispatcher; + btCollisionDispatcher* m_dispatcher; public: - - btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher) - :m_dispatchInfo(dispatchInfo), - m_dispatcher(dispatcher) + btCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher) + : m_dispatchInfo(dispatchInfo), + m_dispatcher(dispatcher) { } @@ -233,83 +204,76 @@ public: } */ - virtual ~btCollisionPairCallback() {} - - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { - (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); + (*m_dispatcher->getNearCallback())(pair, *m_dispatcher, m_dispatchInfo); return false; } }; - - -void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) +void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) { //m_blockedForChanges = true; - btCollisionPairCallback collisionCallback(dispatchInfo,this); + btCollisionPairCallback collisionCallback(dispatchInfo, this); - pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); + { + BT_PROFILE("processAllOverlappingPairs"); + pairCache->processAllOverlappingPairs(&collisionCallback, dispatcher, dispatchInfo); + } //m_blockedForChanges = false; - } - - //by default, Bullet will use this near callback void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) { - btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; - if (dispatcher.needsCollision(colObj0,colObj1)) + if (dispatcher.needsCollision(colObj0, colObj1)) + { + btCollisionObjectWrapper obj0Wrap(0, colObj0->getCollisionShape(), colObj0, colObj0->getWorldTransform(), -1, -1); + btCollisionObjectWrapper obj1Wrap(0, colObj1->getCollisionShape(), colObj1, colObj1->getWorldTransform(), -1, -1); + + //dispatcher will keep algorithms persistent in the collision pair + if (!collisionPair.m_algorithm) { - btCollisionObjectWrapper obj0Wrap(0,colObj0->getCollisionShape(),colObj0,colObj0->getWorldTransform(),-1,-1); - btCollisionObjectWrapper obj1Wrap(0,colObj1->getCollisionShape(),colObj1,colObj1->getWorldTransform(),-1,-1); - - - //dispatcher will keep algorithms persistent in the collision pair - if (!collisionPair.m_algorithm) - { - collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap,0, BT_CONTACT_POINT_ALGORITHMS); - } - - if (collisionPair.m_algorithm) - { - btManifoldResult contactPointResult(&obj0Wrap,&obj1Wrap); - - if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) - { - //discrete collision detection query - - collisionPair.m_algorithm->processCollision(&obj0Wrap,&obj1Wrap,dispatchInfo,&contactPointResult); - } else - { - //continuous collision detection query, time of impact (toi) - btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); - if (dispatchInfo.m_timeOfImpact > toi) - dispatchInfo.m_timeOfImpact = toi; - - } - } + collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap, &obj1Wrap, 0, BT_CONTACT_POINT_ALGORITHMS); } -} + if (collisionPair.m_algorithm) + { + btManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap); + if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) + { + //discrete collision detection query + + collisionPair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatchInfo, &contactPointResult); + } + else + { + //continuous collision detection query, time of impact (toi) + btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0, colObj1, dispatchInfo, &contactPointResult); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + } + } + } +} void* btCollisionDispatcher::allocateCollisionAlgorithm(int size) { - void* mem = m_collisionAlgorithmPoolAllocator->allocate( size ); - if (NULL == mem) - { - //warn user for overflow? - return btAlignedAlloc(static_cast(size), 16); - } - return mem; + 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) @@ -317,7 +281,8 @@ void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) if (m_collisionAlgorithmPoolAllocator->validPtr(ptr)) { m_collisionAlgorithmPoolAllocator->freeMemory(ptr); - } else + } + else { btAlignedFree(ptr); } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index b97ee3c1b..6b9f5e23a 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -37,35 +37,30 @@ class btCollisionDispatcher; ///user can override this nearcallback for collision filtering and more finegrained control over collision detection typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); - ///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs. ///Time of Impact, Closest Points and Penetration Depth. class btCollisionDispatcher : public btDispatcher { - protected: + int m_dispatcherFlags; - int m_dispatcherFlags; + btAlignedObjectArray m_manifoldsPtr; - btAlignedObjectArray m_manifoldsPtr; + btManifoldResult m_defaultManifoldResult; - btManifoldResult m_defaultManifoldResult; + btNearCallback m_nearCallback; - btNearCallback m_nearCallback; - - btPoolAllocator* m_collisionAlgorithmPoolAllocator; + btPoolAllocator* m_collisionAlgorithmPoolAllocator; - btPoolAllocator* m_persistentManifoldPoolAllocator; + btPoolAllocator* m_persistentManifoldPoolAllocator; 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; - + btCollisionConfiguration* m_collisionConfiguration; public: - enum DispatcherFlags { CD_STATIC_STATIC_REPORTED = 1, @@ -73,103 +68,100 @@ public: CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4 }; - int getDispatcherFlags() const + int getDispatcherFlags() const { return m_dispatcherFlags; } - void setDispatcherFlags(int flags) + void setDispatcherFlags(int flags) { m_dispatcherFlags = flags; } ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions - void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); + void registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); - void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc); + void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); - int getNumManifolds() const - { - return int( m_manifoldsPtr.size()); - } - - btPersistentManifold** getInternalManifoldPointer() + int getNumManifolds() const { - return m_manifoldsPtr.size()? &m_manifoldsPtr[0] : 0; + return int(m_manifoldsPtr.size()); } - btPersistentManifold* getManifoldByIndexInternal(int index) + btPersistentManifold** getInternalManifoldPointer() + { + return m_manifoldsPtr.size() ? &m_manifoldsPtr[0] : 0; + } + + btPersistentManifold* getManifoldByIndexInternal(int index) { return m_manifoldsPtr[index]; } - const btPersistentManifold* getManifoldByIndexInternal(int index) const + const btPersistentManifold* getManifoldByIndexInternal(int index) const { return m_manifoldsPtr[index]; } - btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration); + btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration); virtual ~btCollisionDispatcher(); - virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1); - - virtual void releaseManifold(btPersistentManifold* manifold); + virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0, const btCollisionObject* b1); + virtual void releaseManifold(btPersistentManifold* manifold); virtual void clearManifold(btPersistentManifold* manifold); - btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType); - - virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1); - - virtual bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1); - - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ; + btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType); - void setNearCallback(btNearCallback nearCallback) + virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1); + + virtual bool needsResponse(const btCollisionObject* body0, const btCollisionObject* body1); + + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher); + + void setNearCallback(btNearCallback nearCallback) { - m_nearCallback = nearCallback; + m_nearCallback = nearCallback; } - btNearCallback getNearCallback() const + btNearCallback getNearCallback() const { return m_nearCallback; } //by default, Bullet will use this near callback - static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); + static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); - virtual void* allocateCollisionAlgorithm(int size); + virtual void* allocateCollisionAlgorithm(int size); - virtual void freeCollisionAlgorithm(void* ptr); + virtual void freeCollisionAlgorithm(void* ptr); - btCollisionConfiguration* getCollisionConfiguration() + btCollisionConfiguration* getCollisionConfiguration() { return m_collisionConfiguration; } - const btCollisionConfiguration* getCollisionConfiguration() const + const btCollisionConfiguration* getCollisionConfiguration() const { return m_collisionConfiguration; } - void setCollisionConfiguration(btCollisionConfiguration* config) + void setCollisionConfiguration(btCollisionConfiguration* config) { m_collisionConfiguration = config; } - virtual btPoolAllocator* getInternalManifoldPool() + virtual btPoolAllocator* getInternalManifoldPool() { return m_persistentManifoldPoolAllocator; } - virtual const btPoolAllocator* getInternalManifoldPool() const + virtual const btPoolAllocator* getInternalManifoldPool() const { return m_persistentManifoldPoolAllocator; } - }; -#endif //BT_COLLISION__DISPATCHER_H - +#endif //BT_COLLISION__DISPATCHER_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp index 075860c50..6fe56538d 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btCollisionDispatcherMt.h" #include "LinearMath/btQuickprof.h" @@ -27,138 +25,132 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -btCollisionDispatcherMt::btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize ) - : btCollisionDispatcher( config ) +btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize) + : btCollisionDispatcher(config) { - m_batchUpdating = false; - m_grainSize = grainSize; // iterations per task + m_batchUpdating = false; + m_grainSize = grainSize; // iterations per task } - -btPersistentManifold* btCollisionDispatcherMt::getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) +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) + //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 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() ); + 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 ); - } + 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; + return manifold; } -void btCollisionDispatcherMt::releaseManifold( btPersistentManifold* 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(); - } + 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 ); - } + 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; + 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 ); - } - } + 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 ) +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; + 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; + m_batchUpdating = true; + btParallelFor(0, pairCount, m_grainSize, updater); + m_batchUpdating = false; - // reconstruct the manifolds array to ensure determinism - m_manifoldsPtr.resizeNoInitialize( 0 ); + // 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 ); - } - } + 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; - } + // 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/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h index f1d7eafdc..28eba7f32 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h @@ -19,21 +19,19 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "LinearMath/btThreads.h" - class btCollisionDispatcherMt : public btCollisionDispatcher { public: - btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize = 40 ); + 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 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; + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE; protected: - bool m_batchUpdating; - int m_grainSize; + bool m_batchUpdating; + int m_grainSize; }; -#endif //BT_COLLISION_DISPATCHER_MT_H - +#endif //BT_COLLISION_DISPATCHER_MT_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index b595c56bc..b48d9301d 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -13,41 +13,42 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btCollisionObject.h" #include "LinearMath/btSerializer.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" btCollisionObject::btCollisionObject() - : 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), - m_rootCollisionShape(0), - 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(1e4), - m_internalType(CO_COLLISION_OBJECT), - m_userObjectPointer(0), - m_userIndex2(-1), - m_userIndex(-1), - m_hitFraction(btScalar(1.)), - m_ccdSweptSphereRadius(btScalar(0.)), - m_ccdMotionThreshold(btScalar(0.)), - m_checkCollideWith(false), - m_updateRevision(0) + : 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), + m_rootCollisionShape(0), + 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_userIndex3(-1), + m_hitFraction(btScalar(1.)), + m_ccdSweptSphereRadius(btScalar(0.)), + m_ccdMotionThreshold(btScalar(0.)), + m_checkCollideWith(false), + m_updateRevision(0) { m_worldTransform.setIdentity(); m_interpolationWorldTransform.setIdentity(); @@ -58,8 +59,8 @@ btCollisionObject::~btCollisionObject() } void btCollisionObject::setActivationState(int newState) const -{ - if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION)) +{ + if ((m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION)) m_activationState1 = newState; } @@ -70,7 +71,7 @@ void btCollisionObject::forceActivationState(int newState) const void btCollisionObject::activate(bool forceActivation) const { - if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT))) + if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT))) { setActivationState(ACTIVE_TAG); m_deactivationTime = btScalar(0.); @@ -79,7 +80,6 @@ void btCollisionObject::activate(bool forceActivation) const const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const { - btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer; m_worldTransform.serialize(dataOut->m_worldTransform); @@ -91,7 +91,7 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold; dataOut->m_broadphaseHandle = 0; dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape); - dataOut->m_rootCollisionShape = 0;//@todo + dataOut->m_rootCollisionShape = 0; //@todo dataOut->m_collisionFlags = m_collisionFlags; dataOut->m_islandTag1 = m_islandTag1; dataOut->m_companionId = m_companionId; @@ -103,8 +103,8 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_contactStiffness = m_contactStiffness; dataOut->m_restitution = m_restitution; dataOut->m_internalType = m_internalType; - - char* name = (char*) serializer->findNameForPointer(this); + + char* name = (char*)serializer->findNameForPointer(this); dataOut->m_name = (char*)serializer->getUniquePointer(name); if (dataOut->m_name) { @@ -114,18 +114,25 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius; dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; dataOut->m_checkCollideWith = m_checkCollideWith; - - // Fill padding with zeros to appease msan. - memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding)); - + 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; } - void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const { int len = calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this); + serializer->finalizeChunk(chunk, structType, BT_COLLISIONOBJECT_CODE, (void*)this); } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h index fec831bff..85dc488c8 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -25,8 +25,8 @@ subject to the following restrictions: #define DISABLE_DEACTIVATION 4 #define DISABLE_SIMULATION 5 -struct btBroadphaseProxy; -class btCollisionShape; +struct btBroadphaseProxy; +class btCollisionShape; struct btCollisionShapeData; #include "LinearMath/btMotionState.h" #include "LinearMath/btAlignedAllocator.h" @@ -42,123 +42,120 @@ typedef btAlignedObjectArray btCollisionObjectArray; #define btCollisionObjectDataName "btCollisionObjectFloatData" #endif - -/// btCollisionObject can be used to manage collision detection objects. +/// btCollisionObject can be used to manage collision detection objects. /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. /// They can be added to the btCollisionWorld. -ATTRIBUTE_ALIGNED16(class) btCollisionObject +ATTRIBUTE_ALIGNED16(class) +btCollisionObject { - protected: - - btTransform m_worldTransform; + btTransform m_worldTransform; ///m_interpolationWorldTransform is used for CCD and interpolation ///it can be either previous or future (predicted) transform - btTransform m_interpolationWorldTransform; - //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) + btTransform m_interpolationWorldTransform; + //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) //without destroying the continuous interpolated motion (which uses this interpolation velocities) - btVector3 m_interpolationLinearVelocity; - btVector3 m_interpolationAngularVelocity; - - btVector3 m_anisotropicFriction; - int m_hasAnisotropicFriction; - btScalar m_contactProcessingThreshold; + btVector3 m_interpolationLinearVelocity; + btVector3 m_interpolationAngularVelocity; - btBroadphaseProxy* m_broadphaseHandle; - btCollisionShape* m_collisionShape; + btVector3 m_anisotropicFriction; + int m_hasAnisotropicFriction; + btScalar m_contactProcessingThreshold; + + btBroadphaseProxy* m_broadphaseHandle; + btCollisionShape* m_collisionShape; ///m_extensionPointer is used by some internal low-level Bullet extensions. - void* m_extensionPointer; - + void* m_extensionPointer; + ///m_rootCollisionShape is temporarily used to store the original collision shape ///The m_collisionShape might be temporarily replaced by a child collision shape during collision detection purposes ///If it is NULL, the m_collisionShape is not temporarily replaced. - btCollisionShape* m_rootCollisionShape; + btCollisionShape* m_rootCollisionShape; - int m_collisionFlags; + int m_collisionFlags; - int m_islandTag1; - int m_companionId; - int m_worldArrayIndex; // index of object in world's collisionObjects array + 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; + mutable int m_activationState1; + mutable btScalar m_deactivationTime; - btScalar m_friction; - btScalar m_restitution; - 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; - - + btScalar m_friction; + btScalar m_restitution; + 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. - int m_internalType; + int m_internalType; ///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; + int m_userIndex2; + + int m_userIndex; + + int m_userIndex3; ///time of impact calculation - btScalar m_hitFraction; - + btScalar m_hitFraction; + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - btScalar m_ccdSweptSphereRadius; + btScalar m_ccdSweptSphereRadius; /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold - btScalar m_ccdMotionThreshold; - + btScalar m_ccdMotionThreshold; + /// If some object should have elaborate collision filtering by sub-classes - int m_checkCollideWith; + int m_checkCollideWith; btAlignedObjectArray m_objectsWithoutCollisionCheck; ///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation. - int m_updateRevision; + int m_updateRevision; - btVector3 m_customDebugColorRGB; + btVector3 m_customDebugColorRGB; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); enum CollisionFlags { - CF_STATIC_OBJECT= 1, - CF_KINEMATIC_OBJECT= 2, + CF_STATIC_OBJECT = 1, + CF_KINEMATIC_OBJECT = 2, CF_NO_CONTACT_RESPONSE = 4, - CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) + CF_CUSTOM_MATERIAL_CALLBACK = 8, //this allows per-triangle material (friction/restitution) CF_CHARACTER_OBJECT = 16, - CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing - CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing + CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing + 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 + enum CollisionObjectTypes { - CO_COLLISION_OBJECT =1, - CO_RIGID_BODY=2, + CO_COLLISION_OBJECT = 1, + CO_RIGID_BODY = 2, ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter ///It is useful for collision sensors, explosion objects, character controller etc. - CO_GHOST_OBJECT=4, - CO_SOFT_BODY=8, - CO_HF_FLUID=16, - CO_USER_TYPE=32, - CO_FEATHERSTONE_LINK=64 + CO_GHOST_OBJECT = 4, + CO_SOFT_BODY = 8, + CO_HF_FLUID = 16, + CO_USER_TYPE = 32, + CO_FEATHERSTONE_LINK = 64 }; enum AnisotropicFrictionFlags { - CF_ANISOTROPIC_FRICTION_DISABLED=0, + CF_ANISOTROPIC_FRICTION_DISABLED = 0, CF_ANISOTROPIC_FRICTION = 1, CF_ANISOTROPIC_ROLLING_FRICTION = 2 }; @@ -166,76 +163,77 @@ public: SIMD_FORCE_INLINE bool mergesSimulationIslands() const { ///static objects, kinematic and object without contact response don't merge islands - return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0); + return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE)) == 0); } const btVector3& getAnisotropicFriction() const { return m_anisotropicFriction; } - void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION) + void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION) { m_anisotropicFriction = anisotropicFriction; - bool isUnity = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f); - m_hasAnisotropicFriction = isUnity?frictionMode : 0; + bool isUnity = (anisotropicFriction[0] != 1.f) || (anisotropicFriction[1] != 1.f) || (anisotropicFriction[2] != 1.f); + m_hasAnisotropicFriction = isUnity ? frictionMode : 0; } - bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const + bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const { - return (m_hasAnisotropicFriction&frictionMode)!=0; + return (m_hasAnisotropicFriction & frictionMode) != 0; } ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default. ///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges - void setContactProcessingThreshold( btScalar contactProcessingThreshold) + void setContactProcessingThreshold(btScalar contactProcessingThreshold) { m_contactProcessingThreshold = contactProcessingThreshold; } - btScalar getContactProcessingThreshold() const + btScalar getContactProcessingThreshold() const { return m_contactProcessingThreshold; } - SIMD_FORCE_INLINE bool isStaticObject() const { + SIMD_FORCE_INLINE bool isStaticObject() const + { return (m_collisionFlags & CF_STATIC_OBJECT) != 0; } - SIMD_FORCE_INLINE bool isKinematicObject() const + SIMD_FORCE_INLINE bool isKinematicObject() const { return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0; } - SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const + SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const { - return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ; + return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0; } - SIMD_FORCE_INLINE bool hasContactResponse() const { - return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0; + SIMD_FORCE_INLINE bool hasContactResponse() const + { + return (m_collisionFlags & CF_NO_CONTACT_RESPONSE) == 0; } - btCollisionObject(); virtual ~btCollisionObject(); - virtual void setCollisionShape(btCollisionShape* collisionShape) + virtual void setCollisionShape(btCollisionShape * collisionShape) { m_updateRevision++; m_collisionShape = collisionShape; m_rootCollisionShape = collisionShape; } - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_collisionShape; } - SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() + SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { return m_collisionShape; } - void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck) + void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck) { if (ignoreCollisionCheck) { @@ -253,7 +251,7 @@ public: m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0; } - virtual bool checkCollideWithOverride(const btCollisionObject* co) const + virtual bool checkCollideWithOverride(const btCollisionObject* co) const { int index = m_objectsWithoutCollisionCheck.findLinearSearch(co); if (index < m_objectsWithoutCollisionCheck.size()) @@ -263,317 +261,319 @@ public: return true; } - - - - ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. + ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead. - void* internalGetExtensionPointer() const + void* internalGetExtensionPointer() const { return m_extensionPointer; } ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead. - void internalSetExtensionPointer(void* pointer) + void internalSetExtensionPointer(void* pointer) { m_extensionPointer = pointer; } - SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;} - + SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1; } + void setActivationState(int newState) const; - void setDeactivationTime(btScalar time) + void setDeactivationTime(btScalar time) { m_deactivationTime = time; } - btScalar getDeactivationTime() const + btScalar getDeactivationTime() const { return m_deactivationTime; } void forceActivationState(int newState) const; - void activate(bool forceActivation = false) const; + void activate(bool forceActivation = false) const; SIMD_FORCE_INLINE bool isActive() const { return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } - void setRestitution(btScalar rest) + void setRestitution(btScalar rest) { m_updateRevision++; m_restitution = rest; } - btScalar getRestitution() const + btScalar getRestitution() const { return m_restitution; } - void setFriction(btScalar frict) + void setFriction(btScalar frict) { m_updateRevision++; m_friction = frict; } - btScalar getFriction() const + btScalar getFriction() const { return m_friction; } - void setRollingFriction(btScalar frict) + void setRollingFriction(btScalar frict) { m_updateRevision++; m_rollingFriction = frict; } - btScalar getRollingFriction() const + btScalar getRollingFriction() const { return m_rollingFriction; } - void setSpinningFriction(btScalar frict) - { - m_updateRevision++; - m_spinningFriction = frict; - } - btScalar getSpinningFriction() const - { - return m_spinningFriction; - } - void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping) + 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; - } + + m_collisionFlags |= CF_HAS_CONTACT_STIFFNESS_DAMPING; + + //avoid divisions by zero... + if (m_contactStiffness < SIMD_EPSILON) + { + m_contactStiffness = SIMD_EPSILON; + } } - - btScalar getContactStiffness() const + + btScalar getContactStiffness() const { return m_contactStiffness; } - - btScalar getContactDamping() const + + btScalar getContactDamping() const { return m_contactDamping; } - + ///reserved for Bullet internal usage - int getInternalType() const + int getInternalType() const { return m_internalType; } - btTransform& getWorldTransform() + btTransform& getWorldTransform() { return m_worldTransform; } - const btTransform& getWorldTransform() const + const btTransform& getWorldTransform() const { return m_worldTransform; } - void setWorldTransform(const btTransform& worldTrans) + void setWorldTransform(const btTransform& worldTrans) { m_updateRevision++; m_worldTransform = worldTrans; } - - SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle() + SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle() { return m_broadphaseHandle; } - SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const + SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const { return m_broadphaseHandle; } - void setBroadphaseHandle(btBroadphaseProxy* handle) + void setBroadphaseHandle(btBroadphaseProxy * handle) { m_broadphaseHandle = handle; } - - const btTransform& getInterpolationWorldTransform() const + const btTransform& getInterpolationWorldTransform() const { return m_interpolationWorldTransform; } - btTransform& getInterpolationWorldTransform() + btTransform& getInterpolationWorldTransform() { return m_interpolationWorldTransform; } - void setInterpolationWorldTransform(const btTransform& trans) + void setInterpolationWorldTransform(const btTransform& trans) { m_updateRevision++; m_interpolationWorldTransform = trans; } - void setInterpolationLinearVelocity(const btVector3& linvel) + void setInterpolationLinearVelocity(const btVector3& linvel) { m_updateRevision++; m_interpolationLinearVelocity = linvel; } - void setInterpolationAngularVelocity(const btVector3& angvel) + void setInterpolationAngularVelocity(const btVector3& angvel) { m_updateRevision++; m_interpolationAngularVelocity = angvel; } - const btVector3& getInterpolationLinearVelocity() const + const btVector3& getInterpolationLinearVelocity() const { return m_interpolationLinearVelocity; } - const btVector3& getInterpolationAngularVelocity() const + const btVector3& getInterpolationAngularVelocity() const { return m_interpolationAngularVelocity; } SIMD_FORCE_INLINE int getIslandTag() const { - return m_islandTag1; + return m_islandTag1; } - void setIslandTag(int tag) + void setIslandTag(int tag) { m_islandTag1 = tag; } SIMD_FORCE_INLINE int getCompanionId() const { - return m_companionId; + return m_companionId; } - void setCompanionId(int id) + void setCompanionId(int id) { m_companionId = id; } - 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 + SIMD_FORCE_INLINE int getWorldArrayIndex() const { - return m_hitFraction; + return m_worldArrayIndex; } - void setHitFraction(btScalar hitFraction) + // only should be called by CollisionWorld + void setWorldArrayIndex(int ix) + { + m_worldArrayIndex = ix; + } + + SIMD_FORCE_INLINE btScalar getHitFraction() const + { + return m_hitFraction; + } + + void setHitFraction(btScalar hitFraction) { m_hitFraction = hitFraction; } - - SIMD_FORCE_INLINE int getCollisionFlags() const + SIMD_FORCE_INLINE int getCollisionFlags() const { return m_collisionFlags; } - void setCollisionFlags(int flags) + void setCollisionFlags(int flags) { m_collisionFlags = flags; } - + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - btScalar getCcdSweptSphereRadius() const + btScalar getCcdSweptSphereRadius() const { return m_ccdSweptSphereRadius; } ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - void setCcdSweptSphereRadius(btScalar radius) + void setCcdSweptSphereRadius(btScalar radius) { m_ccdSweptSphereRadius = radius; } - btScalar getCcdMotionThreshold() const + btScalar getCcdMotionThreshold() const { return m_ccdMotionThreshold; } - btScalar getCcdSquareMotionThreshold() const + btScalar getCcdSquareMotionThreshold() const { - return m_ccdMotionThreshold*m_ccdMotionThreshold; + return m_ccdMotionThreshold * m_ccdMotionThreshold; } - - /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold - void setCcdMotionThreshold(btScalar ccdMotionThreshold) + void setCcdMotionThreshold(btScalar ccdMotionThreshold) { m_ccdMotionThreshold = ccdMotionThreshold; } ///users can point to their objects, userPointer is not used by Bullet - void* getUserPointer() const + void* getUserPointer() const { return m_userObjectPointer; } - int getUserIndex() const + int getUserIndex() const { return m_userIndex; } - - int getUserIndex2() const + + int getUserIndex2() const { return m_userIndex2; } - + + int getUserIndex3() const + { + return m_userIndex3; + } + ///users can point to their objects, userPointer is not used by Bullet - void setUserPointer(void* userPointer) + void setUserPointer(void* userPointer) { m_userObjectPointer = userPointer; } ///users can point to their objects, userPointer is not used by Bullet - void setUserIndex(int index) + void setUserIndex(int index) { m_userIndex = index; } - - void setUserIndex2(int index) + + void setUserIndex2(int index) { m_userIndex2 = index; } - int getUpdateRevisionInternal() const + void setUserIndex3(int index) + { + m_userIndex3 = index; + } + + int getUpdateRevisionInternal() const { return m_updateRevision; } - void setCustomDebugColor(const btVector3& colorRGB) + void setCustomDebugColor(const btVector3& colorRGB) { m_customDebugColorRGB = colorRGB; m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; } - void removeCustomDebugColor() + void removeCustomDebugColor() { m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; } - bool getCustomDebugColor(btVector3& colorRGB) const + bool getCustomDebugColor(btVector3 & colorRGB) const { - bool hasCustomColor = (0!=(m_collisionFlags&CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); + bool hasCustomColor = (0 != (m_collisionFlags & CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); if (hasCustomColor) { colorRGB = m_customDebugColorRGB; @@ -589,15 +589,16 @@ public: return true; } - virtual int calculateSerializeBufferSize() const; + 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; - - virtual void serializeSingleObject(class btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + virtual void serializeSingleObject(class btSerializer * serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btCollisionObjectDoubleData { @@ -621,7 +622,6 @@ struct btCollisionObjectDoubleData double m_hitFraction; double m_ccdSweptSphereRadius; double m_ccdMotionThreshold; - int m_hasAnisotropicFriction; int m_collisionFlags; int m_islandTag1; @@ -629,8 +629,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 @@ -650,13 +651,12 @@ struct btCollisionObjectFloatData float m_deactivationTime; float m_friction; float m_rollingFriction; - float m_contactDamping; + 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; @@ -664,16 +664,15 @@ struct btCollisionObjectFloatData int m_activationState1; int m_internalType; int m_checkCollideWith; - char m_padding[4]; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId; }; +// clang-format on - - -SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const { return sizeof(btCollisionObjectData); } - - -#endif //BT_COLLISION_OBJECT_H +#endif //BT_COLLISION_OBJECT_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h index 952440b7d..1cc4a5ac5 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h @@ -1,25 +1,25 @@ #ifndef BT_COLLISION_OBJECT_WRAPPER_H #define BT_COLLISION_OBJECT_WRAPPER_H -///btCollisionObjectWrapperis an internal data structure. +///btCollisionObjectWrapperis an internal data structure. ///Most users can ignore this and use btCollisionObject and btCollisionShape instead class btCollisionShape; class btCollisionObject; class btTransform; -#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition +#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition #define BT_DECLARE_STACK_ONLY_OBJECT \ - private: \ - void* operator new(size_t size); \ - void operator delete(void*); +private: \ + void* operator new(size_t size); \ + void operator delete(void*); struct btCollisionObjectWrapper; struct btCollisionObjectWrapper { -BT_DECLARE_STACK_ONLY_OBJECT + BT_DECLARE_STACK_ONLY_OBJECT private: - btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed. + btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed. btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&); public: @@ -27,17 +27,17 @@ public: const btCollisionShape* m_shape; const btCollisionObject* m_collisionObject; const btTransform& m_worldTransform; - int m_partId; - int m_index; + int m_partId; + int m_index; btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index) - : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), - m_partId(partId), m_index(index) - {} + : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_partId(partId), m_index(index) + { + } SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; } SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; } SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; } }; -#endif //BT_COLLISION_OBJECT_WRAPPER_H +#endif //BT_COLLISION_OBJECT_WRAPPER_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index c893b60d3..71184f36a 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -19,8 +19,10 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" //for raycasting #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" @@ -37,7 +39,6 @@ subject to the following restrictions: //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION - //#define USE_BRUTEFORCE_RAYBROADPHASE 1 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest //#define RECALCULATE_AABB_RAYCAST 1 @@ -47,7 +48,6 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" - ///for debug drawing //for debug rendering @@ -64,25 +64,21 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" - - -btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) -:m_dispatcher1(dispatcher), -m_broadphasePairCache(pairCache), -m_debugDrawer(0), -m_forceUpdateAllAabbs(true) +btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) + : m_dispatcher1(dispatcher), + m_broadphasePairCache(pairCache), + m_debugDrawer(0), + m_forceUpdateAllAabbs(true) { } - btCollisionWorld::~btCollisionWorld() { - //clean up remaining objects int i; - for (i=0;igetBroadphaseHandle(); if (bp) @@ -90,74 +86,83 @@ btCollisionWorld::~btCollisionWorld() // // only clear the cached algorithms // - getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); - getBroadphase()->destroyProxy(bp,m_dispatcher1); + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp, m_dispatcher1); + getBroadphase()->destroyProxy(bp, m_dispatcher1); collisionObject->setBroadphaseHandle(0); } } - - } - - - - - - - - - -void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) +void btCollisionWorld::refreshBroadphaseProxy(btCollisionObject* collisionObject) { + if (collisionObject->getBroadphaseHandle()) + { + int collisionFilterGroup = collisionObject->getBroadphaseHandle()->m_collisionFilterGroup; + int collisionFilterMask = collisionObject->getBroadphaseHandle()->m_collisionFilterMask; + getBroadphase()->destroyProxy(collisionObject->getBroadphaseHandle(), getDispatcher()); + + //calculate new AABB + btTransform trans = collisionObject->getWorldTransform(); + + btVector3 minAabb; + btVector3 maxAabb; + collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb); + + int type = collisionObject->getCollisionShape()->getShapeType(); + collisionObject->setBroadphaseHandle(getBroadphase()->createProxy( + minAabb, + maxAabb, + type, + collisionObject, + collisionFilterGroup, + collisionFilterMask, + m_dispatcher1)); + } +} + +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 + 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()); + collisionObject->setWorldArrayIndex(m_collisionObjects.size()); m_collisionObjects.push_back(collisionObject); //calculate new AABB btTransform trans = collisionObject->getWorldTransform(); - btVector3 minAabb; - btVector3 maxAabb; - collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb); + btVector3 minAabb; + btVector3 maxAabb; + collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb); int type = collisionObject->getCollisionShape()->getShapeType(); - collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( + collisionObject->setBroadphaseHandle(getBroadphase()->createProxy( minAabb, maxAabb, type, collisionObject, collisionFilterGroup, collisionFilterMask, - m_dispatcher1)) ; - - - - - + m_dispatcher1)); } - - -void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) +void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) { - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btVector3 minAabb, maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb); //need to increase the aabb for contact thresholds - btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + btVector3 contactThreshold(gContactBreakingThreshold, gContactBreakingThreshold, gContactBreakingThreshold); minAabb -= contactThreshold; maxAabb += contactThreshold; - if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) + if (getDispatchInfo().m_useContinuous && colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) { - btVector3 minAabb2,maxAabb2; - colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + btVector3 minAabb2, maxAabb2; + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(), minAabb2, maxAabb2); minAabb2 -= contactThreshold; maxAabb2 += contactThreshold; minAabb.setMin(minAabb2); @@ -167,10 +172,11 @@ void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) + if (colObj->isStaticObject() || ((maxAabb - minAabb).length2() < btScalar(1e12))) { - bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } else + bp->setAabb(colObj->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1); + } + else { //something went wrong, investigate //this assert is unwanted in 3D modelers (danger of loosing work) @@ -188,15 +194,15 @@ void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) } } -void btCollisionWorld::updateAabbs() +void btCollisionWorld::updateAabbs() { BT_PROFILE("updateAabbs"); btTransform predictedTrans; - for ( int i=0;igetWorldArrayIndex() == i); + btAssert(colObj->getWorldArrayIndex() == i); //only update aabb of active objects if (m_forceUpdateAllAabbs || colObj->isActive()) @@ -206,14 +212,13 @@ void btCollisionWorld::updateAabbs() } } - -void btCollisionWorld::computeOverlappingPairs() +void btCollisionWorld::computeOverlappingPairs() { BT_PROFILE("calculateOverlappingPairs"); m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); } -void btCollisionWorld::performDiscreteCollisionDetection() +void btCollisionWorld::performDiscreteCollisionDetection() { BT_PROFILE("performDiscreteCollisionDetection"); @@ -227,69 +232,61 @@ void btCollisionWorld::performDiscreteCollisionDetection() { BT_PROFILE("dispatchAllCollisionPairs"); if (dispatcher) - dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(), dispatchInfo, m_dispatcher1); } - } - - -void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) { - - //bool removeFromBroadphase = false; { - btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) { // // only clear the cached algorithms // - getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); - getBroadphase()->destroyProxy(bp,m_dispatcher1); + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp, m_dispatcher1); + getBroadphase()->destroyProxy(bp, m_dispatcher1); collisionObject->setBroadphaseHandle(0); } } - - 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); + 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); } - -void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) +void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) { - btCollisionObjectWrapper colObWrap(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1); - btCollisionWorld::rayTestSingleInternal(rayFromTrans,rayToTrans,&colObWrap,resultCallback); + btCollisionObjectWrapper colObWrap(0, collisionShape, collisionObject, colObjWorldTransform, -1, -1); + btCollisionWorld::rayTestSingleInternal(rayFromTrans, rayToTrans, &colObWrap, resultCallback); } -void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans, - const btCollisionObjectWrapper* collisionObjectWrap, - RayResultCallback& resultCallback) +void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, const btTransform& rayToTrans, + const btCollisionObjectWrapper* collisionObjectWrap, + RayResultCallback& resultCallback) { btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); @@ -303,12 +300,12 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con btConvexCast::CastResult castResult; castResult.m_fraction = resultCallback.m_closestHitFraction; - btConvexShape* convexShape = (btConvexShape*) collisionShape; - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast subSimplexConvexCaster(castShape,convexShape,&simplexSolver); - - btGjkConvexCast gjkConvexCaster(castShape,convexShape,&simplexSolver); - + btConvexShape* convexShape = (btConvexShape*)collisionShape; + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast subSimplexConvexCaster(castShape, convexShape, &simplexSolver); + + btGjkConvexCast gjkConvexCaster(castShape, convexShape, &simplexSolver); + //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); btConvexCast* convexCasterPtr = 0; @@ -317,10 +314,10 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con convexCasterPtr = &gjkConvexCaster; else convexCasterPtr = &subSimplexConvexCaster; - + btConvexCast& convexCaster = *convexCasterPtr; - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (convexCaster.calcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) @@ -331,81 +328,105 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con #ifdef USE_SUBSIMPLEX_CONVEX_CAST //rotate normal into worldspace castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; -#endif //USE_SUBSIMPLEX_CONVEX_CAST +#endif //USE_SUBSIMPLEX_CONVEX_CAST castResult.m_normal.normalize(); - btCollisionWorld::LocalRayResult localRayResult - ( + btCollisionWorld::LocalRayResult localRayResult( collisionObjectWrap->getCollisionObject(), 0, castResult.m_normal, - castResult.m_fraction - ); + castResult.m_fraction); bool normalInWorldSpace = true; resultCallback.addSingleResult(localRayResult, normalInWorldSpace); - } } } - } else { + } + else + { if (collisionShape->isConcave()) { - //ConvexCast::CastResult - struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + const btCollisionObject* m_collisionObject; + const btConcaveShape* m_triangleMesh; + + btTransform m_colObjWorldTransform; + + BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, const btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) : //@BP Mod + btTriangleRaycastCallback(from, to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) { - btCollisionWorld::RayResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - const btConcaveShape* m_triangleMesh; + } - btTransform m_colObjWorldTransform; + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; - BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape* triangleMesh,const btTransform& colObjWorldTransform): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh), - m_colObjWorldTransform(colObjWorldTransform) - { - } + btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; + btCollisionWorld::LocalRayResult rayResult(m_collisionObject, + &shapeInfo, + hitNormalWorld, + hitFraction); - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - - btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; - - btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, - &shapeInfo, - hitNormalWorld, - hitFraction); - - bool normalInWorldSpace = true; - return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); - } - - }; + bool normalInWorldSpace = true; + return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace); + } + }; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); // BT_PROFILE("rayTestConcave"); - if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + if (collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { ///optimized version for btBvhTriangleMeshShape btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform); + + BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; - triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); + 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 if (((resultCallback.m_flags&btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator)==0) + && collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE + ) + { + ///optimized version for btHeightfieldTerrainShape + btHeightfieldTerrainShape* heightField = (btHeightfieldTerrainShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), heightField, colObjWorldTransform); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + heightField->performRaycast(&rcb, rayFromLocal, rayToLocal); } else { @@ -422,45 +443,40 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback { btCollisionWorld::RayResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - btConcaveShape* m_triangleMesh; + const btCollisionObject* m_collisionObject; + btConcaveShape* m_triangleMesh; btTransform m_colObjWorldTransform; - BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh), - m_colObjWorldTransform(colObjWorldTransform) + BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) : //@BP Mod + btTriangleRaycastCallback(from, to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) { } - - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; - btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, - &shapeInfo, - hitNormalWorld, - hitFraction); + btCollisionWorld::LocalRayResult rayResult(m_collisionObject, + &shapeInfo, + hitNormalWorld, + hitFraction); - bool normalInWorldSpace = true; - return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); + bool normalInWorldSpace = true; + return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace); } - }; - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform); + BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), concaveShape, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; @@ -468,9 +484,11 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con btVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax(rayToLocal); - concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&rcb, rayAabbMinLocal, rayAabbMaxLocal); } - } else { + } + else + { // BT_PROFILE("rayTestCompound"); if (collisionShape->isCompound()) { @@ -478,10 +496,10 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con { RayResultCallback* m_userCallback; int m_i; - - LocalInfoAdder2 (int i, RayResultCallback *user) + + LocalInfoAdder2(int i, RayResultCallback* user) : m_userCallback(user), m_i(i) - { + { m_closestHitFraction = m_userCallback->m_closestHitFraction; m_flags = m_userCallback->m_flags; } @@ -490,7 +508,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con return m_userCallback->needsCollision(p); } - virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b) + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& r, bool b) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = -1; @@ -503,7 +521,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con return result; } }; - + struct RayTester : btDbvt::ICollide { const btCollisionObject* m_collisionObject; @@ -512,33 +530,29 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con const btTransform& m_rayFromTrans; const btTransform& m_rayToTrans; RayResultCallback& m_resultCallback; - + RayTester(const btCollisionObject* collisionObject, - const btCompoundShape* compoundShape, - const btTransform& colObjWorldTransform, - const btTransform& rayFromTrans, - const btTransform& rayToTrans, - RayResultCallback& resultCallback): - m_collisionObject(collisionObject), - m_compoundShape(compoundShape), - m_colObjWorldTransform(colObjWorldTransform), - m_rayFromTrans(rayFromTrans), - m_rayToTrans(rayToTrans), - m_resultCallback(resultCallback) + const btCompoundShape* compoundShape, + const btTransform& colObjWorldTransform, + const btTransform& rayFromTrans, + const btTransform& rayToTrans, + RayResultCallback& resultCallback) : m_collisionObject(collisionObject), + m_compoundShape(compoundShape), + m_colObjWorldTransform(colObjWorldTransform), + m_rayFromTrans(rayFromTrans), + m_rayToTrans(rayToTrans), + m_resultCallback(resultCallback) { - } - + void ProcessLeaf(int i) { const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i); const btTransform& childTrans = m_compoundShape->getChildTransform(i); btTransform childWorldTrans = m_colObjWorldTransform * childTrans; - - btCollisionObjectWrapper tmpOb(0,childCollisionShape,m_collisionObject,childWorldTrans,-1,i); - // replace collision shape so that callback can determine the triangle - + btCollisionObjectWrapper tmpOb(0, childCollisionShape, m_collisionObject, childWorldTrans, -1, i); + // replace collision shape so that callback can determine the triangle LocalInfoAdder2 my_cb(i, &m_resultCallback); @@ -547,19 +561,17 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con m_rayToTrans, &tmpOb, my_cb); - } - + void Process(const btDbvtNode* leaf) { ProcessLeaf(leaf->dataAsInt); } }; - + const btCompoundShape* compoundShape = static_cast(collisionShape); const btDbvt* dbvt = compoundShape->getDynamicAabbTree(); - RayTester rayCB( collisionObjectWrap->getCollisionObject(), compoundShape, @@ -567,39 +579,39 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con rayFromTrans, rayToTrans, resultCallback); -#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION +#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION if (dbvt) { btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin(); btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin(); - btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB); + btDbvt::rayTest(dbvt->m_root, localRayFrom, localRayTo, rayCB); } else -#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION +#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION { for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i) { rayCB.ProcessLeaf(i); - } + } } } } } } -void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - ConvexResultCallback& resultCallback, btScalar allowedPenetration) +void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback, btScalar allowedPenetration) { - btCollisionObjectWrapper tmpOb(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1); - btCollisionWorld::objectQuerySingleInternal(castShape,convexFromTrans,convexToTrans,&tmpOb,resultCallback,allowedPenetration); + btCollisionObjectWrapper tmpOb(0, collisionShape, collisionObject, colObjWorldTransform, -1, -1); + btCollisionWorld::objectQuerySingleInternal(castShape, convexFromTrans, convexToTrans, &tmpOb, resultCallback, allowedPenetration); } -void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - const btCollisionObjectWrapper* colObjWrap, - ConvexResultCallback& resultCallback, btScalar allowedPenetration) +void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, + const btCollisionObjectWrapper* colObjWrap, + ConvexResultCallback& resultCallback, btScalar allowedPenetration) { const btCollisionShape* collisionShape = colObjWrap->getCollisionShape(); const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform(); @@ -609,21 +621,19 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, //BT_PROFILE("convexSweepConvex"); btConvexCast::CastResult castResult; castResult.m_allowedPenetration = allowedPenetration; - castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//?? + castResult.m_fraction = resultCallback.m_closestHitFraction; //btScalar(1.);//?? - btConvexShape* convexShape = (btConvexShape*) collisionShape; - btVoronoiSimplexSolver simplexSolver; - btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; + btConvexShape* convexShape = (btConvexShape*)collisionShape; + btVoronoiSimplexSolver simplexSolver; + btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; - btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver); + btContinuousConvexCollision convexCaster1(castShape, convexShape, &simplexSolver, &gjkEpaPenetrationSolver); //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver); //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver); btConvexCast* castPtr = &convexCaster1; - - - if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (castPtr->calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) @@ -631,25 +641,24 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, if (castResult.m_fraction < resultCallback.m_closestHitFraction) { castResult.m_normal.normalize(); - btCollisionWorld::LocalConvexResult localConvexResult - ( + btCollisionWorld::LocalConvexResult localConvexResult( colObjWrap->getCollisionObject(), 0, castResult.m_normal, castResult.m_hitPoint, - castResult.m_fraction - ); + castResult.m_fraction); bool normalInWorldSpace = true; resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); - } } } - } else { + } + else + { if (collisionShape->isConcave()) { - if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + if (collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { //BT_PROFILE("convexSweepbtBvhTriangleMesh"); btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; @@ -663,62 +672,57 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback { btCollisionWorld::ConvexResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; + const btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; - BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, - btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from, const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject, btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld) : btTriangleConvexcastCallback(castShape, from, to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) { } - - virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; if (hitFraction <= m_resultCallback->m_closestHitFraction) { + btCollisionWorld::LocalConvexResult convexResult(m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); + bool normalInWorldSpace = true; - bool normalInWorldSpace = true; - - - return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + return m_resultCallback->addSingleResult(convexResult, normalInWorldSpace); } return hitFraction; } - }; - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),triangleMesh, colObjWorldTransform); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans, convexToTrans, &resultCallback, colObjWrap->getCollisionObject(), triangleMesh, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); - triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); - } else + triangleMesh->performConvexcast(&tccb, convexFromLocal, convexToLocal, boxMinLocal, boxMaxLocal); + } + else { - if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE) + if (collisionShape->getShapeType() == STATIC_PLANE_PROXYTYPE) { btConvexCast::CastResult castResult; castResult.m_allowedPenetration = allowedPenetration; castResult.m_fraction = resultCallback.m_closestHitFraction; - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape; - btContinuousConvexCollision convexCaster1(castShape,planeShape); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*)collisionShape; + btContinuousConvexCollision convexCaster1(castShape, planeShape); btConvexCast* castPtr = &convexCaster1; - if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (castPtr->calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) @@ -726,22 +730,20 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, if (castResult.m_fraction < resultCallback.m_closestHitFraction) { castResult.m_normal.normalize(); - btCollisionWorld::LocalConvexResult localConvexResult - ( + btCollisionWorld::LocalConvexResult localConvexResult( colObjWrap->getCollisionObject(), 0, castResult.m_normal, castResult.m_hitPoint, - castResult.m_fraction - ); + castResult.m_fraction); bool normalInWorldSpace = true; resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); } } } - - } else + } + else { //BT_PROFILE("convexSweepConcave"); btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; @@ -755,44 +757,39 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback { btCollisionWorld::ConvexResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - btConcaveShape* m_triangleMesh; + const btCollisionObject* m_collisionObject; + btConcaveShape* m_triangleMesh; - BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, - btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from, const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& triangleToWorld) : btTriangleConvexcastCallback(castShape, from, to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) { } - - virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; if (hitFraction <= m_resultCallback->m_closestHitFraction) { + btCollisionWorld::LocalConvexResult convexResult(m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); + bool normalInWorldSpace = true; - bool normalInWorldSpace = true; - - return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + return m_resultCallback->addSingleResult(convexResult, normalInWorldSpace); } return hitFraction; } - }; - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),concaveShape, colObjWorldTransform); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans, convexToTrans, &resultCallback, colObjWrap->getCollisionObject(), concaveShape, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; @@ -804,35 +801,37 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, rayAabbMaxLocal.setMax(convexToLocal); rayAabbMinLocal += boxMinLocal; rayAabbMaxLocal += boxMaxLocal; - concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&tccb, rayAabbMinLocal, rayAabbMaxLocal); } } - } else { + } + else + { if (collisionShape->isCompound()) { - struct btCompoundLeafCallback : btDbvt::ICollide + struct btCompoundLeafCallback : btDbvt::ICollide { 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* 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 btCollisionObjectWrapper* m_colObjWrap; const btConvexShape* m_castShape; const btTransform& m_convexFromTrans; const btTransform& m_convexToTrans; @@ -842,16 +841,16 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, ConvexResultCallback& m_resultCallback; public: - - void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape) + void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape) { btTransform childWorldTrans = m_colObjWorldTransform * childTrans; - struct LocalInfoAdder : public ConvexResultCallback { + 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; @@ -860,9 +859,9 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, { return m_userCallback->needsCollision(p); } - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& r, bool b) + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& r, bool b) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = -1; shapeInfo.m_triangleIndex = m_i; if (r.m_localShapeInfo == NULL) @@ -870,7 +869,6 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, const btScalar result = m_userCallback->addSingleResult(r, b); m_closestHitFraction = m_userCallback->m_closestHitFraction; return result; - } }; @@ -881,7 +879,7 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration); } - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { // Processing leaf node int index = leaf->dataAsInt; @@ -906,15 +904,18 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, fromLocalAabbMax.setMax(toLocalAabbMax); btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans, - allowedPenetration, compoundShape, colObjWorldTransform, resultCallback); + allowedPenetration, compoundShape, colObjWorldTransform, resultCallback); const btDbvt* tree = compoundShape->getDynamicAabbTree(); - if (tree) { - const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax); + if (tree) + { + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax); tree->collideTV(tree->m_root, bounds, callback); - } else { + } + else + { int i; - for (i=0;igetNumChildShapes();i++) + for (i = 0; i < compoundShape->getNumChildShapes(); i++) { const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); btTransform childTrans = compoundShape->getChildTransform(i); @@ -926,33 +927,31 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, } } - struct btSingleRayCallback : public btBroadphaseRayCallback { + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; + const btCollisionWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; - const btCollisionWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; - - btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) + btSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btCollisionWorld* world, btCollisionWorld::RayResultCallback& resultCallback) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) { m_rayFromTrans.setIdentity(); m_rayFromTrans.setOrigin(m_rayFromWorld); m_rayToTrans.setIdentity(); m_rayToTrans.setOrigin(m_rayToWorld); - btVector3 rayDir = (rayToWorld-rayFromWorld); + btVector3 rayDir = (rayToWorld - rayFromWorld); - rayDir.normalize (); + rayDir.normalize(); ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; @@ -961,22 +960,19 @@ struct btSingleRayCallback : public btBroadphaseRayCallback 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); - + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); } - - - virtual bool process(const btBroadphaseProxy* proxy) + 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; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; @@ -994,57 +990,53 @@ struct btSingleRayCallback : public btBroadphaseRayCallback //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); + m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); } } return true; } }; -void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const +void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const { //BT_PROFILE("rayTest"); /// use the broadphase to accelerate the search for objects, based on their aabb /// and for each object with ray-aabb overlap, perform an exact ray test - btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); + btSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); #ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); #else - for (int i=0;igetNumCollisionObjects();i++) + for (int i = 0; i < this->getNumCollisionObjects(); i++) { rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE - + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE } - struct btSingleSweepCallback : public btBroadphaseRayCallback { - - btTransform m_convexFromTrans; - btTransform m_convexToTrans; - btVector3 m_hitNormal; - const btCollisionWorld* m_world; - btCollisionWorld::ConvexResultCallback& m_resultCallback; - btScalar m_allowedCcdPenetration; + btTransform m_convexFromTrans; + btTransform m_convexToTrans; + btVector3 m_hitNormal; + const btCollisionWorld* m_world; + btCollisionWorld::ConvexResultCallback& m_resultCallback; + btScalar m_allowedCcdPenetration; const btConvexShape* m_castShape; - - btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration) - :m_convexFromTrans(convexFromTrans), - m_convexToTrans(convexToTrans), - m_world(world), - m_resultCallback(resultCallback), - m_allowedCcdPenetration(allowedPenetration), - m_castShape(castShape) + btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, const btCollisionWorld* world, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedPenetration) + : m_convexFromTrans(convexFromTrans), + m_convexToTrans(convexToTrans), + m_world(world), + m_resultCallback(resultCallback), + m_allowedCcdPenetration(allowedPenetration), + m_castShape(castShape) { - btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin()); + btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin() - m_convexFromTrans.getOrigin()); btVector3 rayDir = unnormalizedRayDir.normalized(); ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; @@ -1055,109 +1047,102 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback m_signs[2] = m_rayDirectionInverse[2] < 0.0; m_lambda_max = rayDir.dot(unnormalizedRayDir); - } - virtual bool process(const btBroadphaseProxy* proxy) + virtual bool process(const btBroadphaseProxy* proxy) { ///terminate further convex sweep tests, once the closestHitFraction reached zero if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) return false; - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback, - m_allowedCcdPenetration); + m_world->objectQuerySingle(m_castShape, m_convexFromTrans, m_convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback, + m_allowedCcdPenetration); } return true; } }; - - -void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const { - BT_PROFILE("convexSweepTest"); /// use the broadphase to accelerate the search for objects, based on their aabb /// and for each object with ray-aabb overlap, perform an exact ray test /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical - - - btTransform convexFromTrans,convexToTrans; + btTransform convexFromTrans, convexToTrans; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; btVector3 castShapeAabbMin, castShapeAabbMax; /* Compute AABB that encompasses angular movement */ { btVector3 linVel, angVel; - btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0f, linVel, angVel); + btTransformUtil::calculateVelocity(convexFromTrans, convexToTrans, 1.0f, linVel, angVel); btVector3 zeroLinVel; - zeroLinVel.setValue(0,0,0); + zeroLinVel.setValue(0, 0, 0); btTransform R; - R.setIdentity (); - R.setRotation (convexFromTrans.getRotation()); - castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax); + R.setIdentity(); + R.setRotation(convexFromTrans.getRotation()); + castShape->calculateTemporalAabb(R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax); } #ifndef USE_BRUTEFORCE_RAYBROADPHASE - btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration); + btSingleSweepCallback convexCB(castShape, convexFromWorld, convexToWorld, this, resultCallback, allowedCcdPenetration); - m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax); + m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(), convexToTrans.getOrigin(), convexCB, castShapeAabbMin, castShapeAabbMax); #else /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) int i; - for (i=0;igetBroadphaseHandle())) { + if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); - btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 collisionObjectAabbMin, collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax); + AabbExpand(collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; - if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + if (btRayAabb(convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) { - objectQuerySingle(castShape, convexFromTrans,convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); + objectQuerySingle(castShape, convexFromTrans, convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); } } } -#endif //USE_BRUTEFORCE_RAYBROADPHASE +#endif //USE_BRUTEFORCE_RAYBROADPHASE } - - struct btBridgedManifoldResult : public btManifoldResult { + btCollisionWorld::ContactResultCallback& m_resultCallback; - btCollisionWorld::ContactResultCallback& m_resultCallback; - - btBridgedManifoldResult( const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap,btCollisionWorld::ContactResultCallback& resultCallback ) - :btManifoldResult(obj0Wrap,obj1Wrap), - m_resultCallback(resultCallback) + btBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap, btCollisionWorld::ContactResultCallback& resultCallback) + : btManifoldResult(obj0Wrap, obj1Wrap), + m_resultCallback(resultCallback) { } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); btVector3 pointA = pointInWorld + normalOnBInWorld * depth; @@ -1165,78 +1150,74 @@ struct btBridgedManifoldResult : public btManifoldResult btVector3 localB; if (isSwapped) { - localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } else + } + else { - localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); } - - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + + btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth); newPt.m_positionWorldOnA = pointA; newPt.m_positionWorldOnB = pointInWorld; - - //BP mod, store contact triangles. + + //BP mod, store contact triangles. if (isSwapped) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; - newPt.m_index0 = m_index1; - newPt.m_index1 = m_index0; - } else + newPt.m_index0 = m_index1; + newPt.m_index1 = m_index0; + } + else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; - newPt.m_index0 = m_index0; - newPt.m_index1 = m_index1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; } //experimental feature info, for per-triangle material etc. - const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap; - const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap; - m_resultCallback.addSingleResult(newPt,obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1); - + const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap; + m_resultCallback.addSingleResult(newPt, obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1); } - }; - - struct btSingleContactCallback : public btBroadphaseAabbCallback { - btCollisionObject* m_collisionObject; - btCollisionWorld* m_world; - btCollisionWorld::ContactResultCallback& m_resultCallback; - - - btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback) - :m_collisionObject(collisionObject), - m_world(world), - m_resultCallback(resultCallback) + btCollisionWorld* m_world; + btCollisionWorld::ContactResultCallback& m_resultCallback; + + btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world, btCollisionWorld::ContactResultCallback& resultCallback) + : m_collisionObject(collisionObject), + m_world(world), + m_resultCallback(resultCallback) { } - virtual bool process(const btBroadphaseProxy* proxy) + virtual bool process(const btBroadphaseProxy* proxy) { - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; if (collisionObject == m_collisionObject) return true; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { - btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1); - btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1); + 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,0, BT_CLOSEST_POINT_ALGORITHMS); + btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0, &ob1, 0, BT_CLOSEST_POINT_ALGORITHMS); if (algorithm) { - btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback); + btBridgedManifoldResult contactPointResult(&ob0, &ob1, m_resultCallback); //discrete collision detection query - - algorithm->processCollision(&ob0,&ob1, m_world->getDispatchInfo(),&contactPointResult); + + algorithm->processCollision(&ob0, &ob1, m_world->getDispatchInfo(), &contactPointResult); algorithm->~btCollisionAlgorithm(); m_world->getDispatcher()->freeCollisionAlgorithm(algorithm); @@ -1246,271 +1227,247 @@ struct btSingleContactCallback : public btBroadphaseAabbCallback } }; - ///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback. ///it reports one or more contact points for every overlapping object (including the one with deepest penetration) -void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback) +void btCollisionWorld::contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback) { - btVector3 aabbMin,aabbMax; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax); - btSingleContactCallback contactCB(colObj,this,resultCallback); - - m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB); -} + btVector3 aabbMin, aabbMax; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), aabbMin, aabbMax); + btSingleContactCallback contactCB(colObj, this, resultCallback); + m_broadphasePairCache->aabbTest(aabbMin, aabbMax, contactCB); +} ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected. ///it reports one or more contact points (including the one with deepest penetration) -void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback) +void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback) { - btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1); - btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1); + 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, 0, BT_CLOSEST_POINT_ALGORITHMS); + btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA, &obB, 0, BT_CLOSEST_POINT_ALGORITHMS); if (algorithm) { - btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback); + btBridgedManifoldResult contactPointResult(&obA, &obB, resultCallback); contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold; //discrete collision detection query - algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult); + algorithm->processCollision(&obA, &obB, getDispatchInfo(), &contactPointResult); algorithm->~btCollisionAlgorithm(); getDispatcher()->freeCollisionAlgorithm(algorithm); } - } - - - class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback { - btIDebugDraw* m_debugDrawer; - btVector3 m_color; - btTransform m_worldTrans; + btIDebugDraw* m_debugDrawer; + btVector3 m_color; + btTransform m_worldTrans; public: + DebugDrawcallback(btIDebugDraw* debugDrawer, const btTransform& worldTrans, const btVector3& color) : m_debugDrawer(debugDrawer), + m_color(color), + m_worldTrans(worldTrans) + { + } - DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : - m_debugDrawer(debugDrawer), - m_color(color), - m_worldTrans(worldTrans) - { - } + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + processTriangle(triangle, partId, triangleIndex); + } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - processTriangle(triangle,partId,triangleIndex); - } + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + (void)partId; + (void)triangleIndex; - virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; + btVector3 wv0, wv1, wv2; + wv0 = m_worldTrans * triangle[0]; + wv1 = m_worldTrans * triangle[1]; + wv2 = m_worldTrans * triangle[2]; + btVector3 center = (wv0 + wv1 + wv2) * btScalar(1. / 3.); - btVector3 wv0,wv1,wv2; - wv0 = m_worldTrans*triangle[0]; - wv1 = m_worldTrans*triangle[1]; - wv2 = m_worldTrans*triangle[2]; - btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.); - - if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals ) - { - btVector3 normal = (wv1-wv0).cross(wv2-wv0); - normal.normalize(); - btVector3 normalColor(1,1,0); - m_debugDrawer->drawLine(center,center+normal,normalColor); - } - m_debugDrawer->drawLine(wv0,wv1,m_color); - m_debugDrawer->drawLine(wv1,wv2,m_color); - m_debugDrawer->drawLine(wv2,wv0,m_color); - } + if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals) + { + btVector3 normal = (wv1 - wv0).cross(wv2 - wv0); + normal.normalize(); + btVector3 normalColor(1, 1, 0); + m_debugDrawer->drawLine(center, center + normal, normalColor); + } + m_debugDrawer->drawLine(wv0, wv1, m_color); + m_debugDrawer->drawLine(wv1, wv2, m_color); + m_debugDrawer->drawLine(wv2, wv0, m_color); + } }; - void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) { // 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) { const btCompoundShape* compoundShape = static_cast(shape); - for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) + for (int i = compoundShape->getNumChildShapes() - 1; i >= 0; i--) { btTransform childTrans = compoundShape->getChildTransform(i); const btCollisionShape* colShape = compoundShape->getChildShape(i); - debugDrawObject(worldTransform*childTrans,colShape,color); + debugDrawObject(worldTransform * childTrans, colShape, color); } - - } else + } + else { + switch (shape->getShapeType()) + { + case BOX_SHAPE_PROXYTYPE: + { + const btBoxShape* boxShape = static_cast(shape); + btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); + getDebugDrawer()->drawBox(-halfExtents, halfExtents, worldTransform, color); + break; + } - switch (shape->getShapeType()) - { + case SPHERE_SHAPE_PROXYTYPE: + { + const btSphereShape* sphereShape = static_cast(shape); + btScalar radius = sphereShape->getMargin(); //radius doesn't include the margin, so draw with margin - case BOX_SHAPE_PROXYTYPE: - { - const btBoxShape* boxShape = static_cast(shape); - btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); - getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color); - break; - } + getDebugDrawer()->drawSphere(radius, worldTransform, color); + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + const btMultiSphereShape* multiSphereShape = static_cast(shape); - case SPHERE_SHAPE_PROXYTYPE: - { - const btSphereShape* sphereShape = static_cast(shape); - btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin + btTransform childTransform; + childTransform.setIdentity(); - getDebugDrawer()->drawSphere(radius, worldTransform, color); - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: - { - const btMultiSphereShape* multiSphereShape = static_cast(shape); + for (int i = multiSphereShape->getSphereCount() - 1; i >= 0; i--) + { + childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); + getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform * childTransform, color); + } - btTransform childTransform; - childTransform.setIdentity(); + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + const btCapsuleShape* capsuleShape = static_cast(shape); - for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) - { - childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); - getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color); - } + btScalar radius = capsuleShape->getRadius(); + btScalar halfHeight = capsuleShape->getHalfHeight(); - break; - } - case CAPSULE_SHAPE_PROXYTYPE: - { - const btCapsuleShape* capsuleShape = static_cast(shape); + int upAxis = capsuleShape->getUpAxis(); + getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); + break; + } + case CONE_SHAPE_PROXYTYPE: + { + const btConeShape* coneShape = static_cast(shape); + btScalar radius = coneShape->getRadius(); //+coneShape->getMargin(); + btScalar height = coneShape->getHeight(); //+coneShape->getMargin(); - btScalar radius = capsuleShape->getRadius(); - btScalar halfHeight = capsuleShape->getHalfHeight(); + int upAxis = coneShape->getConeUpIndex(); + getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); + break; + } + case CYLINDER_SHAPE_PROXYTYPE: + { + const btCylinderShape* cylinder = static_cast(shape); + int upAxis = cylinder->getUpAxis(); + btScalar radius = cylinder->getRadius(); + btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; + getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); + break; + } - int upAxis = capsuleShape->getUpAxis(); - getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); - break; - } - case CONE_SHAPE_PROXYTYPE: - { - const btConeShape* coneShape = static_cast(shape); - btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); - btScalar height = coneShape->getHeight();//+coneShape->getMargin(); + case STATIC_PLANE_PROXYTYPE: + { + const btStaticPlaneShape* staticPlaneShape = static_cast(shape); + btScalar planeConst = staticPlaneShape->getPlaneConstant(); + const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); + getDebugDrawer()->drawPlane(planeNormal, planeConst, worldTransform, color); + break; + } + default: + { + /// for polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*)shape; - int upAxis= coneShape->getConeUpIndex(); - getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); - break; + int i; + if (polyshape->getConvexPolyhedron()) + { + const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); + for (i = 0; i < poly->m_faces.size(); i++) + { + btVector3 centroid(0, 0, 0); + int numVerts = poly->m_faces[i].m_indices.size(); + if (numVerts) + { + int lastV = poly->m_faces[i].m_indices[numVerts - 1]; + for (int v = 0; v < poly->m_faces[i].m_indices.size(); v++) + { + int curVert = poly->m_faces[i].m_indices[v]; + centroid += poly->m_vertices[curVert]; + getDebugDrawer()->drawLine(worldTransform * poly->m_vertices[lastV], worldTransform * poly->m_vertices[curVert], color); + lastV = curVert; + } + } + centroid *= btScalar(1.f) / btScalar(numVerts); + if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals) + { + btVector3 normalColor(1, 1, 0); + btVector3 faceNormal(poly->m_faces[i].m_plane[0], poly->m_faces[i].m_plane[1], poly->m_faces[i].m_plane[2]); + getDebugDrawer()->drawLine(worldTransform * centroid, worldTransform * (centroid + faceNormal), normalColor); + } + } + } + else + { + for (i = 0; i < polyshape->getNumEdges(); i++) + { + btVector3 a, b; + polyshape->getEdge(i, a, b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa, wb, color); + } + } + } - } - case CYLINDER_SHAPE_PROXYTYPE: - { - const btCylinderShape* cylinder = static_cast(shape); - int upAxis = cylinder->getUpAxis(); - btScalar radius = cylinder->getRadius(); - btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; - getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); - break; - } + if (shape->isConcave()) + { + btConcaveShape* concaveMesh = (btConcaveShape*)shape; - case STATIC_PLANE_PROXYTYPE: - { - const btStaticPlaneShape* staticPlaneShape = static_cast(shape); - btScalar planeConst = staticPlaneShape->getPlaneConstant(); - const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); - getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color); - break; + ///@todo pass camera, for some culling? no -> we are not a graphics lib + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); - } - default: - { + DebugDrawcallback drawCallback(getDebugDrawer(), worldTransform, color); + concaveMesh->processAllTriangles(&drawCallback, aabbMin, aabbMax); + } - /// for polyhedral shapes - if (shape->isPolyhedral()) - { - btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; - - int i; - if (polyshape->getConvexPolyhedron()) - { - const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); - for (i=0;im_faces.size();i++) - { - btVector3 centroid(0,0,0); - int numVerts = poly->m_faces[i].m_indices.size(); - if (numVerts) - { - int lastV = poly->m_faces[i].m_indices[numVerts-1]; - for (int v=0;vm_faces[i].m_indices.size();v++) - { - int curVert = poly->m_faces[i].m_indices[v]; - centroid+=poly->m_vertices[curVert]; - getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color); - lastV = curVert; - } - } - centroid*= btScalar(1.f)/btScalar(numVerts); - if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals) - { - btVector3 normalColor(1,1,0); - btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]); - getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor); - } - - } - - - } else - { - for (i=0;igetNumEdges();i++) - { - btVector3 a,b; - polyshape->getEdge(i,a,b); - btVector3 wa = worldTransform * a; - btVector3 wb = worldTransform * b; - getDebugDrawer()->drawLine(wa,wb,color); - } - } - - - } - - if (shape->isConcave()) - { - btConcaveShape* concaveMesh = (btConcaveShape*) shape; - - ///@todo pass camera, for some culling? no -> we are not a graphics lib - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); - - } - - if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) - { - btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; - //todo: pass camera for some culling - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - //DebugDrawcallback drawCallback; - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); - } - - - - } - + if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*)shape; + //todo: pass camera for some culling + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + //DebugDrawcallback drawCallback; + DebugDrawcallback drawCallback(getDebugDrawer(), worldTransform, color); + convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback, aabbMin, aabbMax); + } + } } } } - -void btCollisionWorld::debugDrawWorld() +void btCollisionWorld::debugDrawWorld() { if (getDebugDrawer()) { @@ -1518,25 +1475,23 @@ void btCollisionWorld::debugDrawWorld() btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors(); - if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) + if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) { - - if (getDispatcher()) { int numManifolds = getDispatcher()->getNumManifolds(); - - for (int i=0;igetManifoldByIndexInternal(i); //btCollisionObject* obA = static_cast(contactManifold->getBody0()); //btCollisionObject* obB = static_cast(contactManifold->getBody1()); int numContacts = contactManifold->getNumContacts(); - for (int j=0;jgetContactPoint(j); - getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),defaultColors.m_contactPoint); + getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB, cp.m_normalWorldOnB, cp.getDistance(), cp.getLifeTime(), defaultColors.m_contactPoint); } } } @@ -1546,58 +1501,63 @@ void btCollisionWorld::debugDrawWorld() { int i; - for ( i=0;igetCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0) + if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT) == 0) { if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)) { - btVector3 color(btScalar(0.4),btScalar(0.4),btScalar(0.4)); + btVector3 color(btScalar(0.4), btScalar(0.4), btScalar(0.4)); - switch(colObj->getActivationState()) + switch (colObj->getActivationState()) { - case ACTIVE_TAG: - color = defaultColors.m_activeObject; break; - case ISLAND_SLEEPING: - color = defaultColors.m_deactivatedObject;break; - case WANTS_DEACTIVATION: - color = defaultColors.m_wantsDeactivationObject;break; - case DISABLE_DEACTIVATION: - color = defaultColors.m_disabledDeactivationObject;break; - case DISABLE_SIMULATION: - color = defaultColors.m_disabledSimulationObject;break; - default: + case ACTIVE_TAG: + color = defaultColors.m_activeObject; + break; + case ISLAND_SLEEPING: + color = defaultColors.m_deactivatedObject; + break; + case WANTS_DEACTIVATION: + color = defaultColors.m_wantsDeactivationObject; + break; + case DISABLE_DEACTIVATION: + color = defaultColors.m_disabledDeactivationObject; + break; + case DISABLE_SIMULATION: + color = defaultColors.m_disabledSimulationObject; + break; + default: { - color = btVector3(btScalar(.3),btScalar(0.3),btScalar(0.3)); + color = btVector3(btScalar(.3), btScalar(0.3), btScalar(0.3)); } }; colObj->getCustomDebugColor(color); - debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); + debugDrawObject(colObj->getWorldTransform(), colObj->getCollisionShape(), color); } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { - btVector3 minAabb,maxAabb; + btVector3 minAabb, maxAabb; btVector3 colorvec = defaultColors.m_aabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb); + btVector3 contactThreshold(gContactBreakingThreshold, gContactBreakingThreshold, gContactBreakingThreshold); minAabb -= contactThreshold; maxAabb += contactThreshold; - btVector3 minAabb2,maxAabb2; + btVector3 minAabb2, maxAabb2; - if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) + if (getDispatchInfo().m_useContinuous && colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) { - colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(), minAabb2, maxAabb2); minAabb2 -= contactThreshold; maxAabb2 += contactThreshold; minAabb.setMin(minAabb2); maxAabb.setMax(maxAabb2); } - m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); + m_debugDrawer->drawAabb(minAabb, maxAabb, colorvec); } } } @@ -1605,45 +1565,63 @@ void btCollisionWorld::debugDrawWorld() } } - -void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) +void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) { int i; ///keep track of shapes already serialized - btHashMap serializedShapes; + btHashMap serializedShapes; - for (i=0;igetCollisionShape(); if (!serializedShapes.find(shape)) { - serializedShapes.insert(shape,shape); + serializedShapes.insert(shape, shape); shape->serializeSingleShape(serializer); } } //serialize all collision objects - for (i=0;igetInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)) + if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) { colObj->serializeSingleObject(serializer); } } } - -void btCollisionWorld::serialize(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; - serializer->startSerialization(); - - serializeCollisionObjects(serializer); - - serializer->finishSerialization(); + 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/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index eede2b28c..fd0e5b9bb 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - /** * @mainpage Bullet Documentation * @@ -66,8 +65,6 @@ subject to the following restrictions: * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf * */ - - #ifndef BT_COLLISION_WORLD_H #define BT_COLLISION_WORLD_H @@ -87,144 +84,138 @@ class btSerializer; ///CollisionWorld is interface and container for the collision detection class btCollisionWorld { - - protected: + btAlignedObjectArray m_collisionObjects; - btAlignedObjectArray m_collisionObjects; - - btDispatcher* m_dispatcher1; + btDispatcher* m_dispatcher1; - btDispatcherInfo m_dispatchInfo; + btDispatcherInfo m_dispatchInfo; - btBroadphaseInterface* m_broadphasePairCache; + btBroadphaseInterface* m_broadphasePairCache; - btIDebugDraw* m_debugDrawer; + btIDebugDraw* m_debugDrawer; ///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs ///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB) bool m_forceUpdateAllAabbs; - void serializeCollisionObjects(btSerializer* serializer); + void serializeCollisionObjects(btSerializer* serializer); + + void serializeContactManifolds(btSerializer* serializer); public: - //this constructor doesn't own the dispatcher and paircache/broadphase - btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration); + btCollisionWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration); virtual ~btCollisionWorld(); - void setBroadphase(btBroadphaseInterface* pairCache) + void setBroadphase(btBroadphaseInterface* pairCache) { m_broadphasePairCache = pairCache; } - const btBroadphaseInterface* getBroadphase() const + const btBroadphaseInterface* getBroadphase() const { return m_broadphasePairCache; } - btBroadphaseInterface* getBroadphase() + btBroadphaseInterface* getBroadphase() { return m_broadphasePairCache; } - btOverlappingPairCache* getPairCache() + btOverlappingPairCache* getPairCache() { return m_broadphasePairCache->getOverlappingPairCache(); } - - btDispatcher* getDispatcher() + btDispatcher* getDispatcher() { return m_dispatcher1; } - const btDispatcher* getDispatcher() const + const btDispatcher* getDispatcher() const { return m_dispatcher1; } - void updateSingleAabb(btCollisionObject* colObj); + void updateSingleAabb(btCollisionObject* colObj); - virtual void updateAabbs(); + virtual void updateAabbs(); ///the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSimulation) ///it can be useful to use if you perform ray tests without collision detection/simulation - virtual void computeOverlappingPairs(); + virtual void computeOverlappingPairs(); - - virtual void setDebugDrawer(btIDebugDraw* debugDrawer) + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) { - m_debugDrawer = debugDrawer; + m_debugDrawer = debugDrawer; } - virtual btIDebugDraw* getDebugDrawer() + virtual btIDebugDraw* getDebugDrawer() { return m_debugDrawer; } - virtual void debugDrawWorld(); + virtual void debugDrawWorld(); virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); - ///LocalShapeInfo gives extra information for complex shapes ///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart - struct LocalShapeInfo + struct LocalShapeInfo { - int m_shapePart; - int m_triangleIndex; - + int m_shapePart; + int m_triangleIndex; + //const btCollisionShape* m_shapeTemp; //const btTransform* m_shapeLocalTransform; }; - struct LocalRayResult + struct LocalRayResult { - LocalRayResult(const btCollisionObject* collisionObject, - LocalShapeInfo* localShapeInfo, - const btVector3& hitNormalLocal, - btScalar hitFraction) - :m_collisionObject(collisionObject), - m_localShapeInfo(localShapeInfo), - m_hitNormalLocal(hitNormalLocal), - m_hitFraction(hitFraction) + LocalRayResult(const btCollisionObject* collisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + btScalar hitFraction) + : m_collisionObject(collisionObject), + m_localShapeInfo(localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitFraction(hitFraction) { } - const btCollisionObject* m_collisionObject; - LocalShapeInfo* m_localShapeInfo; - btVector3 m_hitNormalLocal; - btScalar m_hitFraction; - + const btCollisionObject* m_collisionObject; + LocalShapeInfo* m_localShapeInfo; + btVector3 m_hitNormalLocal; + btScalar m_hitFraction; }; ///RayResultCallback is used to report new raycast results - struct RayResultCallback + struct RayResultCallback { - btScalar m_closestHitFraction; - const btCollisionObject* m_collisionObject; - int m_collisionFilterGroup; - int m_collisionFilterMask; + btScalar m_closestHitFraction; + const btCollisionObject* m_collisionObject; + 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; virtual ~RayResultCallback() { } - bool hasHit() const + bool hasHit() const { return (m_collisionObject != 0); } RayResultCallback() - :m_closestHitFraction(btScalar(1.)), - m_collisionObject(0), - m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter), - //@BP Mod - m_flags(0) + : m_closestHitFraction(btScalar(1.)), + m_collisionObject(0), + m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + //@BP Mod + m_flags(0) { } @@ -235,62 +226,62 @@ public: return collides; } - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0; + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) = 0; }; - struct ClosestRayResultCallback : public RayResultCallback + struct ClosestRayResultCallback : public RayResultCallback { - ClosestRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld) + ClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld) { } - btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction - btVector3 m_rayToWorld; + btVector3 m_rayFromWorld; //used to calculate hitPointWorld from hitFraction + btVector3 m_rayToWorld; - btVector3 m_hitNormalWorld; - btVector3 m_hitPointWorld; - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; + + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) { //caller already does the filter on the m_closestHitFraction btAssert(rayResult.m_hitFraction <= m_closestHitFraction); - + m_closestHitFraction = rayResult.m_hitFraction; m_collisionObject = rayResult.m_collisionObject; if (normalInWorldSpace) { m_hitNormalWorld = rayResult.m_hitNormalLocal; - } else + } + else { ///need to transform normal into worldspace - m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; } - m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); + m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); return rayResult.m_hitFraction; } }; - struct AllHitsRayResultCallback : public RayResultCallback + struct AllHitsRayResultCallback : public RayResultCallback { - AllHitsRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld) + AllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld) { } - btAlignedObjectArray m_collisionObjects; + btAlignedObjectArray m_collisionObjects; - btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction - btVector3 m_rayToWorld; + btVector3 m_rayFromWorld; //used to calculate hitPointWorld from hitFraction + btVector3 m_rayToWorld; - btAlignedObjectArray m_hitNormalWorld; - btAlignedObjectArray m_hitPointWorld; + btAlignedObjectArray m_hitNormalWorld; + btAlignedObjectArray m_hitPointWorld; btAlignedObjectArray m_hitFractions; - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) + + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) { m_collisionObject = rayResult.m_collisionObject; m_collisionObjects.push_back(rayResult.m_collisionObject); @@ -298,69 +289,66 @@ public: if (normalInWorldSpace) { hitNormalWorld = rayResult.m_hitNormalLocal; - } else + } + else { ///need to transform normal into worldspace - hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; } m_hitNormalWorld.push_back(hitNormalWorld); btVector3 hitPointWorld; - hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); + hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); m_hitPointWorld.push_back(hitPointWorld); m_hitFractions.push_back(rayResult.m_hitFraction); return m_closestHitFraction; } }; - struct LocalConvexResult { - LocalConvexResult(const btCollisionObject* hitCollisionObject, - LocalShapeInfo* localShapeInfo, - const btVector3& hitNormalLocal, - const btVector3& hitPointLocal, - btScalar hitFraction - ) - :m_hitCollisionObject(hitCollisionObject), - m_localShapeInfo(localShapeInfo), - m_hitNormalLocal(hitNormalLocal), - m_hitPointLocal(hitPointLocal), - m_hitFraction(hitFraction) + LocalConvexResult(const btCollisionObject* hitCollisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + const btVector3& hitPointLocal, + btScalar hitFraction) + : m_hitCollisionObject(hitCollisionObject), + m_localShapeInfo(localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitPointLocal(hitPointLocal), + m_hitFraction(hitFraction) { } - const btCollisionObject* m_hitCollisionObject; - LocalShapeInfo* m_localShapeInfo; - btVector3 m_hitNormalLocal; - btVector3 m_hitPointLocal; - btScalar m_hitFraction; + const btCollisionObject* m_hitCollisionObject; + LocalShapeInfo* m_localShapeInfo; + btVector3 m_hitNormalLocal; + btVector3 m_hitPointLocal; + btScalar m_hitFraction; }; ///RayResultCallback is used to report new raycast results - struct ConvexResultCallback + struct ConvexResultCallback { - btScalar m_closestHitFraction; - int m_collisionFilterGroup; - int m_collisionFilterMask; - + btScalar m_closestHitFraction; + int m_collisionFilterGroup; + int m_collisionFilterMask; + ConvexResultCallback() - :m_closestHitFraction(btScalar(1.)), - m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter) + : m_closestHitFraction(btScalar(1.)), + m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter) { } virtual ~ConvexResultCallback() { } - - bool hasHit() const + + bool hasHit() const { return (m_closestHitFraction < btScalar(1.)); } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const { bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; @@ -368,39 +356,40 @@ public: return collides; } - virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0; + virtual btScalar addSingleResult(LocalConvexResult& convexResult, bool normalInWorldSpace) = 0; }; - struct ClosestConvexResultCallback : public ConvexResultCallback + struct ClosestConvexResultCallback : public ConvexResultCallback { - ClosestConvexResultCallback(const btVector3& convexFromWorld,const btVector3& convexToWorld) - :m_convexFromWorld(convexFromWorld), - m_convexToWorld(convexToWorld), - m_hitCollisionObject(0) + ClosestConvexResultCallback(const btVector3& convexFromWorld, const btVector3& convexToWorld) + : m_convexFromWorld(convexFromWorld), + m_convexToWorld(convexToWorld), + m_hitCollisionObject(0) { } - btVector3 m_convexFromWorld;//used to calculate hitPointWorld from hitFraction - btVector3 m_convexToWorld; + btVector3 m_convexFromWorld; //used to calculate hitPointWorld from hitFraction + btVector3 m_convexToWorld; - btVector3 m_hitNormalWorld; - btVector3 m_hitPointWorld; - const btCollisionObject* m_hitCollisionObject; - - virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; + const btCollisionObject* m_hitCollisionObject; + + virtual btScalar addSingleResult(LocalConvexResult& convexResult, bool normalInWorldSpace) { -//caller already does the filter on the m_closestHitFraction + //caller already does the filter on the m_closestHitFraction btAssert(convexResult.m_hitFraction <= m_closestHitFraction); - + m_closestHitFraction = convexResult.m_hitFraction; m_hitCollisionObject = convexResult.m_hitCollisionObject; if (normalInWorldSpace) { m_hitNormalWorld = convexResult.m_hitNormalLocal; - } else + } + else { ///need to transform normal into worldspace - m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; + m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal; } m_hitPointWorld = convexResult.m_hitPointLocal; return convexResult.m_hitFraction; @@ -408,23 +397,23 @@ public: }; ///ContactResultCallback is used to report contact points - struct ContactResultCallback + struct ContactResultCallback { - int m_collisionFilterGroup; - int m_collisionFilterMask; - btScalar m_closestDistanceThreshold; + int m_collisionFilterGroup; + int m_collisionFilterMask; + btScalar m_closestDistanceThreshold; ContactResultCallback() - :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter), - m_closestDistanceThreshold(0) + : m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + m_closestDistanceThreshold(0) { } virtual ~ContactResultCallback() { } - + virtual bool needsCollision(btBroadphaseProxy* proxy0) const { bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; @@ -432,58 +421,57 @@ public: return collides; } - virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) = 0; + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) = 0; }; - - - int getNumCollisionObjects() const + int getNumCollisionObjects() const { return int(m_collisionObjects.size()); } /// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. - virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; + virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; /// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback /// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. - void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const; + void convexSweepTest(const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const; ///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback. ///it reports one or more contact points for every overlapping object (including the one with deepest penetration) - void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback); + void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback); ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected. ///it reports one or more contact points (including the one with deepest penetration) - void contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback); - + void contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback); /// 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); + static void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback); - static void rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans, - const btCollisionObjectWrapper* collisionObjectWrap, - RayResultCallback& resultCallback); + static void rayTestSingleInternal(const btTransform& rayFromTrans, const btTransform& rayToTrans, + const btCollisionObjectWrapper* collisionObjectWrap, + RayResultCallback& resultCallback); /// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest. - static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - ConvexResultCallback& resultCallback, btScalar allowedPenetration); + static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback, btScalar allowedPenetration); - static void objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - const btCollisionObjectWrapper* colObjWrap, - ConvexResultCallback& resultCallback, btScalar allowedPenetration); + static void objectQuerySingleInternal(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, + const btCollisionObjectWrapper* colObjWrap, + ConvexResultCallback& resultCallback, btScalar allowedPenetration); - virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter); + virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); + + virtual void refreshBroadphaseProxy(btCollisionObject* collisionObject); btCollisionObjectArray& getCollisionObjectArray() { @@ -495,10 +483,9 @@ public: return m_collisionObjects; } + virtual void removeCollisionObject(btCollisionObject* collisionObject); - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - virtual void performDiscreteCollisionDetection(); + virtual void performDiscreteCollisionDetection(); btDispatcherInfo& getDispatchInfo() { @@ -509,20 +496,18 @@ public: { return m_dispatchInfo; } - - bool getForceUpdateAllAabbs() const + + bool getForceUpdateAllAabbs() const { return m_forceUpdateAllAabbs; } - void setForceUpdateAllAabbs( bool forceUpdateAllAabbs) + void setForceUpdateAllAabbs(bool forceUpdateAllAabbs) { m_forceUpdateAllAabbs = forceUpdateAllAabbs; } ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (Bullet/Demos/SerializeDemo) - virtual void serialize(btSerializer* serializer); - + virtual void serialize(btSerializer* serializer); }; - -#endif //BT_COLLISION_WORLD_H +#endif //BT_COLLISION_WORLD_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp index f2b083780..9f422dc15 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp @@ -15,269 +15,251 @@ subject to the following restrictions: #include "btCollisionWorldImporter.h" #include "btBulletCollisionCommon.h" -#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition +#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT #include "BulletCollision/Gimpact/btGImpactShape.h" -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT btCollisionWorldImporter::btCollisionWorldImporter(btCollisionWorld* world) -:m_collisionWorld(world), -m_verboseMode(0) + : m_collisionWorld(world), + m_verboseMode(0) { - } btCollisionWorldImporter::~btCollisionWorldImporter() { } - - - - -bool btCollisionWorldImporter::convertAllObjects( btBulletSerializedArrays* arrays) +bool btCollisionWorldImporter::convertAllObjects(btBulletSerializedArrays* arrays) { - m_shapeMap.clear(); m_bodyMap.clear(); int i; - for (i=0;im_bvhsDouble.size();i++) + for (i = 0; i < arrays->m_bvhsDouble.size(); i++) { btOptimizedBvh* bvh = createOptimizedBvh(); btQuantizedBvhDoubleData* bvhData = arrays->m_bvhsDouble[i]; bvh->deSerializeDouble(*bvhData); - m_bvhMap.insert(arrays->m_bvhsDouble[i],bvh); + m_bvhMap.insert(arrays->m_bvhsDouble[i], bvh); } - for (i=0;im_bvhsFloat.size();i++) - { - btOptimizedBvh* bvh = createOptimizedBvh(); - btQuantizedBvhFloatData* bvhData = arrays->m_bvhsFloat[i]; + for (i = 0; i < arrays->m_bvhsFloat.size(); i++) + { + btOptimizedBvh* bvh = createOptimizedBvh(); + btQuantizedBvhFloatData* bvhData = arrays->m_bvhsFloat[i]; bvh->deSerializeFloat(*bvhData); - m_bvhMap.insert(arrays->m_bvhsFloat[i],bvh); + m_bvhMap.insert(arrays->m_bvhsFloat[i], bvh); } - - - - - for (i=0;im_colShapeData.size();i++) + for (i = 0; i < arrays->m_colShapeData.size(); i++) { btCollisionShapeData* shapeData = arrays->m_colShapeData[i]; btCollisionShape* shape = convertCollisionShape(shapeData); if (shape) { - // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); - m_shapeMap.insert(shapeData,shape); + // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); + m_shapeMap.insert(shapeData, shape); } - if (shape&& shapeData->m_name) + if (shape && shapeData->m_name) { char* newname = duplicateName(shapeData->m_name); - m_objectNameMap.insert(shape,newname); - m_nameShapeMap.insert(newname,shape); + m_objectNameMap.insert(shape, newname); + m_nameShapeMap.insert(newname, shape); } } - - for (i=0;im_collisionObjectDataDouble.size();i++) + for (i = 0; i < arrays->m_collisionObjectDataDouble.size(); i++) { - btCollisionObjectDoubleData* colObjData = arrays->m_collisionObjectDataDouble[i]; - btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); - if (shapePtr && *shapePtr) - { - btTransform startTransform; - colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; - startTransform.deSerializeDouble(colObjData->m_worldTransform); + btCollisionObjectDoubleData* colObjData = arrays->m_collisionObjectDataDouble[i]; + btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeDouble(colObjData->m_worldTransform); - btCollisionShape* shape = (btCollisionShape*)*shapePtr; - btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); - body->setFriction(btScalar(colObjData->m_friction)); - body->setRestitution(btScalar(colObjData->m_restitution)); + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + btCollisionObject* body = createCollisionObject(startTransform, shape, colObjData->m_name); + body->setFriction(btScalar(colObjData->m_friction)); + body->setRestitution(btScalar(colObjData->m_restitution)); #ifdef USE_INTERNAL_EDGE_UTILITY - if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; - if (trimesh->getTriangleInfoMap()) - { - body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); - } - } -#endif //USE_INTERNAL_EDGE_UTILITY - m_bodyMap.insert(colObjData,body); - } else - { - printf("error: no shape found\n"); - } + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; + if (trimesh->getTriangleInfoMap()) + { + body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } +#endif //USE_INTERNAL_EDGE_UTILITY + m_bodyMap.insert(colObjData, body); + } + else + { + printf("error: no shape found\n"); + } } - for (i=0;im_collisionObjectDataFloat.size();i++) + for (i = 0; i < arrays->m_collisionObjectDataFloat.size(); i++) { - btCollisionObjectFloatData* colObjData = arrays->m_collisionObjectDataFloat[i]; - btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); - if (shapePtr && *shapePtr) - { - btTransform startTransform; - colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; - startTransform.deSerializeFloat(colObjData->m_worldTransform); + btCollisionObjectFloatData* colObjData = arrays->m_collisionObjectDataFloat[i]; + btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeFloat(colObjData->m_worldTransform); - btCollisionShape* shape = (btCollisionShape*)*shapePtr; - btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + btCollisionObject* body = createCollisionObject(startTransform, shape, colObjData->m_name); #ifdef USE_INTERNAL_EDGE_UTILITY - if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; - if (trimesh->getTriangleInfoMap()) - { - body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); - } - } -#endif //USE_INTERNAL_EDGE_UTILITY - m_bodyMap.insert(colObjData,body); - } else - { - printf("error: no shape found\n"); - } - } + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; + if (trimesh->getTriangleInfoMap()) + { + body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } +#endif //USE_INTERNAL_EDGE_UTILITY + m_bodyMap.insert(colObjData, body); + } + else + { + printf("error: no shape found\n"); + } + } return true; } - - void btCollisionWorldImporter::deleteAllData() { int i; - for (i=0;iremoveCollisionObject(m_allocatedCollisionObjects[i]); delete m_allocatedCollisionObjects[i]; } m_allocatedCollisionObjects.clear(); - - for (i=0;im_numMeshParts;a++) + for (int a = 0; a < curData->m_numMeshParts; a++) { btMeshPartData* curPart = &curData->m_meshPartsPtr[a]; - if(curPart->m_vertices3f) - delete [] curPart->m_vertices3f; + if (curPart->m_vertices3f) + delete[] curPart->m_vertices3f; - if(curPart->m_vertices3d) - delete [] curPart->m_vertices3d; + if (curPart->m_vertices3d) + delete[] curPart->m_vertices3d; - if(curPart->m_indices32) - delete [] curPart->m_indices32; + if (curPart->m_indices32) + delete[] curPart->m_indices32; - if(curPart->m_3indices16) - delete [] curPart->m_3indices16; + if (curPart->m_3indices16) + delete[] curPart->m_3indices16; - if(curPart->m_indices16) - delete [] curPart->m_indices16; + if (curPart->m_indices16) + delete[] curPart->m_indices16; if (curPart->m_3indices8) - delete [] curPart->m_3indices8; - + delete[] curPart->m_3indices8; } - delete [] curData->m_meshPartsPtr; + delete[] curData->m_meshPartsPtr; delete curData; } m_allocatedbtStridingMeshInterfaceDatas.clear(); - for (i=0;im_shapeType) - { - case STATIC_PLANE_PROXYTYPE: + { + case STATIC_PLANE_PROXYTYPE: { btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData; - btVector3 planeNormal,localScaling; + btVector3 planeNormal, localScaling; planeNormal.deSerializeFloat(planeData->m_planeNormal); localScaling.deSerializeFloat(planeData->m_localScaling); - shape = createPlaneShape(planeNormal,planeData->m_planeConstant); + shape = createPlaneShape(planeNormal, planeData->m_planeConstant); shape->setLocalScaling(localScaling); break; } - case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE: + case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE: { - btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*) shapeData; - btCollisionShapeData* colShapeData = (btCollisionShapeData*) &scaledMesh->m_trimeshShapeData; + btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*)shapeData; + btCollisionShapeData* colShapeData = (btCollisionShapeData*)&scaledMesh->m_trimeshShapeData; colShapeData->m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; btCollisionShape* childShape = convertCollisionShape(colShapeData); btBvhTriangleMeshShape* meshShape = (btBvhTriangleMeshShape*)childShape; @@ -288,15 +270,14 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS break; } #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT - case GIMPACT_SHAPE_PROXYTYPE: + case GIMPACT_SHAPE_PROXYTYPE: { - btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData; + btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*)shapeData; if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE) { btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&gimpactData->m_meshInterface); btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData); - btGImpactMeshShape* gimpactShape = createGimpactShape(meshInterface); btVector3 localScaling; localScaling.deSerializeFloat(gimpactData->m_localScaling); @@ -304,47 +285,45 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS gimpactShape->setMargin(btScalar(gimpactData->m_collisionMargin)); gimpactShape->updateBound(); shape = gimpactShape; - } else + } + else { printf("unsupported gimpact sub type\n"); } break; } -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT - //The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API - //so deal with this +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT \ + //The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API \ + //so deal with this case CAPSULE_SHAPE_PROXYTYPE: { btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData; - switch (capData->m_upAxis) { - case 0: + case 0: { - shape = createCapsuleShapeX(1,1); + shape = createCapsuleShapeX(1, 1); break; } - case 1: + case 1: { - shape = createCapsuleShapeY(1,1); + shape = createCapsuleShapeY(1, 1); break; } - case 2: + case 2: { - shape = createCapsuleShapeZ(1,1); + shape = createCapsuleShapeZ(1, 1); break; } - default: + default: { printf("error: wrong up axis for btCapsuleShape\n"); } - - }; if (shape) { - btCapsuleShape* cap = (btCapsuleShape*) shape; + btCapsuleShape* cap = (btCapsuleShape*)shape; cap->deSerializeFloat(capData); } break; @@ -355,163 +334,156 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS case SPHERE_SHAPE_PROXYTYPE: case MULTI_SPHERE_SHAPE_PROXYTYPE: case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData; + btVector3 implicitShapeDimensions; + implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions); + btVector3 localScaling; + localScaling.deSerializeFloat(bsd->m_localScaling); + btVector3 margin(bsd->m_collisionMargin, bsd->m_collisionMargin, bsd->m_collisionMargin); + switch (shapeData->m_shapeType) { - btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData; - btVector3 implicitShapeDimensions; - implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions); + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* box = (btBoxShape*)createBoxShape(implicitShapeDimensions / localScaling + margin); + //box->initializePolyhedralFeatures(); + shape = box; + + break; + } + case SPHERE_SHAPE_PROXYTYPE: + { + shape = createSphereShape(implicitShapeDimensions.getX()); + break; + } + + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShapeData* cylData = (btCylinderShapeData*)shapeData; + btVector3 halfExtents = implicitShapeDimensions + margin; + switch (cylData->m_upAxis) + { + case 0: + { + shape = createCylinderShapeX(halfExtents.getY(), halfExtents.getX()); + break; + } + case 1: + { + shape = createCylinderShapeY(halfExtents.getX(), halfExtents.getY()); + break; + } + case 2: + { + shape = createCylinderShapeZ(halfExtents.getX(), halfExtents.getZ()); + break; + } + default: + { + printf("unknown Cylinder up axis\n"); + } + }; + + break; + } + case CONE_SHAPE_PROXYTYPE: + { + btConeShapeData* conData = (btConeShapeData*)shapeData; + btVector3 halfExtents = implicitShapeDimensions; //+margin; + switch (conData->m_upIndex) + { + case 0: + { + shape = createConeShapeX(halfExtents.getY(), halfExtents.getX()); + break; + } + case 1: + { + shape = createConeShapeY(halfExtents.getX(), halfExtents.getY()); + break; + } + case 2: + { + shape = createConeShapeZ(halfExtents.getX(), halfExtents.getZ()); + break; + } + default: + { + printf("unknown Cone up axis\n"); + } + }; + + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd; + int numSpheres = mss->m_localPositionArraySize; + + btAlignedObjectArray tmpPos; + btAlignedObjectArray radii; + radii.resize(numSpheres); + tmpPos.resize(numSpheres); + int i; + for (i = 0; i < numSpheres; i++) + { + tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos); + radii[i] = mss->m_localPositionArrayPtr[i].m_radius; + } + shape = createMultiSphereShape(&tmpPos[0], &radii[0], numSpheres); + break; + } + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + // int sz = sizeof(btConvexHullShapeData); + // int sz2 = sizeof(btConvexInternalShapeData); + // int sz3 = sizeof(btCollisionShapeData); + btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd; + int numPoints = convexData->m_numUnscaledPoints; + + btAlignedObjectArray tmpPoints; + tmpPoints.resize(numPoints); + int i; + for (i = 0; i < numPoints; i++) + { +#ifdef BT_USE_DOUBLE_PRECISION + if (convexData->m_unscaledPointsDoublePtr) + tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]); + if (convexData->m_unscaledPointsFloatPtr) + tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]); +#else + if (convexData->m_unscaledPointsFloatPtr) + tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]); + if (convexData->m_unscaledPointsDoublePtr) + tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]); +#endif //BT_USE_DOUBLE_PRECISION + } + btConvexHullShape* hullShape = createConvexHullShape(); + for (i = 0; i < numPoints; i++) + { + hullShape->addPoint(tmpPoints[i]); + } + hullShape->setMargin(bsd->m_collisionMargin); + //hullShape->initializePolyhedralFeatures(); + shape = hullShape; + break; + } + default: + { + printf("error: cannot create shape type (%d)\n", shapeData->m_shapeType); + } + } + + if (shape) + { + shape->setMargin(bsd->m_collisionMargin); + btVector3 localScaling; localScaling.deSerializeFloat(bsd->m_localScaling); - btVector3 margin(bsd->m_collisionMargin,bsd->m_collisionMargin,bsd->m_collisionMargin); - switch (shapeData->m_shapeType) - { - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin); - //box->initializePolyhedralFeatures(); - shape = box; - - break; - } - case SPHERE_SHAPE_PROXYTYPE: - { - shape = createSphereShape(implicitShapeDimensions.getX()); - break; - } - - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShapeData* cylData = (btCylinderShapeData*) shapeData; - btVector3 halfExtents = implicitShapeDimensions+margin; - switch (cylData->m_upAxis) - { - case 0: - { - shape = createCylinderShapeX(halfExtents.getY(),halfExtents.getX()); - break; - } - case 1: - { - shape = createCylinderShapeY(halfExtents.getX(),halfExtents.getY()); - break; - } - case 2: - { - shape = createCylinderShapeZ(halfExtents.getX(),halfExtents.getZ()); - break; - } - default: - { - printf("unknown Cylinder up axis\n"); - } - - }; - - - - break; - } - case CONE_SHAPE_PROXYTYPE: - { - btConeShapeData* conData = (btConeShapeData*) shapeData; - btVector3 halfExtents = implicitShapeDimensions;//+margin; - switch (conData->m_upIndex) - { - case 0: - { - shape = createConeShapeX(halfExtents.getY(),halfExtents.getX()); - break; - } - case 1: - { - shape = createConeShapeY(halfExtents.getX(),halfExtents.getY()); - break; - } - case 2: - { - shape = createConeShapeZ(halfExtents.getX(),halfExtents.getZ()); - break; - } - default: - { - printf("unknown Cone up axis\n"); - } - - }; - - - - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: - { - btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd; - int numSpheres = mss->m_localPositionArraySize; - - btAlignedObjectArray tmpPos; - btAlignedObjectArray radii; - radii.resize(numSpheres); - tmpPos.resize(numSpheres); - int i; - for ( i=0;im_localPositionArrayPtr[i].m_pos); - radii[i] = mss->m_localPositionArrayPtr[i].m_radius; - } - shape = createMultiSphereShape(&tmpPos[0],&radii[0],numSpheres); - break; - } - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - // int sz = sizeof(btConvexHullShapeData); - // int sz2 = sizeof(btConvexInternalShapeData); - // int sz3 = sizeof(btCollisionShapeData); - btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd; - int numPoints = convexData->m_numUnscaledPoints; - - btAlignedObjectArray tmpPoints; - tmpPoints.resize(numPoints); - int i; - for ( i=0;im_unscaledPointsDoublePtr) - tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]); - if (convexData->m_unscaledPointsFloatPtr) - tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]); -#else - if (convexData->m_unscaledPointsFloatPtr) - tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]); - if (convexData->m_unscaledPointsDoublePtr) - tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]); -#endif //BT_USE_DOUBLE_PRECISION - } - btConvexHullShape* hullShape = createConvexHullShape(); - for (i=0;iaddPoint(tmpPoints[i]); - } - hullShape->setMargin(bsd->m_collisionMargin); - //hullShape->initializePolyhedralFeatures(); - shape = hullShape; - break; - } - default: - { - printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType); - } - } - - if (shape) - { - shape->setMargin(bsd->m_collisionMargin); - - btVector3 localScaling; - localScaling.deSerializeFloat(bsd->m_localScaling); - shape->setLocalScaling(localScaling); - - } - break; + shape->setLocalScaling(localScaling); } + break; + } case TRIANGLE_MESH_SHAPE_PROXYTYPE: { btTriangleMeshShapeData* trimesh = (btTriangleMeshShapeData*)shapeData; @@ -522,10 +494,10 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS return 0; } - btVector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling); + btVector3 scaling; + scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling); meshInterface->setScaling(scaling); - btOptimizedBvh* bvh = 0; #if 1 if (trimesh->m_quantizedFloatBvh) @@ -534,7 +506,8 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS if (bvhPtr && *bvhPtr) { bvh = *bvhPtr; - } else + } + else { bvh = createOptimizedBvh(); bvh->deSerializeFloat(*trimesh->m_quantizedFloatBvh); @@ -546,7 +519,8 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS if (bvhPtr && *bvhPtr) { bvh = *bvhPtr; - } else + } + else { bvh = createOptimizedBvh(); bvh->deSerializeDouble(*trimesh->m_quantizedDoubleBvh); @@ -554,8 +528,7 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS } #endif - - btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface,bvh); + btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface, bvh); trimeshShape->setMargin(trimesh->m_collisionMargin); shape = trimeshShape; @@ -567,71 +540,66 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS #ifdef USE_INTERNAL_EDGE_UTILITY gContactAddedCallback = btAdjustInternalEdgeContactsCallback; -#endif //USE_INTERNAL_EDGE_UTILITY - +#endif //USE_INTERNAL_EDGE_UTILITY } //printf("trimesh->m_collisionMargin=%f\n",trimesh->m_collisionMargin); break; } case COMPOUND_SHAPE_PROXYTYPE: + { + btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; + btCompoundShape* compoundShape = createCompoundShape(); + + //btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0]; + + btAlignedObjectArray childShapes; + for (int i = 0; i < compoundData->m_numChildShapes; i++) { - btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; - btCompoundShape* compoundShape = createCompoundShape(); + //btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i]; - //btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0]; + btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; - - btAlignedObjectArray childShapes; - for (int i=0;im_numChildShapes;i++) + btCollisionShape* childShape = convertCollisionShape(cd); + if (childShape) { - //btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i]; - - btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; - - btCollisionShape* childShape = convertCollisionShape(cd); - if (childShape) - { - btTransform localTransform; - localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform); - compoundShape->addChildShape(localTransform,childShape); - } else - { -#ifdef _DEBUG - printf("error: couldn't create childShape for compoundShape\n"); -#endif - } - + btTransform localTransform; + localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform); + compoundShape->addChildShape(localTransform, childShape); } - shape = compoundShape; - - break; - } - case SOFTBODY_SHAPE_PROXYTYPE: - { - return 0; - } - default: - { + else + { #ifdef _DEBUG - printf("unsupported shape type (%d)\n",shapeData->m_shapeType); + printf("error: couldn't create childShape for compoundShape\n"); #endif + } } + shape = compoundShape; + + break; } + case SOFTBODY_SHAPE_PROXYTYPE: + { + return 0; + } + default: + { +#ifdef _DEBUG + printf("unsupported shape type (%d)\n", shapeData->m_shapeType); +#endif + } + } - return shape; - + return shape; } - - char* btCollisionWorldImporter::duplicateName(const char* name) { if (name) { int l = (int)strlen(name); - char* newName = new char[l+1]; - memcpy(newName,name,l); + char* newName = new char[l + 1]; + memcpy(newName, name, l); newName[l] = 0; m_allocatedNames.push_back(newName); return newName; @@ -639,53 +607,43 @@ char* btCollisionWorldImporter::duplicateName(const char* name) return 0; } - - - - - - - - - - -btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData& meshData) +btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData& meshData) { btTriangleIndexVertexArray* meshInterface = createTriangleMeshContainer(); - for (int i=0;iaddIndexedMesh(meshPart,meshPart.m_indexType); + meshInterface->addIndexedMesh(meshPart, meshPart.m_indexType); } } return meshInterface; } - btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData) { //create a new btStridingMeshInterfaceData that is an exact copy of shapedata and store it in the WorldImporter @@ -776,7 +733,7 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa newData->m_numMeshParts = interfaceData->m_numMeshParts; newData->m_meshPartsPtr = new btMeshPartData[newData->m_numMeshParts]; - for(int i = 0;i < newData->m_numMeshParts;i++) + for (int i = 0; i < newData->m_numMeshParts; i++) { btMeshPartData* curPart = &interfaceData->m_meshPartsPtr[i]; btMeshPartData* curNewPart = &newData->m_meshPartsPtr[i]; @@ -784,18 +741,18 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa curNewPart->m_numTriangles = curPart->m_numTriangles; curNewPart->m_numVertices = curPart->m_numVertices; - if(curPart->m_vertices3f) + if (curPart->m_vertices3f) { curNewPart->m_vertices3f = new btVector3FloatData[curNewPart->m_numVertices]; - memcpy(curNewPart->m_vertices3f,curPart->m_vertices3f,sizeof(btVector3FloatData) * curNewPart->m_numVertices); + memcpy(curNewPart->m_vertices3f, curPart->m_vertices3f, sizeof(btVector3FloatData) * curNewPart->m_numVertices); } else curNewPart->m_vertices3f = NULL; - if(curPart->m_vertices3d) + if (curPart->m_vertices3d) { curNewPart->m_vertices3d = new btVector3DoubleData[curNewPart->m_numVertices]; - memcpy(curNewPart->m_vertices3d,curPart->m_vertices3d,sizeof(btVector3DoubleData) * curNewPart->m_numVertices); + memcpy(curNewPart->m_vertices3d, curPart->m_vertices3d, sizeof(btVector3DoubleData) * curNewPart->m_numVertices); } else curNewPart->m_vertices3d = NULL; @@ -803,63 +760,60 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa int numIndices = curNewPart->m_numTriangles * 3; ///the m_3indices8 was not initialized in some Bullet versions, this can cause crashes at loading time ///we catch it by only dealing with m_3indices8 if none of the other indices are initialized - bool uninitialized3indices8Workaround =false; + bool uninitialized3indices8Workaround = false; - if(curPart->m_indices32) + if (curPart->m_indices32) { - uninitialized3indices8Workaround=true; + uninitialized3indices8Workaround = true; curNewPart->m_indices32 = new btIntIndexData[numIndices]; - memcpy(curNewPart->m_indices32,curPart->m_indices32,sizeof(btIntIndexData) * numIndices); + memcpy(curNewPart->m_indices32, curPart->m_indices32, sizeof(btIntIndexData) * numIndices); } else curNewPart->m_indices32 = NULL; - if(curPart->m_3indices16) + if (curPart->m_3indices16) { - uninitialized3indices8Workaround=true; + uninitialized3indices8Workaround = true; curNewPart->m_3indices16 = new btShortIntIndexTripletData[curNewPart->m_numTriangles]; - memcpy(curNewPart->m_3indices16,curPart->m_3indices16,sizeof(btShortIntIndexTripletData) * curNewPart->m_numTriangles); + memcpy(curNewPart->m_3indices16, curPart->m_3indices16, sizeof(btShortIntIndexTripletData) * curNewPart->m_numTriangles); } else curNewPart->m_3indices16 = NULL; - if(curPart->m_indices16) + if (curPart->m_indices16) { - uninitialized3indices8Workaround=true; + uninitialized3indices8Workaround = true; curNewPart->m_indices16 = new btShortIntIndexData[numIndices]; - memcpy(curNewPart->m_indices16,curPart->m_indices16,sizeof(btShortIntIndexData) * numIndices); + memcpy(curNewPart->m_indices16, curPart->m_indices16, sizeof(btShortIntIndexData) * numIndices); } else curNewPart->m_indices16 = NULL; - if(!uninitialized3indices8Workaround && curPart->m_3indices8) + if (!uninitialized3indices8Workaround && curPart->m_3indices8) { curNewPart->m_3indices8 = new btCharIndexTripletData[curNewPart->m_numTriangles]; - memcpy(curNewPart->m_3indices8,curPart->m_3indices8,sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles); + memcpy(curNewPart->m_3indices8, curPart->m_3indices8, sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles); } else curNewPart->m_3indices8 = NULL; - } m_allocatedbtStridingMeshInterfaceDatas.push_back(newData); - return(newData); + return (newData); } #ifdef USE_INTERNAL_EDGE_UTILITY -extern ContactAddedCallback gContactAddedCallback; +extern ContactAddedCallback gContactAddedCallback; -static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) +static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp, const btCollisionObject* colObj0, int partId0, int index0, const btCollisionObject* colObj1, int partId1, int index1) { - - btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1); - //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE); - //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED); + btAdjustInternalEdgeContacts(cp, colObj1, colObj0, partId1, index1); + //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE); + //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED); return true; } -#endif //USE_INTERNAL_EDGE_UTILITY - +#endif //USE_INTERNAL_EDGE_UTILITY /* btRigidBody* btWorldImporter::createRigidBody(bool isDynamic, btScalar mass, const btTransform& startTransform,btCollisionShape* shape,const char* bodyName) @@ -898,29 +852,27 @@ btCollisionObject* btCollisionWorldImporter::getCollisionObjectByName(const char return 0; } -btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform,btCollisionShape* shape, const char* bodyName) +btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform, btCollisionShape* shape, const char* bodyName) { btCollisionObject* colObj = new btCollisionObject(); colObj->setWorldTransform(startTransform); colObj->setCollisionShape(shape); - m_collisionWorld->addCollisionObject(colObj);//todo: flags etc + m_collisionWorld->addCollisionObject(colObj); //todo: flags etc if (bodyName) { char* newname = duplicateName(bodyName); - m_objectNameMap.insert(colObj,newname); - m_nameColObjMap.insert(newname,colObj); + m_objectNameMap.insert(colObj, newname); + m_nameColObjMap.insert(newname, colObj); } m_allocatedCollisionObjects.push_back(colObj); return colObj; } - - -btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal,btScalar planeConstant) +btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal, btScalar planeConstant) { - btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal,planeConstant); + btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal, planeConstant); m_allocatedCollisionShapes.push_back(shape); return shape; } @@ -937,85 +889,83 @@ btCollisionShape* btCollisionWorldImporter::createSphereShape(btScalar radius) return shape; } - btCollisionShape* btCollisionWorldImporter::createCapsuleShapeX(btScalar radius, btScalar height) { - btCapsuleShapeX* shape = new btCapsuleShapeX(radius,height); + btCapsuleShapeX* shape = new btCapsuleShapeX(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } btCollisionShape* btCollisionWorldImporter::createCapsuleShapeY(btScalar radius, btScalar height) { - btCapsuleShape* shape = new btCapsuleShape(radius,height); + btCapsuleShape* shape = new btCapsuleShape(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } btCollisionShape* btCollisionWorldImporter::createCapsuleShapeZ(btScalar radius, btScalar height) { - btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius,height); + btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius, btScalar height) { - btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height,radius,radius)); + btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height, radius, radius)); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius, btScalar height) { - btCylinderShape* shape = new btCylinderShape(btVector3(radius,height,radius)); + btCylinderShape* shape = new btCylinderShape(btVector3(radius, height, radius)); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius, btScalar height) { - btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius,radius,height)); + btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius, radius, height)); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius, btScalar height) { - btConeShapeX* shape = new btConeShapeX(radius,height); + btConeShapeX* shape = new btConeShapeX(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius, btScalar height) { - btConeShape* shape = new btConeShape(radius,height); + btConeShape* shape = new btConeShape(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius, btScalar height) { - btConeShapeZ* shape = new btConeShapeZ(radius,height); + btConeShapeZ* shape = new btConeShapeZ(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btTriangleIndexVertexArray* btCollisionWorldImporter::createTriangleMeshContainer() +btTriangleIndexVertexArray* btCollisionWorldImporter::createTriangleMeshContainer() { btTriangleIndexVertexArray* in = new btTriangleIndexVertexArray(); m_allocatedTriangleIndexArrays.push_back(in); return in; } -btOptimizedBvh* btCollisionWorldImporter::createOptimizedBvh() +btOptimizedBvh* btCollisionWorldImporter::createOptimizedBvh() { btOptimizedBvh* bvh = new btOptimizedBvh(); m_allocatedBvhs.push_back(bvh); return bvh; } - btTriangleInfoMap* btCollisionWorldImporter::createTriangleInfoMap() { btTriangleInfoMap* tim = new btTriangleInfoMap(); @@ -1027,16 +977,15 @@ btBvhTriangleMeshShape* btCollisionWorldImporter::createBvhTriangleMeshShape(btS { if (bvh) { - btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh,bvh->isQuantized(), false); + btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh, bvh->isQuantized(), false); bvhTriMesh->setOptimizedBvh(bvh); m_allocatedCollisionShapes.push_back(bvhTriMesh); return bvhTriMesh; } - btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh,true); + btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh, true); m_allocatedCollisionShapes.push_back(ts); return ts; - } btCollisionShape* btCollisionWorldImporter::createConvexTriangleMeshShape(btStridingMeshInterface* trimesh) { @@ -1048,9 +997,8 @@ btGImpactMeshShape* btCollisionWorldImporter::createGimpactShape(btStridingMeshI btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh); m_allocatedCollisionShapes.push_back(shape); return shape; - } -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT btConvexHullShape* btCollisionWorldImporter::createConvexHullShape() { @@ -1066,25 +1014,22 @@ btCompoundShape* btCollisionWorldImporter::createCompoundShape() return shape; } - -btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScaling) +btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape, const btVector3& localScaling) { - btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape,localScaling); + btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape, localScaling); m_allocatedCollisionShapes.push_back(shape); return shape; } -btMultiSphereShape* btCollisionWorldImporter::createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres) +btMultiSphereShape* btCollisionWorldImporter::createMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres) { btMultiSphereShape* shape = new btMultiSphereShape(positions, radi, numSpheres); m_allocatedCollisionShapes.push_back(shape); return shape; } - - - // query for data -int btCollisionWorldImporter::getNumCollisionShapes() const +// query for data +int btCollisionWorldImporter::getNumCollisionShapes() const { return m_allocatedCollisionShapes.size(); } @@ -1097,23 +1042,21 @@ btCollisionShape* btCollisionWorldImporter::getCollisionShapeByIndex(int index) btCollisionShape* btCollisionWorldImporter::getCollisionShapeByName(const char* name) { btCollisionShape** shapePtr = m_nameShapeMap.find(name); - if (shapePtr&& *shapePtr) + if (shapePtr && *shapePtr) { return *shapePtr; } return 0; } - -const char* btCollisionWorldImporter::getNameForPointer(const void* ptr) const +const char* btCollisionWorldImporter::getNameForPointer(const void* ptr) const { - const char*const * namePtr = m_objectNameMap.find(ptr); + const char* const* namePtr = m_objectNameMap.find(ptr); if (namePtr && *namePtr) return *namePtr; return 0; } - int btCollisionWorldImporter::getNumRigidBodies() const { return m_allocatedRigidBodies.size(); @@ -1124,12 +1067,11 @@ btCollisionObject* btCollisionWorldImporter::getRigidBodyByIndex(int index) cons return m_allocatedRigidBodies[index]; } - int btCollisionWorldImporter::getNumBvhs() const { return m_allocatedBvhs.size(); } - btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const +btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const { return m_allocatedBvhs[index]; } @@ -1143,5 +1085,3 @@ btTriangleInfoMap* btCollisionWorldImporter::getTriangleInfoMapByIndex(int index { return m_allocatedTriangleInfoMaps[index]; } - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h index 81c614272..5e8bc9534 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_COLLISION_WORLD_IMPORTER_H #define BT_COLLISION_WORLD_IMPORTER_H @@ -26,7 +25,6 @@ class btCollisionShape; class btCollisionObject; struct btBulletSerializedArrays; - struct ConstraintInput; class btCollisionWorld; struct btCollisionShapeData; @@ -46,9 +44,6 @@ class btSliderConstraint; class btGearConstraint; struct btContactSolverInfo; - - - class btCollisionWorldImporter { protected: @@ -56,60 +51,53 @@ protected: int m_verboseMode; - btAlignedObjectArray m_allocatedCollisionShapes; + btAlignedObjectArray m_allocatedCollisionShapes; btAlignedObjectArray m_allocatedRigidBodies; - btAlignedObjectArray m_allocatedBvhs; + btAlignedObjectArray m_allocatedBvhs; btAlignedObjectArray m_allocatedTriangleInfoMaps; btAlignedObjectArray m_allocatedTriangleIndexArrays; btAlignedObjectArray m_allocatedbtStridingMeshInterfaceDatas; btAlignedObjectArray m_allocatedCollisionObjects; + btAlignedObjectArray m_allocatedNames; - btAlignedObjectArray m_allocatedNames; + btAlignedObjectArray m_indexArrays; + btAlignedObjectArray m_shortIndexArrays; + btAlignedObjectArray m_charIndexArrays; - btAlignedObjectArray m_indexArrays; - btAlignedObjectArray m_shortIndexArrays; - btAlignedObjectArray m_charIndexArrays; + btAlignedObjectArray m_floatVertexArrays; + btAlignedObjectArray m_doubleVertexArrays; - btAlignedObjectArray m_floatVertexArrays; - btAlignedObjectArray m_doubleVertexArrays; + btHashMap m_bvhMap; + btHashMap m_timMap; + btHashMap m_nameShapeMap; + btHashMap m_nameColObjMap; - btHashMap m_bvhMap; - btHashMap m_timMap; - - btHashMap m_nameShapeMap; - btHashMap m_nameColObjMap; - - btHashMap m_objectNameMap; - - btHashMap m_shapeMap; - btHashMap m_bodyMap; + btHashMap m_objectNameMap; + btHashMap m_shapeMap; + btHashMap m_bodyMap; //methods + char* duplicateName(const char* name); - - char* duplicateName(const char* name); - - btCollisionShape* convertCollisionShape( btCollisionShapeData* shapeData ); - + btCollisionShape* convertCollisionShape(btCollisionShapeData* shapeData); public: - btCollisionWorldImporter(btCollisionWorld* world); virtual ~btCollisionWorldImporter(); - bool convertAllObjects( btBulletSerializedArrays* arrays); + bool convertAllObjects(btBulletSerializedArrays* arrays); - ///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load. + ///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load. ///make sure you don't use the dynamics world containing objects after you call this method virtual void deleteAllData(); - void setVerboseMode(int verboseMode) + void setVerboseMode(int verboseMode) { m_verboseMode = verboseMode; } @@ -119,14 +107,14 @@ public: return m_verboseMode; } - // query for data - int getNumCollisionShapes() const; + // query for data + int getNumCollisionShapes() const; btCollisionShape* getCollisionShapeByIndex(int index); int getNumRigidBodies() const; btCollisionObject* getRigidBodyByIndex(int index) const; int getNumBvhs() const; - btOptimizedBvh* getBvhByIndex(int index) const; + btOptimizedBvh* getBvhByIndex(int index) const; int getNumTriangleInfoMaps() const; btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const; @@ -134,56 +122,48 @@ public: btCollisionShape* getCollisionShapeByName(const char* name); btCollisionObject* getCollisionObjectByName(const char* name); - - const char* getNameForPointer(const void* ptr) const; + const char* getNameForPointer(const void* ptr) const; ///those virtuals are called by load and can be overridden by the user - - //bodies - virtual btCollisionObject* createCollisionObject( const btTransform& startTransform, btCollisionShape* shape,const char* bodyName); + virtual btCollisionObject* createCollisionObject(const btTransform& startTransform, btCollisionShape* shape, const char* bodyName); ///shapes - virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal,btScalar planeConstant); + virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal, btScalar planeConstant); virtual btCollisionShape* createBoxShape(const btVector3& halfExtents); virtual btCollisionShape* createSphereShape(btScalar radius); virtual btCollisionShape* createCapsuleShapeX(btScalar radius, btScalar height); virtual btCollisionShape* createCapsuleShapeY(btScalar radius, btScalar height); virtual btCollisionShape* createCapsuleShapeZ(btScalar radius, btScalar height); - virtual btCollisionShape* createCylinderShapeX(btScalar radius,btScalar height); - virtual btCollisionShape* createCylinderShapeY(btScalar radius,btScalar height); - virtual btCollisionShape* createCylinderShapeZ(btScalar radius,btScalar height); - virtual btCollisionShape* createConeShapeX(btScalar radius,btScalar height); - virtual btCollisionShape* createConeShapeY(btScalar radius,btScalar height); - virtual btCollisionShape* createConeShapeZ(btScalar radius,btScalar height); - virtual class btTriangleIndexVertexArray* createTriangleMeshContainer(); - virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh); + virtual btCollisionShape* createCylinderShapeX(btScalar radius, btScalar height); + virtual btCollisionShape* createCylinderShapeY(btScalar radius, btScalar height); + virtual btCollisionShape* createCylinderShapeZ(btScalar radius, btScalar height); + virtual btCollisionShape* createConeShapeX(btScalar radius, btScalar height); + virtual btCollisionShape* createConeShapeY(btScalar radius, btScalar height); + virtual btCollisionShape* createConeShapeZ(btScalar radius, btScalar height); + virtual class btTriangleIndexVertexArray* createTriangleMeshContainer(); + virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh); virtual btCollisionShape* createConvexTriangleMeshShape(btStridingMeshInterface* trimesh); #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT virtual btGImpactMeshShape* createGimpactShape(btStridingMeshInterface* trimesh); -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT virtual btStridingMeshInterfaceData* createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData); virtual class btConvexHullShape* createConvexHullShape(); virtual class btCompoundShape* createCompoundShape(); - virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScalingbtBvhTriangleMeshShape); + virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape, const btVector3& localScalingbtBvhTriangleMeshShape); - virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres); + virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres); virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData); ///acceleration and connectivity structures - virtual btOptimizedBvh* createOptimizedBvh(); + virtual btOptimizedBvh* createOptimizedBvh(); virtual btTriangleInfoMap* createTriangleInfoMap(); - - - - }; - -#endif //BT_WORLD_IMPORTER_H +#endif //BT_WORLD_IMPORTER_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 7f4dea1c6..633bee482 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -25,62 +25,58 @@ subject to the following restrictions: btShapePairCallback gCompoundChildShapePairCallback = 0; -btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_isSwapped(isSwapped), -m_sharedManifold(ci.m_manifold) +btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_isSwapped(isSwapped), + m_sharedManifold(ci.m_manifold) { m_ownsManifold = false; - const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; - btAssert (colObjWrap->getCollisionShape()->isCompound()); - + const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + btAssert(colObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape = static_cast(colObjWrap->getCollisionShape()); m_compoundShapeRevision = compoundShape->getUpdateRevision(); - - - preallocateChildAlgorithms(body0Wrap,body1Wrap); + + preallocateChildAlgorithms(body0Wrap, body1Wrap); } -void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) +void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap; - btAssert (colObjWrap->getCollisionShape()->isCompound()); - + const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap; + btAssert(colObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape = static_cast(colObjWrap->getCollisionShape()); int numChildren = compoundShape->getNumChildShapes(); int i; - + m_childCollisionAlgorithms.resize(numChildren); - for (i=0;igetDynamicAabbTree()) { m_childCollisionAlgorithms[i] = 0; - } else + } + else { - 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, BT_CONTACT_POINT_ALGORITHMS); - + 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, BT_CONTACT_POINT_ALGORITHMS); btAlignedObjectArray m_childCollisionAlgorithmsContact; btAlignedObjectArray m_childCollisionAlgorithmsClosestPoints; - - } } } -void btCompoundCollisionAlgorithm::removeChildAlgorithms() +void btCompoundCollisionAlgorithm::removeChildAlgorithms() { int numChildren = m_childCollisionAlgorithms.size(); int i; - for (i=0;i=0); + btAssert(index >= 0); const btCompoundShape* compoundShape = static_cast(m_compoundColObjWrap->getCollisionShape()); - btAssert(indexgetNumChildShapes()); - - - //backup - btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); - - const btTransform& childTrans = compoundShape->getChildTransform(index); - btTransform newChildWorldTrans = orgTrans*childTrans ; - - //perform an AABB check first - 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); + btAssert(index < compoundShape->getNumChildShapes()); if (gCompoundChildShapePairCallback) { @@ -150,16 +119,35 @@ public: return; } - if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) - { + //backup + btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); + + const btTransform& childTrans = compoundShape->getChildTransform(index); + btTransform newChildWorldTrans = orgTrans * childTrans; + + //perform an AABB check first + 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 (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) + { + btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, -1, index); - btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); - btCollisionAlgorithm* algo = 0; + bool allocatedAlgorithm = false; if (m_resultOut->m_closestPointDistanceThreshold > 0) { algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + allocatedAlgorithm = true; } else { @@ -170,7 +158,7 @@ public: } algo = m_childCollisionAlgorithms[index]; } - + const btCollisionObjectWrapper* tmpWrap = 0; ///detect swapping case @@ -178,15 +166,16 @@ public: { tmpWrap = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&compoundWrap); - m_resultOut->setShapeIdentifiersA(-1,index); - } else + m_resultOut->setShapeIdentifiersA(-1, index); + } + else { tmpWrap = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&compoundWrap); - m_resultOut->setShapeIdentifiersB(-1,index); + m_resultOut->setShapeIdentifiersB(-1, index); } - algo->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)) @@ -200,14 +189,19 @@ public: if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject()) { m_resultOut->setBody0Wrap(tmpWrap); - } else + } + else { m_resultOut->setBody1Wrap(tmpWrap); } - + if (allocatedAlgorithm) + { + algo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + } } } - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { int index = leaf->dataAsInt; @@ -224,22 +218,16 @@ public: } #endif - ProcessChildShape(childShape,index); - + ProcessChildShape(childShape, index); } }; - - - - - -void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap; + const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap; - btAssert (colObjWrap->getCollisionShape()->isCompound()); + btAssert(colObjWrap->getCollisionShape()->isCompound()); const btCompoundShape* compoundShape = static_cast(colObjWrap->getCollisionShape()); ///btCompoundShape might have changed: @@ -248,17 +236,17 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap { ///clear and update all removeChildAlgorithms(); - - preallocateChildAlgorithms(body0Wrap,body1Wrap); + + preallocateChildAlgorithms(body0Wrap, body1Wrap); m_compoundShapeRevision = compoundShape->getUpdateRevision(); } - if (m_childCollisionAlgorithms.size()==0) - return; - + 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); + btCompoundLeafCallback callback(colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, &m_childCollisionAlgorithms[0], m_sharedManifold); ///we need to refresh all contact manifolds ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep @@ -266,18 +254,18 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap { int i; manifoldArray.resize(0); - for (i=0;igetAllContactManifolds(manifoldArray); - for (int m=0;mgetNumContacts()) { resultOut->setPersistentManifold(manifoldArray[m]); resultOut->refreshContactPoints(); - resultOut->setPersistentManifold(0);//??necessary? + resultOut->setPersistentManifold(0); //??necessary? } } manifoldArray.resize(0); @@ -287,57 +275,56 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap if (tree) { - - btVector3 localAabbMin,localAabbMax; + btVector3 localAabbMin, localAabbMax; btTransform otherInCompoundSpace; otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform(); - otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); + 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); + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); //process all children, that overlap with the given AABB bounds - tree->collideTVNoStackAlloc(tree->m_root,bounds,stack2,callback); - - } else + tree->collideTVNoStackAlloc(tree->m_root, bounds, stack2, callback); + } + else { //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.size(); int i; - for (i=0;igetChildShape(i),i); + callback.ProcessChildShape(compoundShape->getChildShape(i), i); } } { - //iterate over all children, perform an AABB check inside ProcessChildShape + //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.size(); int i; manifoldArray.resize(0); - const btCollisionShape* childShape = 0; - btTransform orgTrans; - - btTransform newChildWorldTrans; - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - - for (i=0;igetChildShape(i); - //if not longer overlapping, remove the algorithm + //if not longer overlapping, remove the algorithm orgTrans = colObjWrap->getWorldTransform(); - + const btTransform& childTrans = compoundShape->getChildTransform(i); - newChildWorldTrans = orgTrans*childTrans ; + newChildWorldTrans = orgTrans * childTrans; //perform an AABB check first - childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); - otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); + childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0); + otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1); - if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); @@ -348,15 +335,15 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap } } -btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { btAssert(0); //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures - btCollisionObject* colObj = m_isSwapped? body1 : body0; - btCollisionObject* otherObj = m_isSwapped? body0 : body1; + btCollisionObject* colObj = m_isSwapped ? body1 : body0; + btCollisionObject* otherObj = m_isSwapped ? body0 : body1; + + btAssert(colObj->getCollisionShape()->isCompound()); - btAssert (colObj->getCollisionShape()->isCompound()); - btCompoundShape* compoundShape = static_cast(colObj->getCollisionShape()); //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps @@ -370,33 +357,29 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* int numChildren = m_childCollisionAlgorithms.size(); int i; - btTransform orgTrans; - btScalar frac; - for (i=0;igetChildShape(i); //backup - orgTrans = colObj->getWorldTransform(); - + orgTrans = colObj->getWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(i); //btTransform newChildWorldTrans = orgTrans*childTrans ; - colObj->setWorldTransform( orgTrans*childTrans ); + colObj->setWorldTransform(orgTrans * childTrans); //btCollisionShape* tmpShape = colObj->getCollisionShape(); //colObj->internalSetTemporaryCollisionShape( childShape ); - frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); - if (fraccalculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut); + if (frac < hitFraction) { hitFraction = frac; } //revert back //colObj->internalSetTemporaryCollisionShape( tmpShape); - colObj->setWorldTransform( orgTrans); + colObj->setWorldTransform(orgTrans); } return hitFraction; - } - - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index d2086fbc0..4ea5e7718 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -35,7 +35,7 @@ typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCol extern btShapePairCallback gCompoundChildShapePairCallback; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes -class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm +class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm { btNodeStack stack2; btManifoldArray manifoldArray; @@ -44,61 +44,56 @@ protected: btAlignedObjectArray m_childCollisionAlgorithms; bool m_isSwapped; - class btPersistentManifold* m_sharedManifold; - bool m_ownsManifold; + class btPersistentManifold* m_sharedManifold; + bool m_ownsManifold; + int m_compoundShapeRevision; //to keep track of changes, so that childAlgorithm array can be updated - int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated - - void removeChildAlgorithms(); - - void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + void removeChildAlgorithms(); + + void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); public: - - btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btCompoundCollisionAlgorithm(); - btCollisionAlgorithm* getChildAlgorithm (int n) const + btCollisionAlgorithm* getChildAlgorithm(int n) const { return m_childCollisionAlgorithms[n]; } + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { int i; - for (i=0;igetAllContactManifolds(manifoldArray); } } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); - return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + return new (mem) btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); - return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + return new (mem) btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_COMPOUND_COLLISION_ALGORITHM_H +#endif //BT_COMPOUND_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index d4a1aa78e..044b60dbb 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -29,29 +29,25 @@ subject to the following restrictions: btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; -btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped) +btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, isSwapped) { - - void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); - m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); + void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache), 16); + m_childCollisionAlgorithmCache = new (ptr) btHashedSimplePairCache(); const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; - btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + btAssert(col0ObjWrap->getCollisionShape()->isCompound()); const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; - btAssert (col1ObjWrap->getCollisionShape()->isCompound()); - + btAssert(col1ObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape0 = static_cast(col0ObjWrap->getCollisionShape()); m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); const btCompoundShape* compoundShape1 = static_cast(col1ObjWrap->getCollisionShape()); m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); - - } - btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() { removeChildAlgorithms(); @@ -59,32 +55,30 @@ btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() btAlignedFree(m_childCollisionAlgorithmCache); } -void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) { int i; btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - for (i=0;igetAllContactManifolds(manifoldArray); } } } - -void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() +void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() { btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); int numChildren = pairs.size(); int i; - for (i=0;i~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algo); } @@ -92,77 +86,65 @@ void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() m_childCollisionAlgorithmCache->removeAllPairs(); } -struct btCompoundCompoundLeafCallback : btDbvt::ICollide +struct btCompoundCompoundLeafCallback : btDbvt::ICollide { int m_numOverlapPairs; - const btCollisionObjectWrapper* m_compound0ColObjWrap; const btCollisionObjectWrapper* m_compound1ColObjWrap; btDispatcher* m_dispatcher; const btDispatcherInfo& m_dispatchInfo; - btManifoldResult* m_resultOut; - - - class btHashedSimplePairCache* m_childCollisionAlgorithmCache; - - btPersistentManifold* m_sharedManifold; - - btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap, - const btCollisionObjectWrapper* compound0ObjWrap, - btDispatcher* dispatcher, - const btDispatcherInfo& dispatchInfo, - btManifoldResult* resultOut, - btHashedSimplePairCache* childAlgorithmsCache, - btPersistentManifold* sharedManifold) - :m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), - m_childCollisionAlgorithmCache(childAlgorithmsCache), - m_sharedManifold(sharedManifold) - { + btManifoldResult* m_resultOut; + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + + btPersistentManifold* m_sharedManifold; + + btCompoundCompoundLeafCallback(const btCollisionObjectWrapper* compound1ObjWrap, + const btCollisionObjectWrapper* compound0ObjWrap, + btDispatcher* dispatcher, + const btDispatcherInfo& dispatchInfo, + btManifoldResult* resultOut, + btHashedSimplePairCache* childAlgorithmsCache, + btPersistentManifold* sharedManifold) + : m_numOverlapPairs(0), m_compound0ColObjWrap(compound1ObjWrap), m_compound1ColObjWrap(compound0ObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithmCache(childAlgorithmsCache), m_sharedManifold(sharedManifold) + { } - - - - void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) + void Process(const btDbvtNode* leaf0, const btDbvtNode* leaf1) { BT_PROFILE("btCompoundCompoundLeafCallback::Process"); m_numOverlapPairs++; - int childIndex0 = leaf0->dataAsInt; int childIndex1 = leaf1->dataAsInt; - - - btAssert(childIndex0>=0); - btAssert(childIndex1>=0); + btAssert(childIndex0 >= 0); + btAssert(childIndex1 >= 0); const btCompoundShape* compoundShape0 = static_cast(m_compound0ColObjWrap->getCollisionShape()); - btAssert(childIndex0getNumChildShapes()); + btAssert(childIndex0 < compoundShape0->getNumChildShapes()); const btCompoundShape* compoundShape1 = static_cast(m_compound1ColObjWrap->getCollisionShape()); - btAssert(childIndex1getNumChildShapes()); + btAssert(childIndex1 < compoundShape1->getNumChildShapes()); const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0); const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1); //backup - btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); + btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0); - btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ; - - btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); + btTransform newChildWorldTrans0 = orgTrans0 * childTrans0; + + btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1); - btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ; - + btTransform newChildWorldTrans1 = orgTrans1 * childTrans1; //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); - childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); - + btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1; + 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; @@ -170,29 +152,28 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide if (gCompoundCompoundChildShapePairCallback) { - if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) + if (!gCompoundCompoundChildShapePairCallback(childShape0, childShape1)) return; } - if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { - btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0); - btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1); - - - btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); + btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap, childShape0, m_compound0ColObjWrap->getCollisionObject(), newChildWorldTrans0, -1, childIndex0); + btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap, childShape1, m_compound1ColObjWrap->getCollisionObject(), newChildWorldTrans1, -1, childIndex1); + btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0, childIndex1); + bool removePair = false; btCollisionAlgorithm* colAlgo = 0; if (m_resultOut->m_closestPointDistanceThreshold > 0) { colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS); + removePair = true; } else { if (pair) { colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; - } else { @@ -204,7 +185,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide } btAssert(colAlgo); - + const btCollisionObjectWrapper* tmpWrap0 = 0; const btCollisionObjectWrapper* tmpWrap1 = 0; @@ -214,101 +195,100 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide m_resultOut->setBody0Wrap(&compoundWrap0); m_resultOut->setBody1Wrap(&compoundWrap1); - m_resultOut->setShapeIdentifiersA(-1,childIndex0); - m_resultOut->setShapeIdentifiersB(-1,childIndex1); + m_resultOut->setShapeIdentifiersA(-1, childIndex0); + m_resultOut->setShapeIdentifiersB(-1, childIndex1); + colAlgo->processCollision(&compoundWrap0, &compoundWrap1, m_dispatchInfo, m_resultOut); - colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut); - m_resultOut->setBody0Wrap(tmpWrap0); m_resultOut->setBody1Wrap(tmpWrap1); - - + if (removePair) + { + colAlgo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(colAlgo); + } } } }; - -static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold) +static DBVT_INLINE bool MyIntersect(const btDbvtAabbMm& a, + const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold) { - btVector3 newmin,newmax; - btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); + 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); + btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin, newmax); + return Intersect(a, newb); } - -static inline void MycollideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold) +static inline void MycollideTT(const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold) { - - if(root0&&root1) - { - int depth=1; - int treshold=btDbvt::DOUBLE_STACKSIZE-4; - btAlignedObjectArray stkStack; + if (root0 && root1) + { + 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); + ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]); + stkStack.initializeFromBuffer(&localStack, btDbvt::DOUBLE_STACKSIZE, btDbvt::DOUBLE_STACKSIZE); #else - stkStack.resize(btDbvt::DOUBLE_STACKSIZE); + 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, distanceThreshold)) + stkStack[0] = btDbvt::sStkNN(root0, root1); + do + { + btDbvt::sStkNN p = stkStack[--depth]; + if (MyIntersect(p.a->volume, p.b->volume, xform, distanceThreshold)) + { + if (depth > treshold) { - if(depth>treshold) + stkStack.resize(stkStack.size() * 2); + treshold = stkStack.size() - 4; + } + if (p.a->isinternal()) + { + if (p.b->isinternal()) { - stkStack.resize(stkStack.size()*2); - treshold=stkStack.size()-4; - } - if(p.a->isinternal()) - { - if(p.b->isinternal()) - { - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b); - } + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[0]); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[0]); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[1]); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[1]); } else { - if(p.b->isinternal()) - { - stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]); - } - else - { - callback->Process(p.a,p.b); - } + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b); } } - } while(depth); - } + else + { + if (p.b->isinternal()) + { + stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[0]); + stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[1]); + } + else + { + callback->Process(p.a, p.b); + } + } + } + } while (depth); + } } -void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btCompoundCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; - const btCollisionObjectWrapper* col1ObjWrap= body1Wrap; + const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; - btAssert (col0ObjWrap->getCollisionShape()->isCompound()); - btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + btAssert(col0ObjWrap->getCollisionShape()->isCompound()); + btAssert(col1ObjWrap->getCollisionShape()->isCompound()); const btCompoundShape* compoundShape0 = static_cast(col0ObjWrap->getCollisionShape()); const btCompoundShape* compoundShape1 = static_cast(col1ObjWrap->getCollisionShape()); @@ -316,7 +296,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); if (!tree0 || !tree1) { - return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut); + return btCompoundCollisionAlgorithm::processCollision(body0Wrap, body1Wrap, dispatchInfo, resultOut); } ///btCompoundShape might have changed: ////make sure the internal child collision algorithm caches are still valid @@ -326,28 +306,26 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb removeChildAlgorithms(); m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); - } - ///we need to refresh all contact manifolds ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { int i; btManifoldArray manifoldArray; -#ifdef USE_LOCAL_STACK +#ifdef USE_LOCAL_STACK btPersistentManifold localManifolds[4]; - manifoldArray.initializeFromBuffer(&localManifolds,0,4); + manifoldArray.initializeFromBuffer(&localManifolds, 0, 4); #endif btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - for (i=0;igetAllContactManifolds(manifoldArray); - for (int m=0;mgetNumContacts()) { @@ -361,96 +339,75 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb } } + btCompoundCompoundLeafCallback callback(col0ObjWrap, col1ObjWrap, this->m_dispatcher, dispatchInfo, resultOut, this->m_childCollisionAlgorithmCache, m_sharedManifold); - - - btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold); - - - const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform(); - MycollideTT(tree0->m_root,tree1->m_root,xform,&callback, resultOut->m_closestPointDistanceThreshold); + const btTransform xform = col0ObjWrap->getWorldTransform().inverse() * col1ObjWrap->getWorldTransform(); + MycollideTT(tree0->m_root, tree1->m_root, xform, &callback, resultOut->m_closestPointDistanceThreshold); //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); //remove non-overlapping child pairs { - btAssert(m_removePairs.size()==0); + btAssert(m_removePairs.size() == 0); //iterate over all children, perform an AABB check inside ProcessChildShape btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - - int i; - btManifoldArray manifoldArray; - - - - - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - - for (i=0;igetChildShape(pairs[i].m_indexA); - orgTrans0 = col0ObjWrap->getWorldTransform(); - orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); - newChildWorldTrans0 = orgTrans0*childTrans0 ; - childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + 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; + 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 ; - childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + newChildWorldTrans1 = col1ObjWrap->getWorldTransform() * childTrans1; + childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1); } - + aabbMin1 -= thresholdVec; aabbMax1 += thresholdVec; - if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { algo->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algo); - m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB)); + m_removePairs.push_back(btSimplePair(pairs[i].m_indexA, pairs[i].m_indexB)); } } } - for (int i=0;iremoveOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB); + m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA, m_removePairs[i].m_indexB); } m_removePairs.clear(); } - } -btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { btAssert(0); return 0.f; - } - - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h index f29f7a709..a940d840e 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -34,54 +34,49 @@ class btCollisionObject; class btCollisionShape; -/// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes -class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm -{ +extern btShapePairCallback gCompoundCompoundChildShapePairCallback; - class btHashedSimplePairCache* m_childCollisionAlgorithmCache; +/// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes +class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm +{ + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; btSimplePairArray m_removePairs; + int m_compoundShapeRevision0; //to keep track of changes, so that childAlgorithm array can be updated + int m_compoundShapeRevision1; - int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated - int m_compoundShapeRevision1; - - void removeChildAlgorithms(); - -// void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + void removeChildAlgorithms(); + + // void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); public: - - btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btCompoundCompoundCollisionAlgorithm(); - + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); - return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + return new (mem) btCompoundCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); - return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + return new (mem) btCompoundCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H +#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp index 1cb3d2e7a..9087f8439 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -22,7 +22,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h" - #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -34,8 +33,6 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" - - #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -45,31 +42,28 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) { m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; } -btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() -{ +btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() +{ } -btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_simplexSolver(simplexSolver), -m_pdSolver(pdSolver), -m_ownManifold (false), -m_manifoldPtr(mf), -m_lowLevelOfDetail(false) +btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_simplexSolver(simplexSolver), + m_pdSolver(pdSolver), + m_ownManifold(false), + m_manifoldPtr(mf), + m_lowLevelOfDetail(false) { (void)body0Wrap; (void)body1Wrap; } - - - btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm() { if (m_ownManifold) @@ -79,26 +73,22 @@ btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm() } } -void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel) { m_lowLevelOfDetail = useLowLevel; } - - extern btScalar gContactBreakingThreshold; - // // Convex-Convex collision algorithm // -void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvex2dConvex2dAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - if (!m_manifoldPtr) { //swapped? - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); @@ -106,49 +96,41 @@ void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrap //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); - const btConvexShape* min0 = static_cast(body0Wrap->getCollisionShape()); const btConvexShape* min1 = static_cast(body1Wrap->getCollisionShape()); - btVector3 normalOnB; - btVector3 pointOnBWorld; + btVector3 normalOnB; + btVector3 pointOnBWorld; { - - btGjkPairDetector::ClosestPointInput input; - btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); + btGjkPairDetector gjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA(min0); gjkPairDetector.setMinkowskiB(min1); { input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB = body1Wrap->getWorldTransform(); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); - btVector3 v0,v1; + btVector3 v0, v1; btVector3 sepNormalWorldSpace; - } if (m_ownManifold) { resultOut->refreshContactPoints(); } - } - - - -btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; @@ -158,7 +140,6 @@ btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* c ///col0->m_worldTransform, btScalar resultFraction = btScalar(1.); - btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); @@ -166,77 +147,65 @@ btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* c squareMot1 < col1->getCcdSquareMotionThreshold()) return resultFraction; - //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) - /// Convex0 against sphere for Convex1 { btConvexShape* convex0 = static_cast(col0->getCollisionShape()); - btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - if (col0->getHitFraction()> result.m_fraction) - col0->setHitFraction( result.m_fraction ); + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } - - - - } /// Sphere (for convex0) against Convex1 { btConvexShape* convex1 = static_cast(col1->getCollisionShape()); - btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - if (col0->getHitFraction() > result.m_fraction) - col0->setHitFraction( result.m_fraction); + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } } return resultFraction; - } - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h index 24d133677..9fca463fb 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -23,70 +23,61 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil class btConvexPenetrationDepthSolver; - ///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape ///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm { - btSimplexSolverInterface* m_simplexSolver; + btSimplexSolverInterface* m_simplexSolver; btConvexPenetrationDepthSolver* m_pdSolver; - - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_lowLevelOfDetail; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; + public: - - btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); - + btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); virtual ~btConvex2dConvex2dAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { ///should we use m_ownManifold to avoid adding duplicates? if (m_manifoldPtr && m_ownManifold) manifoldArray.push_back(m_manifoldPtr); } + void setLowLevelOfDetail(bool useLowLevel); - void setLowLevelOfDetail(bool useLowLevel); - - - const btPersistentManifold* getManifold() + const btPersistentManifold* getManifold() { return m_manifoldPtr; } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - - btConvexPenetrationDepthSolver* m_pdSolver; - btSimplexSolverInterface* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; + btSimplexSolverInterface* m_simplexSolver; int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); - + CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); + virtual ~CreateFunc(); - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm)); - return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + return new (mem) btConvex2dConvex2dAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_simplexSolver, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); } }; - - }; -#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H +#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 39ff7934d..e50f85e2b 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btConvexConcaveCollisionAlgorithm.h" #include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" @@ -27,11 +26,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_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped), -m_isSwapped(isSwapped) +btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped), + m_isSwapped(isSwapped) { } @@ -39,7 +39,7 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() { } -void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_btConvexTriangleCallback.m_manifoldPtr) { @@ -47,38 +47,32 @@ void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& } } - -btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped): - m_dispatcher(dispatcher), - m_dispatchInfoPtr(0) +btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher), + m_dispatchInfoPtr(0) { - m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap; - m_triBodyWrap = isSwapped? body0Wrap:body1Wrap; - - // - // create the manifold from the dispatcher 'manifold pool' - // - m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject()); + m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap; + m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap; - clearCache(); + // + // create the manifold from the dispatcher 'manifold pool' + // + m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(), m_triBodyWrap->getCollisionObject()); + + clearCache(); } btConvexTriangleCallback::~btConvexTriangleCallback() { clearCache(); - m_dispatcher->releaseManifold( m_manifoldPtr ); - + m_dispatcher->releaseManifold(m_manifoldPtr); } - -void btConvexTriangleCallback::clearCache() +void btConvexTriangleCallback::clearCache() { m_dispatcher->clearManifold(m_manifoldPtr); } - -void btConvexTriangleCallback::processTriangle(btVector3* triangle,int -partId, int triangleIndex) +void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { BT_PROFILE("btConvexTriangleCallback::processTriangle"); @@ -87,16 +81,12 @@ partId, int triangleIndex) return; } - //just for debugging purposes - //printf("triangle %d",m_triangleCount++); - - + //just for debugging purposes + //printf("triangle %d",m_triangleCount++); btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; - - #if 0 ///debug drawing of the overlapping triangles @@ -110,16 +100,15 @@ partId, int triangleIndex) m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); } #endif - + if (m_convexBodyWrap->getCollisionShape()->isConvex()) { - btTriangleShape tm(triangle[0],triangle[1],triangle[2]); + btTriangleShape tm(triangle[0], triangle[1], triangle[2]); tm.setMargin(m_collisionMarginTriangle); - - - btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform? + + btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex); //correct transform? btCollisionAlgorithm* colAlgo = 0; - + if (m_resultOut->m_closestPointDistanceThreshold > 0) { colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); @@ -134,36 +123,32 @@ partId, int triangleIndex) { tmpWrap = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersA(partId,triangleIndex); + m_resultOut->setShapeIdentifiersA(partId, triangleIndex); } else { tmpWrap = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersB(partId,triangleIndex); + m_resultOut->setShapeIdentifiersB(partId, triangleIndex); } - - colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut); + + colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut); if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) { m_resultOut->setBody0Wrap(tmpWrap); - } else + } + else { m_resultOut->setBody1Wrap(tmpWrap); } - - colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); } - } - - -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; @@ -177,66 +162,120 @@ 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+ resultOut->m_closestPointDistanceThreshold; - - 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() { m_btConvexTriangleCallback.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()) { - - - - const btConcaveShape* concaveShape = static_cast( triBodyWrap->getCollisionShape()); - - if (convexBodyWrap->getCollisionShape()->isConvex()) + if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE) { - btScalar collisionMarginTriangle = concaveShape->getMargin(); - - resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); - m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut); + btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape(); + if (convexBodyWrap->getCollisionShape()->isConvex()) + { + btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape(); + btAlignedObjectArray queryVertices; - m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject()); + 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; - concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); - - resultOut->refreshContactPoints(); + 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(); - m_btConvexTriangleCallback.clearWrapperData(); - + 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(); + } + } } - - } + else + { + 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); + + m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject()); + + concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax()); + + resultOut->refreshContactPoints(); + + m_btConvexTriangleCallback.clearWrapperData(); + } + } + } } - -btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; btCollisionObject* convexbody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; - //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) //only perform CCD above a certain threshold, this prevents blocking on the long run @@ -255,25 +294,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - struct LocalTriangleSphereCastCallback : public btTriangleCallback + struct LocalTriangleSphereCastCallback : public btTriangleCallback { btTransform m_ccdSphereFromTrans; btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; + btTransform m_meshTransform; - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; - + btScalar m_ccdSphereRadius; + btScalar m_hitFraction; - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) - :m_ccdSphereFromTrans(from), - m_ccdSphereToTrans(to), - m_ccdSphereRadius(ccdSphereRadius), - m_hitFraction(hitFraction) - { + LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction) + : m_ccdSphereFromTrans(from), + m_ccdSphereToTrans(to), + m_ccdSphereRadius(ccdSphereRadius), + m_hitFraction(hitFraction) + { } - - + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { BT_PROFILE("processTriangle"); @@ -284,29 +321,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj ident.setIdentity(); btConvexCast::CastResult castResult; castResult.m_fraction = m_hitFraction; - btSphereShape pointShape(m_ccdSphereRadius); - btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); + btSphereShape pointShape(m_ccdSphereRadius); + btTriangleShape triShape(triangle[0], triangle[1], triangle[2]); + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); //local space? - if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, - ident,ident,castResult)) + if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans, + ident, ident, castResult)) { if (m_hitFraction > castResult.m_fraction) m_hitFraction = castResult.m_fraction; } - } - }; - - - - if (triBody->getCollisionShape()->isConcave()) { btVector3 rayAabbMin = convexFromLocal.getOrigin(); @@ -314,33 +345,30 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj btVector3 rayAabbMax = convexFromLocal.getOrigin(); rayAabbMax.setMax(convexToLocal.getOrigin()); btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0); + rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0); - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->getCcdSweptSphereRadius(),curHitFraction); + btScalar curHitFraction = btScalar(1.); //is this available? + LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal, + convexbody->getCcdSweptSphereRadius(), curHitFraction); raycastCallback.m_hitFraction = convexbody->getHitFraction(); btCollisionObject* concavebody = triBody; - btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - + btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape(); + if (triangleMesh) { - triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); + triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax); } - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) { - convexbody->setHitFraction( raycastCallback.m_hitFraction); + convexbody->setHitFraction(raycastCallback.m_hitFraction); return raycastCallback.m_hitFraction; } } return btScalar(1.); - } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index 93d842ef5..b72e40298 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -26,42 +26,40 @@ 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. -ATTRIBUTE_ALIGNED16(class) btConvexTriangleCallback : public btTriangleCallback +ATTRIBUTE_ALIGNED16(class) +btConvexTriangleCallback : public btTriangleCallback { - - btVector3 m_aabbMin; - btVector3 m_aabbMax ; + btVector3 m_aabbMin; + btVector3 m_aabbMax; const btCollisionObjectWrapper* m_convexBodyWrap; const btCollisionObjectWrapper* m_triBodyWrap; - - btManifoldResult* m_resultOut; - btDispatcher* m_dispatcher; + btDispatcher* m_dispatcher; const btDispatcherInfo* m_dispatchInfoPtr; btScalar m_collisionMarginTriangle; - + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - -int m_triangleCount; - - btPersistentManifold* m_manifoldPtr; - btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + int m_triangleCount; - void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut); + btPersistentManifold* m_manifoldPtr; - void clearWrapperData() + btConvexTriangleCallback(btDispatcher * dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); + + void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut); + + void clearWrapperData() { m_convexBodyWrap = 0; m_triBodyWrap = 0; } virtual ~btConvexTriangleCallback(); - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - + virtual void processTriangle(btVector3 * triangle, int partId, int triangleIndex); + void clearCache(); SIMD_FORCE_INLINE const btVector3& getAabbMin() const @@ -72,56 +70,48 @@ int m_triangleCount; { return m_aabbMax; } - }; - - - /// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. -ATTRIBUTE_ALIGNED16(class) btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm +ATTRIBUTE_ALIGNED16(class) +btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm { - btConvexTriangleCallback m_btConvexTriangleCallback; - bool m_isSwapped; - - + bool m_isSwapped; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + + btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btConvexConcaveCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject * body0, btCollisionObject * body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - - void clearCache(); + virtual void getAllContactManifolds(btManifoldArray & manifoldArray); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + void clearCache(); + + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); - return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + return new (mem) btConvexConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); - return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + return new (mem) btConvexConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index b54bd4893..44dd3c553 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -28,8 +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" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -42,8 +41,6 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" - - #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -56,8 +53,6 @@ subject to the following restrictions: /////////// - - static SIMD_FORCE_INLINE void segmentsClosestPoints( btVector3& ptsVector, btVector3& offsetA, @@ -65,43 +60,49 @@ static SIMD_FORCE_INLINE void segmentsClosestPoints( btScalar& tA, btScalar& tB, const btVector3& translation, const btVector3& dirA, btScalar hlenA, - const btVector3& dirB, btScalar hlenB ) + const btVector3& dirB, btScalar hlenB) { // compute the parameters of the closest points on each line segment - btScalar dirA_dot_dirB = btDot(dirA,dirB); - btScalar dirA_dot_trans = btDot(dirA,translation); - btScalar dirB_dot_trans = btDot(dirB,translation); + btScalar dirA_dot_dirB = btDot(dirA, dirB); + btScalar dirA_dot_trans = btDot(dirA, translation); + btScalar dirB_dot_trans = btDot(dirB, translation); btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - if ( denom == 0.0f ) { + if (denom == 0.0f) + { tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) + } + else + { + tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom; + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } tB = tA * dirA_dot_dirB - dirB_dot_trans; - if ( tB < -hlenB ) { + if (tB < -hlenB) + { tB = -hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; - } else if ( tB > hlenB ) { + } + else if (tB > hlenB) + { tB = hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } @@ -113,19 +114,18 @@ static SIMD_FORCE_INLINE void segmentsClosestPoints( ptsVector = translation - offsetA + offsetB; } - static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( btVector3& normalOnB, btVector3& pointOnB, btScalar capsuleLengthA, - btScalar capsuleRadiusA, + btScalar capsuleRadiusA, btScalar capsuleLengthB, - btScalar capsuleRadiusB, + btScalar capsuleRadiusB, int capsuleAxisA, int capsuleAxisB, const btTransform& transformA, const btTransform& transformB, - btScalar distanceThreshold ) + btScalar distanceThreshold) { btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA); btVector3 translationA = transformA.getOrigin(); @@ -138,47 +138,38 @@ static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( // compute the closest points of the capsule line segments - btVector3 ptsVector; // the vector between the closest points - - btVector3 offsetA, offsetB; // offsets from segment centers to their closest points - btScalar tA, tB; // parameters on line segment + btVector3 ptsVector; // the vector between the closest points - segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation, - directionA, capsuleLengthA, directionB, capsuleLengthB ); + btVector3 offsetA, offsetB; // offsets from segment centers to their closest points + btScalar tA, tB; // parameters on line segment + + segmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, translation, + directionA, capsuleLengthA, directionB, capsuleLengthB); btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB; - if ( distance > distanceThreshold ) + if (distance > distanceThreshold) return distance; btScalar lenSqr = ptsVector.length2(); - if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON)) + if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON)) { //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA' btVector3 q; - btPlaneSpace1(directionA,normalOnB,q); - } else + btPlaneSpace1(directionA, normalOnB, q); + } + else { // compute the contact normal - normalOnB = ptsVector*-btRecipSqrt(lenSqr); + normalOnB = ptsVector * -btRecipSqrt(lenSqr); } - pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB; + pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB; return distance; } - - - - - - ////////// - - - - btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* pdSolver) { m_numPerturbationIterations = 0; @@ -186,30 +177,27 @@ btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* m_pdSolver = pdSolver; } -btConvexConvexAlgorithm::CreateFunc::~CreateFunc() -{ +btConvexConvexAlgorithm::CreateFunc::~CreateFunc() +{ } -btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_pdSolver(pdSolver), -m_ownManifold (false), -m_manifoldPtr(mf), -m_lowLevelOfDetail(false), +btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_pdSolver(pdSolver), + m_ownManifold(false), + m_manifoldPtr(mf), + m_lowLevelOfDetail(false), #ifdef USE_SEPDISTANCE_UTIL2 -m_sepDistance((static_cast(body0->getCollisionShape()))->getAngularMotionDisc(), - (static_cast(body1->getCollisionShape()))->getAngularMotionDisc()), + m_sepDistance((static_cast(body0->getCollisionShape()))->getAngularMotionDisc(), + (static_cast(body1->getCollisionShape()))->getAngularMotionDisc()), #endif -m_numPerturbationIterations(numPerturbationIterations), -m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) + m_numPerturbationIterations(numPerturbationIterations), + m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) { (void)body0Wrap; (void)body1Wrap; } - - - btConvexConvexAlgorithm::~btConvexConvexAlgorithm() { if (m_ownManifold) @@ -219,112 +207,105 @@ btConvexConvexAlgorithm::~btConvexConvexAlgorithm() } } -void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) { m_lowLevelOfDetail = useLowLevel; } - struct btPerturbedContactResult : public btManifoldResult { btManifoldResult* m_originalManifoldResult; btTransform m_transformA; btTransform m_transformB; - btTransform m_unPerturbedTransform; - bool m_perturbA; - btIDebugDraw* m_debugDrawer; + btTransform m_unPerturbedTransform; + bool m_perturbA; + btIDebugDraw* m_debugDrawer; - - btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer) - :m_originalManifoldResult(originalResult), - m_transformA(transformA), - m_transformB(transformB), - m_unPerturbedTransform(unPerturbedTransform), - m_perturbA(perturbA), - m_debugDrawer(debugDrawer) + btPerturbedContactResult(btManifoldResult* originalResult, const btTransform& transformA, const btTransform& transformB, const btTransform& unPerturbedTransform, bool perturbA, btIDebugDraw* debugDrawer) + : m_originalManifoldResult(originalResult), + m_transformA(transformA), + m_transformB(transformB), + m_unPerturbedTransform(unPerturbedTransform), + m_perturbA(perturbA), + m_debugDrawer(debugDrawer) { } - virtual ~ btPerturbedContactResult() + virtual ~btPerturbedContactResult() { } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth) + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar orgDepth) { - btVector3 endPt,startPt; + btVector3 endPt, startPt; btScalar newDepth; btVector3 newNormal; if (m_perturbA) { - btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth; - endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg); - newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); - startPt = endPt+normalOnBInWorld*newDepth; - } else + btVector3 endPtOrg = pointInWorld + normalOnBInWorld * orgDepth; + endPt = (m_unPerturbedTransform * m_transformA.inverse())(endPtOrg); + newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); + startPt = endPt - normalOnBInWorld * newDepth; + } + else { - endPt = pointInWorld + normalOnBInWorld*orgDepth; - startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld); - newDepth = (endPt - startPt).dot(normalOnBInWorld); - + endPt = pointInWorld + normalOnBInWorld * orgDepth; + startPt = (m_unPerturbedTransform * m_transformB.inverse())(pointInWorld); + newDepth = (endPt - startPt).dot(normalOnBInWorld); } //#define DEBUG_CONTACTS 1 #ifdef DEBUG_CONTACTS - m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0)); - m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0)); - m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1)); -#endif //DEBUG_CONTACTS + m_debugDrawer->drawLine(startPt, endPt, btVector3(1, 0, 0)); + m_debugDrawer->drawSphere(startPt, 0.05, btVector3(0, 1, 0)); + m_debugDrawer->drawSphere(endPt, 0.05, btVector3(0, 0, 1)); +#endif //DEBUG_CONTACTS - - m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth); + m_originalManifoldResult->addContactPoint(normalOnBInWorld, startPt, newDepth); } - }; extern btScalar gContactBreakingThreshold; - // // Convex-Convex collision algorithm // -void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - if (!m_manifoldPtr) { //swapped? - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); - const btConvexShape* min0 = static_cast(body0Wrap->getCollisionShape()); const btConvexShape* min1 = static_cast(body1Wrap->getCollisionShape()); - btVector3 normalOnB; - btVector3 pointOnBWorld; + btVector3 normalOnB; + btVector3 pointOnBWorld; #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; - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btCapsuleShape* capsuleA = (btCapsuleShape*)min0; + btCapsuleShape* capsuleB = (btCapsuleShape*)min1; - btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), - capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(), - body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - if (distgetHalfHeight(), capsuleA->getRadius(), + capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(), + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); + + if (dist < threshold) { - btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); - resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); } resultOut->refreshContactPoints(); return; @@ -334,19 +315,19 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* { //m_manifoldPtr->clearManifold(); - btCapsuleShape* capsuleA = (btCapsuleShape*) min0; - btSphereShape* capsuleB = (btSphereShape*) min1; - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btCapsuleShape* capsuleA = (btCapsuleShape*)min0; + btSphereShape* capsuleB = (btSphereShape*)min1; - btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), - 0.,capsuleB->getRadius(),capsuleA->getUpAxis(),1, - body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - if (distgetHalfHeight(), capsuleA->getRadius(), + 0., capsuleB->getRadius(), capsuleA->getUpAxis(), 1, + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); + + if (dist < threshold) { - btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); - resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); } resultOut->refreshContactPoints(); return; @@ -356,248 +337,336 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* { //m_manifoldPtr->clearManifold(); - btSphereShape* capsuleA = (btSphereShape*) min0; - btCapsuleShape* capsuleB = (btCapsuleShape*) min1; - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btSphereShape* capsuleA = (btSphereShape*)min0; + btCapsuleShape* capsuleB = (btCapsuleShape*)min1; - btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,0.,capsuleA->getRadius(), - capsuleB->getHalfHeight(),capsuleB->getRadius(),1,capsuleB->getUpAxis(), - body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - if (distgetRadius(), + capsuleB->getHalfHeight(), capsuleB->getRadius(), 1, capsuleB->getUpAxis(), + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); + + if (dist < threshold) { - btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); - resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); } resultOut->refreshContactPoints(); return; } -#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER - - - +#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); + m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(), body1->getWorldTransform()); } - if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) -#endif //USE_SEPDISTANCE_UTIL2 + if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f) +#endif //USE_SEPDISTANCE_UTIL2 { - - - btGjkPairDetector::ClosestPointInput input; - btVoronoiSimplexSolver simplexSolver; - btGjkPairDetector gjkPairDetector( min0, min1, &simplexSolver, m_pdSolver ); - //TODO: if (dispatchInfo.m_useContinuous) - gjkPairDetector.setMinkowskiA(min0); - gjkPairDetector.setMinkowskiB(min1); + btGjkPairDetector::ClosestPointInput input; + btVoronoiSimplexSolver simplexSolver; + btGjkPairDetector gjkPairDetector(min0, min1, &simplexSolver, m_pdSolver); + //TODO: if (dispatchInfo.m_useContinuous) + gjkPairDetector.setMinkowskiA(min0); + gjkPairDetector.setMinkowskiB(min1); #ifdef USE_SEPDISTANCE_UTIL2 - if (dispatchInfo.m_useConvexConservativeDistanceUtil) - { - input.m_maximumDistanceSquared = BT_LARGE_FLOAT; - } else -#endif //USE_SEPDISTANCE_UTIL2 - { - //if (dispatchInfo.m_convexMaxDistanceUseCPT) - //{ - // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); - //} else - //{ - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold()+resultOut->m_closestPointDistanceThreshold; -// } - - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; - } - - input.m_transformA = body0Wrap->getWorldTransform(); - input.m_transformB = body1Wrap->getWorldTransform(); - - - - - -#ifdef USE_SEPDISTANCE_UTIL2 - btScalar sepDist = 0.f; - if (dispatchInfo.m_useConvexConservativeDistanceUtil) - { - sepDist = gjkPairDetector.getCachedSeparatingDistance(); - if (sepDist>SIMD_EPSILON) + if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; - //now perturbe directions to get multiple contact points - + input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } - } -#endif //USE_SEPDISTANCE_UTIL2 - - if (min0->isPolyhedral() && min1->isPolyhedral()) - { - - - struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result + else +#endif //USE_SEPDISTANCE_UTIL2 { - 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) - { - } - }; + //if (dispatchInfo.m_convexMaxDistanceUseCPT) + //{ + // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); + //} else + //{ + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold() + resultOut->m_closestPointDistanceThreshold; + // } - - struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result + input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; + } + + input.m_transformA = body0Wrap->getWorldTransform(); + input.m_transformB = body1Wrap->getWorldTransform(); + +#ifdef USE_SEPDISTANCE_UTIL2 + btScalar sepDist = 0.f; + if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - btDiscreteCollisionDetectorInterface::Result* m_originalResult; - btVector3 m_reportedNormalOnWorld; - btScalar m_marginOnA; - btScalar m_marginOnB; - btScalar m_reportedDistance; - - bool m_foundResult; - btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB) - :m_originalResult(result), - m_marginOnA(marginOnA), - m_marginOnB(marginOnB), - m_foundResult(false) + sepDist = gjkPairDetector.getCachedSeparatingDistance(); + if (sepDist > SIMD_EPSILON) { + sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; + //now perturbe directions to get multiple contact points } - - virtual void setShapeIdentifiersA(int partId0,int index0){} - virtual void setShapeIdentifiersB(int partId1,int index1){} - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg) + } +#endif //USE_SEPDISTANCE_UTIL2 + + if (min0->isPolyhedral() && min1->isPolyhedral()) + { + struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result { - m_reportedDistance = depthOrg; - m_reportedNormalOnWorld = normalOnBInWorld; - - btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB; - m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB); - if (m_reportedDistance<0.f) + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + btScalar m_depth; + bool m_hasContact; + + btDummyResult() + : m_hasContact(false) { - m_foundResult = true; } - m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance); - } - }; - - btDummyResult dummy; - -///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it - - btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin(); - btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin(); - - btWithoutMarginResult withoutMargin(resultOut, min0Margin,min1Margin); - - btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0; - btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1; - if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) - { - - - - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); - - btScalar minDist = -1e30f; - btVector3 sepNormalWorldSpace; - bool foundSepAxis = true; - - if (dispatchInfo.m_enableSatConvex) - { - foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), - sepNormalWorldSpace,*resultOut); - } else - { -#ifdef ZERO_MARGIN - gjkPairDetector.setIgnoreMargin(true); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); -#else - - - gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw); - //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); -#endif //ZERO_MARGIN - //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - //if (l2>SIMD_EPSILON) + 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) { - sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); - minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin(); - -#ifdef ZERO_MARGIN - foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; -#else - foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin()); -#endif + m_hasContact = true; + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; } - } - if (foundSepAxis) + }; + + struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result { - -// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + btDiscreteCollisionDetectorInterface::Result* m_originalResult; + btVector3 m_reportedNormalOnWorld; + btScalar m_marginOnA; + btScalar m_marginOnB; + btScalar m_reportedDistance; - worldVertsB1.resize(0); - btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), minDist-threshold, threshold, worldVertsB1,worldVertsB2, - *resultOut); - - } - if (m_ownManifold) + bool m_foundResult; + btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB) + : m_originalResult(result), + m_marginOnA(marginOnA), + m_marginOnB(marginOnB), + m_foundResult(false) + { + } + + virtual void setShapeIdentifiersA(int partId0, int index0) {} + virtual void setShapeIdentifiersB(int partId1, int index1) {} + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorldOrg, btScalar depthOrg) + { + m_reportedDistance = depthOrg; + m_reportedNormalOnWorld = normalOnBInWorld; + + btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld * m_marginOnB; + m_reportedDistance = depthOrg + (m_marginOnA + m_marginOnB); + if (m_reportedDistance < 0.f) + { + m_foundResult = true; + } + m_originalResult->addContactPoint(normalOnBInWorld, adjustedPointB, m_reportedDistance); + } + }; + + btDummyResult dummy; + + ///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it + + btScalar min0Margin = min0->getShapeType() == BOX_SHAPE_PROXYTYPE ? 0.f : min0->getMargin(); + btScalar min1Margin = min1->getShapeType() == BOX_SHAPE_PROXYTYPE ? 0.f : min1->getMargin(); + + btWithoutMarginResult withoutMargin(resultOut, min0Margin, min1Margin); + + btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*)min0; + btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*)min1; + if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) { - resultOut->refreshContactPoints(); - } - return; - - } else - { - //we can also deal with convex versus triangle (without connectivity data) - if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) - { - - btVertexArray vertices; - 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]); - - //tri->initializePolyhedralFeatures(); - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; + btScalar minDist = -1e30f; btVector3 sepNormalWorldSpace; - btScalar minDist =-1e30f; - btScalar maxDist = threshold; - - bool foundSepAxis = false; - if (0) - { - polyhedronB->initializePolyhedralFeatures(); - foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), - sepNormalWorldSpace,*resultOut); - // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + bool foundSepAxis = true; - } else + if (dispatchInfo.m_enableSatConvex) + { + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), + sepNormalWorldSpace, *resultOut); + } + else { #ifdef ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); #else - gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); -#endif//ZERO_MARGIN - + + gjkPairDetector.getClosestPoints(input, withoutMargin, dispatchInfo.m_debugDraw); + //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); +#endif //ZERO_MARGIN \ + //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); \ + //if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld; //gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); + minDist = withoutMargin.m_reportedDistance; //gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin(); + +#ifdef ZERO_MARGIN + foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f; +#else + foundSepAxis = withoutMargin.m_foundResult && minDist < 0; //-(min0->getMargin()+min1->getMargin()); +#endif + } + } + if (foundSepAxis) + { + // 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, worldVertsB1, worldVertsB2, + *resultOut); + } + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + return; + } + else + { + //we can also deal with convex versus triangle (without connectivity data) + if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE) + { + btVertexArray worldSpaceVertices; + btTriangleShape* tri = (btTriangleShape*)polyhedronB; + 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(); + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; + + btVector3 sepNormalWorldSpace; + btScalar minDist = -1e30f; + btScalar maxDist = threshold; + + bool foundSepAxis = false; + bool useSatSepNormal = true; + + if (useSatSepNormal) + { +#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; v < combinedFaceA.m_indices.size(); v++) + { + btScalar eq = tri->m_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; v < combinedFaceB.m_indices.size(); v++) + { + btScalar eq = tri->m_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); + // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + } + else + { +#ifdef ZERO_MARGIN + gjkPairDetector.setIgnoreMargin(true); + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); +#else + 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) { @@ -607,144 +676,132 @@ 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, worldVertsB2,minDist-threshold, maxDist, *resultOut); - } - - - if (m_ownManifold) - { - resultOut->refreshContactPoints(); + if (foundSepAxis) + { + worldVertsB2.resize(0); + btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2, minDist - threshold, maxDist, *resultOut); + } + + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + + return; } - - return; } - } + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); - } - - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects - //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects - - //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points - if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) - { - - int i; - btVector3 v0,v1; - btVector3 sepNormalWorldSpace; - btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - - if (l2>SIMD_EPSILON) + //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points + if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - - btPlaneSpace1(sepNormalWorldSpace,v0,v1); + int i; + btVector3 v0, v1; + btVector3 sepNormalWorldSpace; + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + if (l2 > SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis() * (1.f / l2); - bool perturbeA = true; - const btScalar angleLimit = 0.125f * SIMD_PI; - btScalar perturbeAngle; - btScalar radiusA = min0->getAngularMotionDisc(); - btScalar radiusB = min1->getAngularMotionDisc(); - if (radiusA < radiusB) - { - perturbeAngle = gContactBreakingThreshold /radiusA; - perturbeA = true; - } else - { - perturbeAngle = gContactBreakingThreshold / radiusB; - perturbeA = false; - } - if ( perturbeAngle > angleLimit ) + btPlaneSpace1(sepNormalWorldSpace, v0, v1); + + bool perturbeA = true; + const btScalar angleLimit = 0.125f * SIMD_PI; + btScalar perturbeAngle; + btScalar radiusA = min0->getAngularMotionDisc(); + btScalar radiusB = min1->getAngularMotionDisc(); + if (radiusA < radiusB) + { + perturbeAngle = gContactBreakingThreshold / radiusA; + perturbeA = true; + } + else + { + perturbeAngle = gContactBreakingThreshold / radiusB; + perturbeA = false; + } + if (perturbeAngle > angleLimit) perturbeAngle = angleLimit; - btTransform unPerturbedTransform; - if (perturbeA) - { - unPerturbedTransform = input.m_transformA; - } else - { - unPerturbedTransform = input.m_transformB; - } - - for ( i=0;iSIMD_EPSILON) - { - btQuaternion perturbeRot(v0,perturbeAngle); - btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); - btQuaternion rotq(sepNormalWorldSpace,iterationAngle); - - + btTransform unPerturbedTransform; if (perturbeA) { - input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis()); - input.m_transformB = body1Wrap->getWorldTransform(); - #ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); - #endif //DEBUG_CONTACTS - } else - { - input.m_transformA = body0Wrap->getWorldTransform(); - input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis()); - #ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); - #endif + unPerturbedTransform = input.m_transformA; } - - btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); - gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + else + { + unPerturbedTransform = input.m_transformB; + } + + for (i = 0; i < m_numPerturbationIterations; i++) + { + if (v0.length2() > SIMD_EPSILON) + { + btQuaternion perturbeRot(v0, perturbeAngle); + btScalar iterationAngle = i * (SIMD_2_PI / btScalar(m_numPerturbationIterations)); + btQuaternion rotq(sepNormalWorldSpace, iterationAngle); + + if (perturbeA) + { + input.m_transformA.setBasis(btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body0Wrap->getWorldTransform().getBasis()); + input.m_transformB = body1Wrap->getWorldTransform(); +#ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformA, 10.0); +#endif //DEBUG_CONTACTS + } + else + { + input.m_transformA = body0Wrap->getWorldTransform(); + input.m_transformB.setBasis(btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1Wrap->getWorldTransform().getBasis()); +#ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformB, 10.0); +#endif + } + + btPerturbedContactResult perturbedResultOut(resultOut, input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input, perturbedResultOut, dispatchInfo.m_debugDraw); + } } } } - } - - #ifdef USE_SEPDISTANCE_UTIL2 - if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) - { - m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); - } -#endif //USE_SEPDISTANCE_UTIL2 - - + if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > SIMD_EPSILON)) + { + m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0->getWorldTransform(), body1->getWorldTransform()); + } +#endif //USE_SEPDISTANCE_UTIL2 } if (m_ownManifold) { resultOut->refreshContactPoints(); } - } - - bool disableCcd = false; -btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold - + ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0->m_worldTransform, btScalar resultFraction = btScalar(1.); - btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); - + if (squareMot0 < col0->getCcdSquareMotionThreshold() && squareMot1 < col1->getCcdSquareMotionThreshold()) return resultFraction; @@ -752,77 +809,65 @@ btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, if (disableCcd) return btScalar(1.); - //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) - /// Convex0 against sphere for Convex1 { btConvexShape* convex0 = static_cast(col0->getCollisionShape()); - btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - - if (col0->getHitFraction()> result.m_fraction) - col0->setHitFraction( result.m_fraction ); + + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } - - - - } /// Sphere (for convex0) against Convex1 { btConvexShape* convex1 = static_cast(col1->getCollisionShape()); - btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - - if (col0->getHitFraction() > result.m_fraction) - col0->setHitFraction( result.m_fraction); + + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } } - + return resultFraction; - } - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index cd75ba12d..eac5b4d82 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -23,7 +23,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" -#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil #include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h" class btConvexPenetrationDepthSolver; @@ -41,69 +41,61 @@ class btConvexPenetrationDepthSolver; class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm { #ifdef USE_SEPDISTANCE_UTIL2 - btConvexSeparatingDistanceUtil m_sepDistance; + btConvexSeparatingDistanceUtil m_sepDistance; #endif btConvexPenetrationDepthSolver* m_pdSolver; btVertexArray worldVertsB1; btVertexArray worldVertsB2; - - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_lowLevelOfDetail; - + + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; + int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - ///cache separating vector to speedup collision detection - public: - - btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, 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(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { ///should we use m_ownManifold to avoid adding duplicates? if (m_manifoldPtr && m_ownManifold) manifoldArray.push_back(m_manifoldPtr); } + void setLowLevelOfDetail(bool useLowLevel); - void setLowLevelOfDetail(bool useLowLevel); - - - const btPersistentManifold* getManifold() + const btPersistentManifold* getManifold() { return m_manifoldPtr; } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - - btConvexPenetrationDepthSolver* m_pdSolver; + btConvexPenetrationDepthSolver* m_pdSolver; int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; CreateFunc(btConvexPenetrationDepthSolver* pdSolver); - + virtual ~CreateFunc(); - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + 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_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + return new (mem) btConvexConvexAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); } }; - - }; -#endif //BT_CONVEX_CONVEX_ALGORITHM_H +#endif //BT_CONVEX_CONVEX_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index cce2d95bc..ba1bc06b6 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -23,25 +23,24 @@ subject to the following restrictions: //#include -btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold) -: btCollisionAlgorithm(ci), -m_ownManifold(false), -m_manifoldPtr(mf), -m_isSwapped(isSwapped), -m_numPerturbationIterations(numPerturbationIterations), -m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold) + : btCollisionAlgorithm(ci), + m_ownManifold(false), + m_manifoldPtr(mf), + m_isSwapped(isSwapped), + m_numPerturbationIterations(numPerturbationIterations), + m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) { - const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? col1Wrap : col0Wrap; - const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? col0Wrap : col1Wrap; + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? col0Wrap : col1Wrap; - if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject())) + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject()); m_ownManifold = true; } } - btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() { if (m_ownManifold) @@ -51,32 +50,32 @@ btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() } } -void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexPlaneCollisionAlgorithm::collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap; + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap; - btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); + btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape(); - bool hasCollision = false; + bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); - + btTransform convexWorldTransform = convexObjWrap->getWorldTransform(); btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform; + convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexWorldTransform; //now perturbe the convex-world transform - convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot); + convexWorldTransform.getBasis() *= btMatrix3x3(perturbeRot); btTransform planeInConvex; - planeInConvex= convexWorldTransform.inverse() * planeObjWrap->getWorldTransform(); - - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + planeInConvex = convexWorldTransform.inverse() * planeObjWrap->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); @@ -86,70 +85,69 @@ void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; btVector3 pOnB = vtxInPlaneWorld; - resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance); } } - -void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexPlaneCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)dispatchInfo; if (!m_manifoldPtr) return; - const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap; + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap; - btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); + btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape(); bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); btTransform planeInConvex; - planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform(); + planeInConvex = convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform(); btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform(); + convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform(); - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; - hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; resultOut->setPersistentManifold(m_manifoldPtr); if (hasCollision) { /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; btVector3 pOnB = vtxInPlaneWorld; - resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance); } //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones: //they keep on rolling forever because of the additional off-center contact points //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc) - if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { - btVector3 v0,v1; - btPlaneSpace1(planeNormal,v0,v1); + btVector3 v0, v1; + btPlaneSpace1(planeNormal, v0, v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radius = convexShape->getAngularMotionDisc(); perturbeAngle = gContactBreakingThreshold / radius; - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; + if (perturbeAngle > angleLimit) + perturbeAngle = angleLimit; - btQuaternion perturbeRot(v0,perturbeAngle); - for (int i=0;iallocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); if (!m_swapped) { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); - } else + return new (mem) btConvexPlaneCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); + } + else { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + return new (mem) btConvexPlaneCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); } } }; - }; -#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H - +#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index f6e4e57b0..ef3ea9e39 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -26,114 +26,108 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" - - #include "LinearMath/btPoolAllocator.h" - - - - btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) //btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool) { - - void* mem = NULL; + void* mem = NULL; if (constructionInfo.m_useEpaPenetrationAlgorithm) { - mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); - m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; - }else - { - mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); - m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; + mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver), 16); + m_pdSolver = new (mem) btGjkEpaPenetrationDepthSolver; } - + else + { + mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver), 16); + m_pdSolver = new (mem) btMinkowskiPenetrationDepthSolver; + } + //default CreationFunctions, filling the m_doubleDispatch table - mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16); - m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_pdSolver); - mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); - m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); - m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; - mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16); - m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc), 16); + 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); + m_swappedConvexConcaveCreateFunc = new (mem) btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc), 16); + m_compoundCreateFunc = new (mem) btCompoundCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16); - m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc), 16); + m_compoundCompoundCreateFunc = new (mem) btCompoundCompoundCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16); - m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; - mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); - m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16); - m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc), 16); + m_swappedCompoundCreateFunc = new (mem) btCompoundCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc), 16); + m_emptyCreateFunc = new (mem) btEmptyAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc), 16); + m_sphereSphereCF = new (mem) btSphereSphereCollisionAlgorithm::CreateFunc; #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM - mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); - m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); - m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc), 16); + m_sphereBoxCF = new (mem) btSphereBoxCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc), 16); + m_boxSphereCF = new (mem) btSphereBoxCollisionAlgorithm::CreateFunc; m_boxSphereCF->m_swapped = true; -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM - mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); - m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); - m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc), 16); + m_sphereTriangleCF = new (mem) btSphereTriangleCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc), 16); + m_triangleSphereCF = new (mem) btSphereTriangleCollisionAlgorithm::CreateFunc; m_triangleSphereCF->m_swapped = true; - - mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16); - m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc), 16); + m_boxBoxCF = new (mem) btBoxBoxCollisionAlgorithm::CreateFunc; //convex versus plane - mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + mem = btAlignedAlloc(sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc), 16); m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + mem = btAlignedAlloc(sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc), 16); m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; m_planeConvexCF->m_swapped = true; - + ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool int maxSize = sizeof(btConvexConvexAlgorithm); int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); int maxSize3 = sizeof(btCompoundCollisionAlgorithm); int maxSize4 = sizeof(btCompoundCompoundCollisionAlgorithm); - int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4); - + int collisionAlgorithmMaxElementSize = btMax(maxSize, constructionInfo.m_customCollisionAlgorithmMaxElementSize); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize2); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize3); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize4); + if (constructionInfo.m_persistentManifoldPool) { m_ownsPersistentManifoldPool = false; m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool; - } else + } + else { m_ownsPersistentManifoldPool = true; - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize); + void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16); + m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold), constructionInfo.m_defaultMaxPersistentManifoldPoolSize); } - - collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize+16)&0xffffffffffff0; + + collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize + 16) & 0xffffffffffff0; if (constructionInfo.m_collisionAlgorithmPool) { m_ownsCollisionAlgorithmPool = false; m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool; - } else + } + else { m_ownsCollisionAlgorithmPool = true; - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); + void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16); + m_collisionAlgorithmPool = new (mem) btPoolAllocator(collisionAlgorithmMaxElementSize, constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); } - - } btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() @@ -150,83 +144,78 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() } m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexConvexCreateFunc); + btAlignedFree(m_convexConvexCreateFunc); m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexConcaveCreateFunc); + btAlignedFree(m_convexConcaveCreateFunc); m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedConvexConcaveCreateFunc); + btAlignedFree(m_swappedConvexConcaveCreateFunc); m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_compoundCreateFunc); + btAlignedFree(m_compoundCreateFunc); m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); btAlignedFree(m_compoundCompoundCreateFunc); m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedCompoundCreateFunc); + btAlignedFree(m_swappedCompoundCreateFunc); m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_emptyCreateFunc); + btAlignedFree(m_emptyCreateFunc); m_sphereSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereSphereCF); + btAlignedFree(m_sphereSphereCF); #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM m_sphereBoxCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereBoxCF); + btAlignedFree(m_sphereBoxCF); m_boxSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_boxSphereCF); -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + btAlignedFree(m_boxSphereCF); +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereTriangleCF); + btAlignedFree(m_sphereTriangleCF); m_triangleSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_triangleSphereCF); + btAlignedFree(m_triangleSphereCF); m_boxBoxCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_boxBoxCF); + btAlignedFree(m_boxBoxCF); m_convexPlaneCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexPlaneCF); + btAlignedFree(m_convexPlaneCF); m_planeConvexCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_planeConvexCF); + btAlignedFree(m_planeConvexCF); m_pdSolver->~btConvexPenetrationDepthSolver(); - + btAlignedFree(m_pdSolver); - - } btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) { - - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_sphereSphereCF; + return m_sphereSphereCF; } #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) { - return m_sphereBoxCF; + return m_sphereBoxCF; } if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_boxSphereCF; + return m_boxSphereCF; } -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM - +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE)) { - return m_sphereTriangleCF; + return m_sphereTriangleCF; } if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_triangleSphereCF; + return m_triangleSphereCF; } if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) @@ -239,8 +228,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPoint return m_planeConvexCF; } - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) { return m_convexConvexCreateFunc; @@ -256,7 +243,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPoint return m_swappedConvexConcaveCreateFunc; } - if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) { return m_compoundCompoundCreateFunc; @@ -276,46 +262,41 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPoint //failed to find an algorithm return m_emptyCreateFunc; - } -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) { - - - - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_sphereSphereCF; + return m_sphereSphereCF; } #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) { - return m_sphereBoxCF; + return m_sphereBoxCF; } - if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_boxSphereCF; + return m_boxSphereCF; } -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM - - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE)) + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE)) { - return m_sphereTriangleCF; + return m_sphereTriangleCF; } - if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_triangleSphereCF; - } + return m_triangleSphereCF; + } if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) { return m_boxBoxCF; } - + if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) { return m_convexPlaneCF; @@ -325,8 +306,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg { return m_planeConvexCF; } - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) { @@ -343,7 +322,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg return m_swappedConvexConcaveCreateFunc; } - if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) { return m_compoundCompoundCreateFunc; @@ -352,7 +330,8 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg if (btBroadphaseProxy::isCompound(proxyType0)) { return m_compoundCreateFunc; - } else + } + else { if (btBroadphaseProxy::isCompound(proxyType1)) { @@ -366,17 +345,17 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) { - btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc; + btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*)m_convexConvexCreateFunc; convexConvex->m_numPerturbationIterations = numPerturbationIterations; convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; } -void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) +void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) { btConvexPlaneCollisionAlgorithm::CreateFunc* cpCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_convexPlaneCF; cpCF->m_numPerturbationIterations = numPerturbationIterations; cpCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; - + btConvexPlaneCollisionAlgorithm::CreateFunc* pcCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_planeConvexCF; pcCF->m_numPerturbationIterations = numPerturbationIterations; pcCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 17c7596cf..b39a3f41d 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -20,76 +20,68 @@ subject to the following restrictions: class btVoronoiSimplexSolver; class btConvexPenetrationDepthSolver; -struct btDefaultCollisionConstructionInfo +struct btDefaultCollisionConstructionInfo { - btPoolAllocator* m_persistentManifoldPool; - btPoolAllocator* m_collisionAlgorithmPool; - int m_defaultMaxPersistentManifoldPoolSize; - int m_defaultMaxCollisionAlgorithmPoolSize; - int m_customCollisionAlgorithmMaxElementSize; - int m_useEpaPenetrationAlgorithm; + btPoolAllocator* m_persistentManifoldPool; + btPoolAllocator* m_collisionAlgorithmPool; + int m_defaultMaxPersistentManifoldPoolSize; + int m_defaultMaxCollisionAlgorithmPoolSize; + int m_customCollisionAlgorithmMaxElementSize; + int m_useEpaPenetrationAlgorithm; btDefaultCollisionConstructionInfo() - :m_persistentManifoldPool(0), - m_collisionAlgorithmPool(0), - m_defaultMaxPersistentManifoldPoolSize(4096), - m_defaultMaxCollisionAlgorithmPoolSize(4096), - m_customCollisionAlgorithmMaxElementSize(0), - m_useEpaPenetrationAlgorithm(true) + : m_persistentManifoldPool(0), + m_collisionAlgorithmPool(0), + m_defaultMaxPersistentManifoldPoolSize(4096), + m_defaultMaxCollisionAlgorithmPoolSize(4096), + m_customCollisionAlgorithmMaxElementSize(0), + m_useEpaPenetrationAlgorithm(true) { } }; - - ///btCollisionConfiguration allows to configure Bullet collision detection ///stack allocator, pool memory allocators ///@todo: describe the meaning -class btDefaultCollisionConfiguration : public btCollisionConfiguration +class btDefaultCollisionConfiguration : public btCollisionConfiguration { - protected: + int m_persistentManifoldPoolSize; - int m_persistentManifoldPoolSize; - + btPoolAllocator* m_persistentManifoldPool; + bool m_ownsPersistentManifoldPool; - btPoolAllocator* m_persistentManifoldPool; - bool m_ownsPersistentManifoldPool; - - - btPoolAllocator* m_collisionAlgorithmPool; - bool m_ownsCollisionAlgorithmPool; + btPoolAllocator* m_collisionAlgorithmPool; + bool m_ownsCollisionAlgorithmPool; //default penetration depth solver - btConvexPenetrationDepthSolver* m_pdSolver; - + btConvexPenetrationDepthSolver* m_pdSolver; + //default CreationFunctions, filling the m_doubleDispatch table - btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; - btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; - btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc; - - btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc; + + btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; btCollisionAlgorithmCreateFunc* m_sphereSphereCF; btCollisionAlgorithmCreateFunc* m_sphereBoxCF; btCollisionAlgorithmCreateFunc* m_boxSphereCF; btCollisionAlgorithmCreateFunc* m_boxBoxCF; - btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; - btCollisionAlgorithmCreateFunc* m_triangleSphereCF; - btCollisionAlgorithmCreateFunc* m_planeConvexCF; - btCollisionAlgorithmCreateFunc* m_convexPlaneCF; - + btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; + btCollisionAlgorithmCreateFunc* m_triangleSphereCF; + btCollisionAlgorithmCreateFunc* m_planeConvexCF; + btCollisionAlgorithmCreateFunc* m_convexPlaneCF; + public: - - btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); virtual ~btDefaultCollisionConfiguration(); - ///memory pools + ///memory pools virtual btPoolAllocator* getPersistentManifoldPool() { return m_persistentManifoldPool; @@ -100,8 +92,7 @@ public: return m_collisionAlgorithmPool; } - - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1); virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1); @@ -112,11 +103,9 @@ public: ///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first. ///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points. ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection. - void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); - - void setPlaneConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); + void setConvexConvexMultipointIterations(int numPerturbationIterations = 3, int minimumPointsPerturbationThreshold = 3); + void setPlaneConvexMultipointIterations(int numPerturbationIterations = 3, int minimumPointsPerturbationThreshold = 3); }; -#endif //BT_DEFAULT_COLLISION_CONFIGURATION - +#endif //BT_DEFAULT_COLLISION_CONFIGURATION diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp index 5fa1c8be5..7cd41bdb3 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp @@ -15,20 +15,16 @@ subject to the following restrictions: #include "btEmptyCollisionAlgorithm.h" - - btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btCollisionAlgorithm(ci) { } -void btEmptyAlgorithm::processCollision (const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ,const btDispatcherInfo& ,btManifoldResult* ) +void btEmptyAlgorithm::processCollision(const btCollisionObjectWrapper*, const btCollisionObjectWrapper*, const btDispatcherInfo&, btManifoldResult*) { } -btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* ) +btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject*, btCollisionObject*, const btDispatcherInfo&, btManifoldResult*) { return btScalar(1.); } - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index cb0f15218..65ef83e09 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -25,30 +25,28 @@ subject to the following restrictions: ///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. class btEmptyAlgorithm : public btCollisionAlgorithm { - public: - btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + { (void)body0Wrap; (void)body1Wrap; void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm)); - return new(mem) btEmptyAlgorithm(ci); + return new (mem) btEmptyAlgorithm(ci); } }; } ATTRIBUTE_ALIGNED(16); -#endif //BT_EMPTY_ALGORITH +#endif //BT_EMPTY_ALGORITH diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.cpp index 86141fa68..00f16fd0a 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.cpp @@ -29,60 +29,58 @@ btGhostObject::~btGhostObject() btAssert(!m_overlappingObjects.size()); } - -void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy) { btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btAssert(otherObject); ///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index==m_overlappingObjects.size()) + if (index == m_overlappingObjects.size()) { //not found m_overlappingObjects.push_back(otherObject); } } -void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy) +void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy) { btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btAssert(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index~btHashedOverlappingPairCache(); - btAlignedFree( m_hashPairCache ); + btAlignedFree(m_hashPairCache); } -void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy) { - btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); + btBroadphaseProxy* actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); btAssert(actualThisProxy); btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btAssert(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index==m_overlappingObjects.size()) + if (index == m_overlappingObjects.size()) { m_overlappingObjects.push_back(otherObject); - m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); + m_hashPairCache->addOverlappingPair(actualThisProxy, otherProxy); } } -void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) +void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy1) { btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); @@ -90,82 +88,79 @@ void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy btAssert(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject); - if (indexremoveOverlappingPair(actualThisProxy,otherProxy,dispatcher); + m_hashPairCache->removeOverlappingPair(actualThisProxy, otherProxy, dispatcher); } } - -void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const { - btTransform convexFromTrans,convexToTrans; + btTransform convexFromTrans, convexToTrans; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; btVector3 castShapeAabbMin, castShapeAabbMax; /* Compute AABB that encompasses angular movement */ { btVector3 linVel, angVel; - btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); + btTransformUtil::calculateVelocity(convexFromTrans, convexToTrans, 1.0, linVel, angVel); btTransform R; - R.setIdentity (); - R.setRotation (convexFromTrans.getRotation()); - castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); + R.setIdentity(); + R.setRotation(convexFromTrans.getRotation()); + castShape->calculateTemporalAabb(R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); } /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) int i; - for (i=0;igetBroadphaseHandle())) { + if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); - btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 collisionObjectAabbMin, collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax); + AabbExpand(collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; - if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + if (btRayAabb(convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) { - btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); + btCollisionWorld::objectQuerySingle(castShape, convexFromTrans, convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); } } } - } -void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const +void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const { btTransform rayFromTrans; rayFromTrans.setIdentity(); rayFromTrans.setOrigin(rayFromWorld); - btTransform rayToTrans; + btTransform rayToTrans; rayToTrans.setIdentity(); rayToTrans.setOrigin(rayToWorld); - int i; - for (i=0;igetBroadphaseHandle())) + if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback); + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); } } } - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.h index 8ec861385..aa7f48d5c 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BT_GHOST_OBJECT_H #define BT_GHOST_OBJECT_H - #include "btCollisionObject.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" #include "LinearMath/btAlignedAllocator.h" @@ -31,48 +30,47 @@ class btDispatcher; ///By default, this overlap is based on the AABB ///This is useful for creating a character controller, collision sensors/triggers, explosions etc. ///We plan on adding rayTest and other queries for the btGhostObject -ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject +ATTRIBUTE_ALIGNED16(class) +btGhostObject : public btCollisionObject { protected: - btAlignedObjectArray m_overlappingObjects; public: - btGhostObject(); virtual ~btGhostObject(); - void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; + void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; - void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; ///this method is mainly for expert/internal use only. - virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + virtual void addOverlappingObjectInternal(btBroadphaseProxy * otherProxy, btBroadphaseProxy* thisProxy = 0); ///this method is mainly for expert/internal use only. - virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + virtual void removeOverlappingObjectInternal(btBroadphaseProxy * otherProxy, btDispatcher * dispatcher, btBroadphaseProxy* thisProxy = 0); - int getNumOverlappingObjects() const + int getNumOverlappingObjects() const { return m_overlappingObjects.size(); } - btCollisionObject* getOverlappingObject(int index) + btCollisionObject* getOverlappingObject(int index) { return m_overlappingObjects[index]; } - const btCollisionObject* getOverlappingObject(int index) const + const btCollisionObject* getOverlappingObject(int index) const { return m_overlappingObjects[index]; } - btAlignedObjectArray& getOverlappingPairs() + btAlignedObjectArray& getOverlappingPairs() { return m_overlappingObjects; } - const btAlignedObjectArray getOverlappingPairs() const + const btAlignedObjectArray getOverlappingPairs() const { return m_overlappingObjects; } @@ -81,49 +79,43 @@ public: // internal cast // - static const btGhostObject* upcast(const btCollisionObject* colObj) + static const btGhostObject* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()==CO_GHOST_OBJECT) + if (colObj->getInternalType() == CO_GHOST_OBJECT) return (const btGhostObject*)colObj; return 0; } - static btGhostObject* upcast(btCollisionObject* colObj) + static btGhostObject* upcast(btCollisionObject * colObj) { - if (colObj->getInternalType()==CO_GHOST_OBJECT) + if (colObj->getInternalType() == CO_GHOST_OBJECT) return (btGhostObject*)colObj; return 0; } - }; -class btPairCachingGhostObject : public btGhostObject +class btPairCachingGhostObject : public btGhostObject { - btHashedOverlappingPairCache* m_hashPairCache; + btHashedOverlappingPairCache* m_hashPairCache; public: - btPairCachingGhostObject(); virtual ~btPairCachingGhostObject(); ///this method is mainly for expert/internal use only. - virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy = 0); - virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy = 0); - btHashedOverlappingPairCache* getOverlappingPairCache() + btHashedOverlappingPairCache* getOverlappingPairCache() { return m_hashPairCache; } - }; - - ///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject. class btGhostPairCallback : public btOverlappingPairCallback { - public: btGhostPairCallback() { @@ -131,15 +123,14 @@ public: virtual ~btGhostPairCallback() { - } - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; - btGhostObject* ghost0 = btGhostObject::upcast(colObj0); - btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + btCollisionObject* colObj0 = (btCollisionObject*)proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); if (ghost0) ghost0->addOverlappingObjectInternal(proxy1, proxy0); if (ghost1) @@ -147,29 +138,25 @@ public: return 0; } - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher) { - btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; - btGhostObject* ghost0 = btGhostObject::upcast(colObj0); - btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + btCollisionObject* colObj0 = (btCollisionObject*)proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); if (ghost0) - ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0); + ghost0->removeOverlappingObjectInternal(proxy1, dispatcher, proxy0); if (ghost1) - ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1); + ghost1->removeOverlappingObjectInternal(proxy0, dispatcher, proxy1); return 0; } - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/, btDispatcher* /*dispatcher*/) { btAssert(0); //need to keep track of all ghost objects and call them here //m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher); } - - - }; #endif - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp index 8c8a7c3c1..b686d98d1 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -13,61 +13,49 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btHashedSimplePairCache.h" - #include -int gOverlappingSimplePairs = 0; -int gRemoveSimplePairs =0; -int gAddedSimplePairs =0; -int gFindSimplePairs =0; +#ifdef BT_DEBUG_COLLISION_PAIRS +int gOverlappingSimplePairs = 0; +int gRemoveSimplePairs = 0; +int gAddedSimplePairs = 0; +int gFindSimplePairs = 0; +#endif //BT_DEBUG_COLLISION_PAIRS - - - -btHashedSimplePairCache::btHashedSimplePairCache() { - int initialAllocatedSize= 2; +btHashedSimplePairCache::btHashedSimplePairCache() +{ + int initialAllocatedSize = 2; m_overlappingPairArray.reserve(initialAllocatedSize); growTables(); } - - - btHashedSimplePairCache::~btHashedSimplePairCache() { } - - - - - void btHashedSimplePairCache::removeAllPairs() { m_overlappingPairArray.clear(); m_hashTable.clear(); m_next.clear(); - int initialAllocatedSize= 2; + int initialAllocatedSize = 2; m_overlappingPairArray.reserve(initialAllocatedSize); growTables(); } - - btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gFindSimplePairs++; - - +#endif + /*if (indexA > indexB) btSwap(indexA, indexB);*/ - int hash = static_cast(getHash(static_cast(indexA), static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); + int hash = static_cast(getHash(static_cast(indexA), static_cast(indexB)) & (m_overlappingPairArray.capacity() - 1)); if (hash >= m_hashTable.size()) { @@ -92,9 +80,8 @@ btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) //#include -void btHashedSimplePairCache::growTables() +void btHashedSimplePairCache::growTables() { - int newCapacity = m_overlappingPairArray.capacity(); if (m_hashTable.size() < newCapacity) @@ -105,10 +92,9 @@ void btHashedSimplePairCache::growTables() m_hashTable.resize(newCapacity); m_next.resize(newCapacity); - int i; - for (i= 0; i < newCapacity; ++i) + for (i = 0; i < newCapacity; ++i) { m_hashTable[i] = BT_SIMPLE_NULL_PAIR; } @@ -117,27 +103,22 @@ void btHashedSimplePairCache::growTables() m_next[i] = BT_SIMPLE_NULL_PAIR; } - for(i=0;i(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + int hashValue = static_cast(getHash(static_cast(indexA), static_cast(indexB)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask m_next[i] = m_hashTable[hashValue]; m_hashTable[hashValue] = i; } - - } } btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) { - - int hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask - + int hash = static_cast(getHash(static_cast(indexA), static_cast(indexB)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask btSimplePair* pair = internalFindPair(indexA, indexB, hash); if (pair != NULL) @@ -155,30 +136,29 @@ btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) { growTables(); //hash with new capacity - hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); + hash = static_cast(getHash(static_cast(indexA), static_cast(indexB)) & (m_overlappingPairArray.capacity() - 1)); } - - pair = new (mem) btSimplePair(indexA,indexB); + + pair = new (mem) btSimplePair(indexA, indexB); pair->m_userPointer = 0; - + m_next[count] = m_hashTable[hash]; m_hashTable[hash] = count; return pair; } - - void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gRemoveSimplePairs++; - +#endif /*if (indexA > indexB) btSwap(indexA, indexB);*/ - int hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); + int hash = static_cast(getHash(static_cast(indexA), static_cast(indexB)) & (m_overlappingPairArray.capacity() - 1)); btSimplePair* pair = internalFindPair(indexA, indexB, hash); if (pair == NULL) @@ -186,10 +166,8 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) return 0; } - void* userData = pair->m_userPointer; - int pairIndex = int(pair - &m_overlappingPairArray[0]); btAssert(pairIndex < m_overlappingPairArray.size()); @@ -229,8 +207,8 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) // Remove the last pair from the hash table. const btSimplePair* last = &m_overlappingPairArray[lastPairIndex]; - /* missing swap here too, Nat. */ - int lastHash = static_cast(getHash(static_cast(last->m_indexA), static_cast(last->m_indexB)) & (m_overlappingPairArray.capacity()-1)); + /* missing swap here too, Nat. */ + int lastHash = static_cast(getHash(static_cast(last->m_indexA), static_cast(last->m_indexB)) & (m_overlappingPairArray.capacity() - 1)); index = m_hashTable[lastHash]; btAssert(index != BT_SIMPLE_NULL_PAIR); @@ -264,13 +242,3 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) return userData; } //#include - - - - - - - - - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h index 2aaf6201f..fd38a4f0e 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -16,142 +16,126 @@ subject to the following restrictions: #ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H #define BT_HASHED_SIMPLE_PAIR_CACHE_H - - #include "LinearMath/btAlignedObjectArray.h" -const int BT_SIMPLE_NULL_PAIR=0xffffffff; +const int BT_SIMPLE_NULL_PAIR = 0xffffffff; struct btSimplePair { - btSimplePair(int indexA,int indexB) - :m_indexA(indexA), - m_indexB(indexB), - m_userPointer(0) + btSimplePair(int indexA, int indexB) + : m_indexA(indexA), + m_indexB(indexB), + m_userPointer(0) { } int m_indexA; int m_indexB; - union - { - void* m_userPointer; - int m_userValue; + union { + void* m_userPointer; + int m_userValue; }; }; -typedef btAlignedObjectArray btSimplePairArray; - - +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 class btHashedSimplePairCache { - btSimplePairArray m_overlappingPairArray; - + btSimplePairArray m_overlappingPairArray; protected: - - btAlignedObjectArray m_hashTable; - btAlignedObjectArray m_next; - + btAlignedObjectArray m_hashTable; + btAlignedObjectArray m_next; public: btHashedSimplePairCache(); virtual ~btHashedSimplePairCache(); - + void removeAllPairs(); - virtual void* removeOverlappingPair(int indexA,int indexB); - + virtual void* removeOverlappingPair(int indexA, int indexB); + // 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 btSimplePair* addOverlappingPair(int indexA,int indexB) + virtual btSimplePair* addOverlappingPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gAddedSimplePairs++; +#endif - return internalAddPair(indexA,indexB); + return internalAddPair(indexA, indexB); } - - virtual btSimplePair* getOverlappingPairArrayPtr() + virtual btSimplePair* getOverlappingPairArrayPtr() { return &m_overlappingPairArray[0]; } - const btSimplePair* getOverlappingPairArrayPtr() const + const btSimplePair* getOverlappingPairArrayPtr() const { return &m_overlappingPairArray[0]; } - btSimplePairArray& getOverlappingPairArray() + btSimplePairArray& getOverlappingPairArray() { return m_overlappingPairArray; } - const btSimplePairArray& getOverlappingPairArray() const + const btSimplePairArray& getOverlappingPairArray() const { return m_overlappingPairArray; } - - btSimplePair* findPair(int indexA,int indexB); + btSimplePair* findPair(int indexA, int indexB); int GetCount() const { return m_overlappingPairArray.size(); } - int getNumOverlappingPairs() const + int getNumOverlappingPairs() const { return m_overlappingPairArray.size(); } -private: - - btSimplePair* internalAddPair(int indexA, int indexB); - void growTables(); +private: + btSimplePair* internalAddPair(int indexA, int indexB); + + void growTables(); SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB) - { + { return pair.m_indexA == indexA && pair.m_indexB == indexB; } - - SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) { unsigned int key = indexA | (indexB << 16); // Thomas Wang's hash key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); key += ~(key << 11); - key ^= (key >> 16); + key ^= (key >> 16); return key; } - - - - - SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash) + SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA, int proxyIdB, int hash) { - int index = m_hashTable[hash]; - - while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) + + while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) { index = m_next[index]; } - if ( index == BT_SIMPLE_NULL_PAIR ) + if (index == BT_SIMPLE_NULL_PAIR) { return NULL; } @@ -160,13 +144,6 @@ private: return &m_overlappingPairArray[index]; } - - }; - - - -#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H - - +#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index 6cba442ca..e74c83f9f 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -12,50 +12,44 @@ #ifdef DEBUG_INTERNAL_EDGE #include -#endif //DEBUG_INTERNAL_EDGE - +#endif //DEBUG_INTERNAL_EDGE #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW static btIDebugDraw* gDebugDrawer = 0; -void btSetDebugDrawer(btIDebugDraw* debugDrawer) +void btSetDebugDrawer(btIDebugDraw* debugDrawer) { gDebugDrawer = debugDrawer; } -static void btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color) +static void btDebugDrawLine(const btVector3& from, const btVector3& to, const btVector3& color) { if (gDebugDrawer) - gDebugDrawer->drawLine(from,to,color); + gDebugDrawer->drawLine(from, to, color); } -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - -static int btGetHash(int partId, int triangleIndex) +static int btGetHash(int partId, int triangleIndex) { - int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; + int hash = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex; return hash; } - - -static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB) +static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA, const btVector3& normalB) { - const btVector3 refAxis0 = edgeA; - const btVector3 refAxis1 = normalA; + const btVector3 refAxis0 = edgeA; + const btVector3 refAxis1 = normalA; const btVector3 swingAxis = normalB; btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); - return angle; + return angle; } - struct btConnectivityProcessor : public btTriangleCallback { - int m_partIdA; - int m_triangleIndexA; - btVector3* m_triangleVerticesA; - btTriangleInfoMap* m_triangleInfoMap; - + int m_partIdA; + int m_triangleIndexA; + btVector3* m_triangleVerticesA; + btTriangleInfoMap* m_triangleInfoMap; virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { @@ -69,18 +63,17 @@ struct btConnectivityProcessor : public btTriangleCallback //search for shared vertices and edges int numshared = 0; - int sharedVertsA[3]={-1,-1,-1}; - int sharedVertsB[3]={-1,-1,-1}; + int sharedVertsA[3] = {-1, -1, -1}; + int sharedVertsB[3] = {-1, -1, -1}; ///skip degenerate triangles - btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2(); + btScalar crossBSqr = ((triangle[1] - triangle[0]).cross(triangle[2] - triangle[0])).length2(); if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold) return; - - btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2(); + btScalar crossASqr = ((m_triangleVerticesA[1] - m_triangleVerticesA[0]).cross(m_triangleVerticesA[2] - m_triangleVerticesA[0])).length2(); ///skip degenerate triangles - if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold) + if (crossASqr < m_triangleInfoMap->m_equalVertexThreshold) return; #if 0 @@ -96,36 +89,36 @@ struct btConnectivityProcessor : public btTriangleCallback triangle[2].getX(),triangle[2].getY(),triangle[2].getZ()); #endif - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - for (int j=0;j<3;j++) + for (int j = 0; j < 3; j++) { - if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) + if ((m_triangleVerticesA[i] - triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) { sharedVertsA[numshared] = i; sharedVertsB[numshared] = j; numshared++; ///degenerate case - if(numshared >= 3) + if (numshared >= 3) return; } } ///degenerate case - if(numshared >= 3) + if (numshared >= 3) return; } switch (numshared) { - case 0: + case 0: { break; } - case 1: + case 1: { //shared vertex break; } - case 2: + case 2: { //shared edge //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct @@ -138,26 +131,25 @@ struct btConnectivityProcessor : public btTriangleCallback sharedVertsB[0] = tmp; } - int hash = btGetHash(m_partIdA,m_triangleIndexA); + int hash = btGetHash(m_partIdA, m_triangleIndexA); btTriangleInfo* info = m_triangleInfoMap->find(hash); if (!info) { btTriangleInfo tmp; - m_triangleInfoMap->insert(hash,tmp); + m_triangleInfoMap->insert(hash, tmp); info = m_triangleInfoMap->find(hash); } - int sumvertsA = sharedVertsA[0]+sharedVertsA[1]; - int otherIndexA = 3-sumvertsA; + int sumvertsA = sharedVertsA[0] + sharedVertsA[1]; + int otherIndexA = 3 - sumvertsA; - - btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]); + btVector3 edge(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]); - btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]); - int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]); + btTriangleShape tA(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]); + int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]); - btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]); + btTriangleShape tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]); //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); btVector3 normalA; @@ -168,26 +160,25 @@ struct btConnectivityProcessor : public btTriangleCallback btVector3 edgeCrossA = edge.cross(normalA).normalize(); { - btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]]; + btVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]]; if (edgeCrossA.dot(tmp) < 0) { - edgeCrossA*=-1; + edgeCrossA *= -1; } } btVector3 edgeCrossB = edge.cross(normalB).normalize(); { - btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]]; + btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]]; if (edgeCrossB.dot(tmp) < 0) { - edgeCrossB*=-1; + edgeCrossB *= -1; } } - btScalar angle2 = 0; - btScalar ang4 = 0.f; - + btScalar angle2 = 0; + btScalar ang4 = 0.f; btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB); btScalar len2 = calculatedEdge.length2(); @@ -196,52 +187,47 @@ struct btConnectivityProcessor : public btTriangleCallback //btVector3 calculatedNormalB = normalA; bool isConvex = false; - if (len2m_planarEpsilon) + if (len2 < m_triangleInfoMap->m_planarEpsilon) { angle2 = 0.f; ang4 = 0.f; - } else + } + else { - calculatedEdge.normalize(); btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA); calculatedNormalA.normalize(); - angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB); - ang4 = SIMD_PI-angle2; + angle2 = btGetAngle(calculatedNormalA, edgeCrossA, edgeCrossB); + ang4 = SIMD_PI - angle2; btScalar dotA = normalA.dot(edgeCrossB); ///@todo: check if we need some epsilon, due to floating point imprecision - isConvex = (dotA<0.); + isConvex = (dotA < 0.); correctedAngle = isConvex ? ang4 : -ang4; } - - - - - //alternatively use + //alternatively use //btVector3 calculatedNormalB2 = quatRotate(orn,normalA); - switch (sumvertsA) { - case 1: + case 1: { - btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); + btVector3 edge = m_triangleVerticesA[0] - m_triangleVerticesA[1]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); btScalar bla = computedNormalB.dot(normalB); - if (bla<0) + if (bla < 0) { - computedNormalB*=-1; + computedNormalB *= -1; info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif//DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV0V1Angle = -correctedAngle; @@ -249,44 +235,44 @@ struct btConnectivityProcessor : public btTriangleCallback info->m_flags |= TRI_INFO_V0V1_CONVEX; break; } - case 2: + case 2: { - btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); - if (computedNormalB.dot(normalB)<0) + btVector3 edge = m_triangleVerticesA[2] - m_triangleVerticesA[0]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); + if (computedNormalB.dot(normalB) < 0) { - computedNormalB*=-1; + computedNormalB *= -1; info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV2V0Angle = -correctedAngle; if (isConvex) info->m_flags |= TRI_INFO_V2V0_CONVEX; - break; + break; } - case 3: + case 3: { - btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); - if (computedNormalB.dot(normalB)<0) + btVector3 edge = m_triangleVerticesA[1] - m_triangleVerticesA[2]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); + if (computedNormalB.dot(normalB) < 0) { info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB; - computedNormalB*=-1; + computedNormalB *= -1; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV1V2Angle = -correctedAngle; if (isConvex) @@ -297,18 +283,17 @@ struct btConnectivityProcessor : public btTriangleCallback break; } - default: + default: { // printf("warning: duplicate triangle\n"); } - } } }; ///////////////////////////////////////////////////////// ///////////////////////////////////////////////////////// -void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap) +void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap) { //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! if (trimeshShape->getTriangleInfoMap()) @@ -319,46 +304,45 @@ void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangle btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface(); const btVector3& meshScaling = meshInterface->getScaling(); - for (int partId = 0; partId< meshInterface->getNumSubParts();partId++) + for (int partId = 0; partId < meshInterface->getNumSubParts(); partId++) { - const unsigned char *vertexbase = 0; + const unsigned char* vertexbase = 0; int numverts = 0; PHY_ScalarType type = PHY_INTEGER; int stride = 0; - const unsigned char *indexbase = 0; + const unsigned char* indexbase = 0; int indexstride = 0; int numfaces = 0; PHY_ScalarType indicestype = PHY_INTEGER; //PHY_ScalarType indexType=0; btVector3 triangleVerts[3]; - meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId); - btVector3 aabbMin,aabbMax; + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, partId); + btVector3 aabbMin, aabbMax; - for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) + for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++) { - unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride); + unsigned int* gfxbase = (unsigned int*)(indexbase + triangleIndex * indexstride); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); triangleVerts[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); + graphicsbase[0] * meshScaling.getX(), + graphicsbase[1] * meshScaling.getY(), + graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ())); } } - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangleVerts[0]); aabbMax.setMax(triangleVerts[0]); aabbMin.setMin(triangleVerts[1]); @@ -370,131 +354,127 @@ void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangle connectivityProcessor.m_partIdA = partId; connectivityProcessor.m_triangleIndexA = triangleIndex; connectivityProcessor.m_triangleVerticesA = &triangleVerts[0]; - connectivityProcessor.m_triangleInfoMap = triangleInfoMap; + connectivityProcessor.m_triangleInfoMap = triangleInfoMap; - trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax); + trimeshShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax); } - } - } - - - // Given a point and a line segment (defined by two points), compute the closest point // in the line. Cap the point at the endpoints of the line segment. -void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint) +void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint) { - btVector3 lineDelta = line1 - line0; + btVector3 lineDelta = line1 - line0; // Handle degenerate lines - if ( lineDelta.fuzzyZero()) + if (lineDelta.fuzzyZero()) { nearestPoint = line0; } else { - btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta); + btScalar delta = (point - line0).dot(lineDelta) / (lineDelta).dot(lineDelta); // Clamp the point to conform to the segment's endpoints - if ( delta < 0 ) + if (delta < 0) delta = 0; - else if ( delta > 1 ) + else if (delta > 1) delta = 1; - nearestPoint = line0 + lineDelta*delta; + nearestPoint = line0 + lineDelta * delta; } } - - - -bool btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal) +bool btClampNormal(const btVector3& edge, const btVector3& tri_normal_org, const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3& clampedLocalNormal) { btVector3 tri_normal = tri_normal_org; //we only have a local triangle normal, not a local contact normal -> only normal in world space... //either compute the current angle all in local space, or all in world space btVector3 edgeCross = edge.cross(tri_normal).normalize(); - btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB); + btScalar curAngle = btGetAngle(edgeCross, tri_normal, localContactNormalOnB); - if (correctedEdgeAngle<0) + if (correctedEdgeAngle < 0) { if (curAngle < correctedEdgeAngle) { - btScalar diffAngle = correctedEdgeAngle-curAngle; - btQuaternion rotation(edge,diffAngle ); - clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + btScalar diffAngle = correctedEdgeAngle - curAngle; + btQuaternion rotation(edge, diffAngle); + clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB; return true; } } - if (correctedEdgeAngle>=0) + if (correctedEdgeAngle >= 0) { if (curAngle > correctedEdgeAngle) { - btScalar diffAngle = correctedEdgeAngle-curAngle; - btQuaternion rotation(edge,diffAngle ); - clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + btScalar diffAngle = correctedEdgeAngle - curAngle; + btQuaternion rotation(edge, diffAngle); + clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB; return true; } } return false; } - - /// Changes a btManifoldPoint collision normal to the normal from the mesh. -void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags) +void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags) { //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE); if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) return; btBvhTriangleMeshShape* trimesh = 0; - - if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE ) - trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); - else - trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); - - btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); + + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); + } + else + { + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); + } + } + if (trimesh == 0) + return; + + btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap(); if (!triangleInfoMapPtr) return; - int hash = btGetHash(partId0,index0); - + int hash = btGetHash(partId0, index0); btTriangleInfo* info = triangleInfoMapPtr->find(hash); if (!info) return; - btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f; - + btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.f : -1.f; + const btTriangleShape* tri_shape = static_cast(colObj0Wrap->getCollisionShape()); - btVector3 v0,v1,v2; - tri_shape->getVertex(0,v0); - tri_shape->getVertex(1,v1); - tri_shape->getVertex(2,v2); + btVector3 v0, v1, v2; + tri_shape->getVertex(0, v0); + tri_shape->getVertex(1, v1); + tri_shape->getVertex(2, v2); //btVector3 center = (v0+v1+v2)*btScalar(1./3.); - btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0); + btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0); btVector3 tri_normal; tri_shape->calcNormal(tri_normal); //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB); btVector3 nearest; - btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest); + btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest); btVector3 contact = cp.m_localPointB; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW const btTransform& tr = colObj0->getWorldTransform(); - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, red); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW bool isNearEdge = false; @@ -502,334 +482,325 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr int numConvexEdgeHits = 0; btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - localContactNormalOnB.normalize();//is this necessary? - + localContactNormalOnB.normalize(); //is this necessary? + // Get closest edge - int bestedge=-1; - btScalar disttobestedge=BT_LARGE_FLOAT; + int bestedge = -1; + btScalar disttobestedge = BT_LARGE_FLOAT; // // Edge 0 -> 1 - if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=0; - disttobestedge=len; - } - } + if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 0; + disttobestedge = len; + } + } // Edge 1 -> 2 - if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=1; - disttobestedge=len; - } - } + if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v1, v2, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 1; + disttobestedge = len; + } + } // Edge 2 -> 0 - if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=2; - disttobestedge=len; - } - } - -#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f); - btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red ); -#endif - if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v2, v0, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 2; + disttobestedge = len; + } + } + #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); + btVector3 upfix = tri_normal * btVector3(0.1f, 0.1f, 0.1f); + btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red); #endif - btScalar len = (contact-nearest).length(); - if(lenm_edgeDistanceThreshold) - if( bestedge==0 ) - { - btVector3 edge(v0-v1); - isNearEdge = true; - - if (info->m_edgeV0V1Angle==btScalar(0)) - { - numConcaveEdgeHits++; - } else + if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 0) { + btVector3 edge(v0 - v1); + isNearEdge = true; - bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 nA = swapFactor * tri_normal; - - btQuaternion orn(edge,info->m_edgeV0V1Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; - - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotBm_convexEpsilon); - -#ifdef DEBUG_INTERNAL_EDGE - { - - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE - - - if (backFacingNormal) + if (info->m_edgeV0V1Angle == btScalar(0)) { numConcaveEdgeHits++; } else { - numConvexEdgeHits++; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal); - if (isClamped) + bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + + btQuaternion orn(edge, info->m_edgeV0V1Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; + + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); + +#ifdef DEBUG_INTERNAL_EDGE { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV0V1Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. (what about cp.m_distance1?) - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); - + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. (what about cp.m_distance1?) + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } } } - } } - btNearestPointInLineSegment(contact,v1,v2,nearest); + btNearestPointInLineSegment(contact, v1, v2, nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, green); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green ); -#endif + btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green); +#endif - if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - - btScalar len = (contact-nearest).length(); - if(lenm_edgeDistanceThreshold) - if( bestedge==1 ) - { - isNearEdge = true; + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 1) + { + isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 edge(v1-v2); + btVector3 edge(v1 - v2); - isNearEdge = true; + isNearEdge = true; - if (info->m_edgeV1V2Angle == btScalar(0)) - { - numConcaveEdgeHits++; - } else - { - bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0; - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 nA = swapFactor * tri_normal; - - btQuaternion orn(edge,info->m_edgeV1V2Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; - -#ifdef DEBUG_INTERNAL_EDGE - { - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE - - - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotBm_convexEpsilon); - - if (backFacingNormal) + if (info->m_edgeV1V2Angle == btScalar(0)) { numConcaveEdgeHits++; } else { - numConvexEdgeHits++; - btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal); - if (isClamped) + bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX) != 0; + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + + btQuaternion orn(edge, info->m_edgeV1V2Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; + +#ifdef DEBUG_INTERNAL_EDGE { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE + + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV1V2Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } } } - } } - btNearestPointInLineSegment(contact,v2,v0,nearest); + btNearestPointInLineSegment(contact, v2, v0, nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue ); -#endif + btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue); +#endif - if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - btScalar len = (contact-nearest).length(); - if(lenm_edgeDistanceThreshold) - if( bestedge==2 ) - { - isNearEdge = true; + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 2) + { + isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 edge(v2-v0); + btVector3 edge(v2 - v0); - if (info->m_edgeV2V0Angle==btScalar(0)) - { - numConcaveEdgeHits++; - } else - { - - bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0; - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 nA = swapFactor * tri_normal; - btQuaternion orn(edge,info->m_edgeV2V0Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; - -#ifdef DEBUG_INTERNAL_EDGE - { - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE - - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotBm_convexEpsilon); - - if (backFacingNormal) + if (info->m_edgeV2V0Angle == btScalar(0)) { numConcaveEdgeHits++; } else { - numConvexEdgeHits++; - // printf("hitting convex edge\n"); + bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX) != 0; + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btVector3 nA = swapFactor * tri_normal; + btQuaternion orn(edge, info->m_edgeV2V0Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; - btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal); - if (isClamped) +#ifdef DEBUG_INTERNAL_EDGE { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE + + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + // printf("hitting convex edge\n"); + + btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV2V0Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } - } + } } - - - } } #ifdef DEBUG_INTERNAL_EDGE { - btVector3 color(0,1,1); - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color); + btVector3 color(0, 1, 1); + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + cp.m_normalWorldOnB * 10, color); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE if (isNearEdge) { - - if (numConcaveEdgeHits>0) + if (numConcaveEdgeHits > 0) { - if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0) + if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0) { //fix tri_normal so it pointing the same direction as the current local contact normal if (tri_normal.dot(localContactNormalOnB) < 0) { tri_normal *= -1; } - cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal; - } else + cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * tri_normal; + } + else { - btVector3 newNormal = tri_normal *frontFacing; + btVector3 newNormal = tri_normal * frontFacing; //if the tri_normal is pointing opposite direction as the current local contact normal, skip it - btScalar d = newNormal.dot(localContactNormalOnB) ; - if (d< 0) + btScalar d = newNormal.dot(localContactNormalOnB); + if (d < 0) { return; } //modify the normal to be the triangle normal (or backfacing normal) - cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal; + cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * newNormal; } - + // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h index 7d9aafeee..9d9cff040 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h @@ -16,32 +16,26 @@ struct btCollisionObjectWrapper; class btManifoldPoint; class btIDebugDraw; - - enum btInternalEdgeAdjustFlags { BT_TRIANGLE_CONVEX_BACKFACE_MODE = 1, - BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended + BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended BT_TRIANGLE_CONVEX_DOUBLE_SIDED = 4 }; - ///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo' -void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap); - +void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap); ///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo) ///If this info map is missing, or the triangle is not store in this map, nothing will be done -void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap,const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0); +void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap, const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0); ///Enable the BT_INTERNAL_EDGE_DEBUG_DRAW define and call btSetDebugDrawer, to get visual info to see if the internal edge utility works properly. ///If the utility doesn't work properly, you might have to adjust the threshold values in btTriangleInfoMap //#define BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW -void btSetDebugDrawer(btIDebugDraw* debugDrawer); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - -#endif //BT_INTERNAL_EDGE_UTILITY_H +void btSetDebugDrawer(btIDebugDraw* debugDrawer); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW +#endif //BT_INTERNAL_EDGE_UTILITY_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index aa3d159f8..770eb2436 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -13,101 +13,102 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btManifoldResult.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" ///This is to allow MaterialCombiner/Custom Friction/Restitution values -ContactAddedCallback gContactAddedCallback=0; +ContactAddedCallback gContactAddedCallback = 0; +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 btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1) { btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction(); - const btScalar MAX_FRICTION = btScalar(10.); + const btScalar MAX_FRICTION = btScalar(10.); if (friction < -MAX_FRICTION) friction = -MAX_FRICTION; if (friction > MAX_FRICTION) friction = MAX_FRICTION; return friction; - } -btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1) +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; + 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) +btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1) { btScalar friction = body0->getFriction() * body1->getFriction(); - const btScalar MAX_FRICTION = btScalar(10.); + const btScalar MAX_FRICTION = btScalar(10.); if (friction < -MAX_FRICTION) friction = -MAX_FRICTION; if (friction > MAX_FRICTION) friction = MAX_FRICTION; return friction; - } -btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1) +btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1) { return body0->getRestitution() * body1->getRestitution(); } -btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1) +btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1) { - return body0->getContactDamping() + body1->getContactDamping(); + return body0->getContactDamping() + body1->getContactDamping(); } -btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1) +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; + 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) - :m_manifoldPtr(0), - m_body0Wrap(body0Wrap), - m_body1Wrap(body1Wrap) +btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + : m_manifoldPtr(0), + m_body0Wrap(body0Wrap), + m_body1Wrap(body1Wrap) #ifdef DEBUG_PART_INDEX - ,m_partId0(-1), - m_partId1(-1), - m_index0(-1), - m_index1(-1) -#endif //DEBUG_PART_INDEX - , m_closestPointDistanceThreshold(0) + , + m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX + , + m_closestPointDistanceThreshold(0) { } - -void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) +void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { btAssert(m_manifoldPtr); //order in manifold needs to match if (depth > m_manifoldPtr->getContactBreakingThreshold()) -// if (depth > m_manifoldPtr->getContactProcessingThreshold()) + // if (depth > m_manifoldPtr->getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); @@ -117,81 +118,82 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b btVector3 localA; btVector3 localB; - + if (isSwapped) { - localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } else + } + else { - localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); } - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth); newPt.m_positionWorldOnA = pointA; newPt.m_positionWorldOnB = pointInWorld; - + int insertIndex = m_manifoldPtr->getCacheEntry(newPt); - newPt.m_combinedFriction = calculateCombinedFriction(m_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_combinedSpinningFriction = calculateCombinedSpinningFriction(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 = calculateCombinedContactDamping(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); - newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); - newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING; - } + 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_FRICTION_ANCHOR) || - (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR)) - { - newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR; - } + 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; + } - btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2); - + 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; + } - - //BP mod, store contact triangles. + btPlaneSpace1(newPt.m_normalWorldOnB, newPt.m_lateralFrictionDir1, newPt.m_lateralFrictionDir2); + + //BP mod, store contact triangles. if (isSwapped) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; - newPt.m_index0 = m_index1; - newPt.m_index1 = m_index0; - } else + newPt.m_index0 = m_index1; + newPt.m_index1 = m_index0; + } + else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; - newPt.m_index0 = m_index0; - newPt.m_index1 = m_index1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; } //printf("depth=%f\n",depth); ///@todo, check this for any side effects if (insertIndex >= 0) { //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); - m_manifoldPtr->replaceContactPoint(newPt,insertIndex); - } else + m_manifoldPtr->replaceContactPoint(newPt, insertIndex); + } + else { insertIndex = m_manifoldPtr->addManifoldPoint(newPt); } - + //User can override friction and/or restitution if (gContactAddedCallback && //and if either of the two bodies requires custom material - ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || - (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) + ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) { //experimental feature info, for per-triangle material etc. - const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap; - const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap; - (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1); + const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap; + (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1); } if (gContactStartedCallback && isNewCollision) @@ -199,4 +201,3 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b gContactStartedCallback(m_manifoldPtr); } } - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 86bbc3f72..6c0a2d9a4 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_MANIFOLD_RESULT_H #define BT_MANIFOLD_RESULT_H @@ -29,76 +28,81 @@ class btManifoldPoint; #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" -typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1); -extern ContactAddedCallback gContactAddedCallback; +typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1); +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 { protected: - btPersistentManifold* m_manifoldPtr; const btCollisionObjectWrapper* m_body0Wrap; const btCollisionObjectWrapper* m_body1Wrap; - int m_partId0; + int m_partId0; int m_partId1; int m_index0; int m_index1; - - -public: +public: btManifoldResult() : #ifdef DEBUG_PART_INDEX - - m_partId0(-1), - m_partId1(-1), - m_index0(-1), - m_index1(-1) -#endif //DEBUG_PART_INDEX - m_closestPointDistanceThreshold(0) + + m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX + m_closestPointDistanceThreshold(0) { } - btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); - virtual ~btManifoldResult() {}; + virtual ~btManifoldResult(){}; - void setPersistentManifold(btPersistentManifold* manifoldPtr) + void setPersistentManifold(btPersistentManifold* manifoldPtr) { m_manifoldPtr = manifoldPtr; } - const btPersistentManifold* getPersistentManifold() const + const btPersistentManifold* getPersistentManifold() const { return m_manifoldPtr; } - btPersistentManifold* getPersistentManifold() + btPersistentManifold* getPersistentManifold() { return m_manifoldPtr; } - virtual void setShapeIdentifiersA(int partId0,int index0) + virtual void setShapeIdentifiersA(int partId0, int index0) { - m_partId0=partId0; - m_index0=index0; + m_partId0 = partId0; + m_index0 = index0; } - virtual void setShapeIdentifiersB( int partId1,int index1) + virtual void setShapeIdentifiersB(int partId1, int index1) { - m_partId1=partId1; - m_index1=index1; + m_partId1 = partId1; + m_index1 = index1; } + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth); - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); - - SIMD_FORCE_INLINE void refreshContactPoints() + SIMD_FORCE_INLINE void refreshContactPoints() { btAssert(m_manifoldPtr); if (!m_manifoldPtr->getNumContacts()) @@ -108,10 +112,11 @@ public: if (isSwapped) { - m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(),m_body0Wrap->getCollisionObject()->getWorldTransform()); - } else + m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(), m_body0Wrap->getCollisionObject()->getWorldTransform()); + } + else { - m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(),m_body1Wrap->getCollisionObject()->getWorldTransform()); + m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(), m_body1Wrap->getCollisionObject()->getWorldTransform()); } } @@ -144,15 +149,15 @@ public: return m_body1Wrap->getCollisionObject(); } - btScalar m_closestPointDistanceThreshold; + 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); + 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 +#endif //BT_MANIFOLD_RESULT_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 134478225..e5097ccbb 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -14,7 +14,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "LinearMath/btScalar.h" #include "btSimulationIslandManager.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" @@ -25,8 +24,7 @@ subject to the following restrictions: //#include #include "LinearMath/btQuickprof.h" -btSimulationIslandManager::btSimulationIslandManager(): -m_splitIslands(true) +btSimulationIslandManager::btSimulationIslandManager() : m_splitIslands(true) { } @@ -34,53 +32,47 @@ btSimulationIslandManager::~btSimulationIslandManager() { } - void btSimulationIslandManager::initUnionFind(int n) { - m_unionFind.reset(n); + m_unionFind.reset(n); } - -void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld) +void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */, btCollisionWorld* colWorld) { - { btOverlappingPairCache* pairCachePtr = colWorld->getPairCache(); const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs(); if (numOverlappingPairs) { - btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); - - for (int i=0;im_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); - if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && - ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + for (int i = 0; i < numOverlappingPairs; i++) { + const btBroadphasePair& collisionPair = pairPtr[i]; + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; - m_unionFind.unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + m_unionFind.unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); + } } } - } } } #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION -void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher) { - - // put the index into m_controllers into m_tag + // put the index into m_controllers into m_tag int index = 0; { - int i; - for (i=0;igetCollisionObjectArray().size(); i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; //Adding filtering here if (!collisionObject->isStaticOrKinematicObject()) { @@ -92,28 +84,29 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWor } // do the union find - initUnionFind( index ); + initUnionFind(index); - findUnions(dispatcher,colWorld); + findUnions(dispatcher, colWorld); } -void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) +void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) { - // put the islandId ('find' value) into m_tag + // put the islandId ('find' value) into m_tag { int index = 0; int i; - for (i=0;igetCollisionObjectArray().size();i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; if (!collisionObject->isStaticOrKinematicObject()) { - collisionObject->setIslandTag( m_unionFind.find(index) ); + collisionObject->setIslandTag(m_unionFind.find(index)); //Set the correct object offset in Collision Object Array m_unionFind.getElement(index).m_sz = i; collisionObject->setCompanionId(-1); index++; - } else + } + else { collisionObject->setIslandTag(-1); collisionObject->setCompanionId(-2); @@ -122,49 +115,44 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* c } } - -#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION -void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher) { + initUnionFind(int(colWorld->getCollisionObjectArray().size())); - initUnionFind( int (colWorld->getCollisionObjectArray().size())); - - // put the index into m_controllers into m_tag + // put the index into m_controllers into m_tag { - int index = 0; int i; - for (i=0;igetCollisionObjectArray().size(); i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; collisionObject->setIslandTag(index); collisionObject->setCompanionId(-1); collisionObject->setHitFraction(btScalar(1.)); index++; - } } // do the union find - findUnions(dispatcher,colWorld); + findUnions(dispatcher, colWorld); } -void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) +void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) { - // put the islandId ('find' value) into m_tag + // put the islandId ('find' value) into m_tag { - - int index = 0; int i; - for (i=0;igetCollisionObjectArray().size();i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; if (!collisionObject->isStaticOrKinematicObject()) { - collisionObject->setIslandTag( m_unionFind.find(index) ); + collisionObject->setIslandTag(m_unionFind.find(index)); collisionObject->setCompanionId(-1); - } else + } + else { collisionObject->setIslandTag(-1); collisionObject->setCompanionId(-2); @@ -174,56 +162,59 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col } } -#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION +#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION -inline int getIslandId(const btPersistentManifold* lhs) +inline int getIslandId(const btPersistentManifold* lhs) { int islandId; const btCollisionObject* rcolObj0 = static_cast(lhs->getBody0()); const btCollisionObject* rcolObj1 = static_cast(lhs->getBody1()); - islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag(); + islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag(); return islandId; - } - - /// function object that routes calls to operator< class btPersistentManifoldSortPredicate { - public: - - SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const - { - return getIslandId(lhs) < getIslandId(rhs); - } +public: + SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const + { + return getIslandId(lhs) < getIslandId(rhs); + } }; - -void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) +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) +{ BT_PROFILE("islandUnionFindAndQuickSort"); - + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); m_islandmanifold.resize(0); //we are going to sort the unionfind array, and store the element id in the size //afterwards, we clean unionfind, to make sure no-one uses it anymore - + getUnionFind().sortIslands(); int numElem = getUnionFind().getNumElements(); - int endIslandIndex=1; + 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"); + // printf("error in island management\n"); } 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; } } } - if (allSleeping) { int idx; - for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - colObj0->setActivationState( ISLAND_SLEEPING ); + colObj0->setActivationState(ISLAND_SLEEPING); } } - } else + } + else { - int idx; - for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - if ( colObj0->getActivationState() == ISLAND_SLEEPING) + if (colObj0->getActivationState() == ISLAND_SLEEPING) { - colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setActivationState(WANTS_DEACTIVATION); colObj0->setDeactivationTime(0.f); } } @@ -304,29 +292,30 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio } } - int i; int maxNumManifolds = dispatcher->getNumManifolds(); -//#define SPLIT_ISLANDS 1 -//#ifdef SPLIT_ISLANDS + //#define SPLIT_ISLANDS 1 + //#ifdef SPLIT_ISLANDS - -//#endif //SPLIT_ISLANDS + //#endif //SPLIT_ISLANDS - - for (i=0;igetManifoldByIndexInternal(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) || + btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(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()); + + ///@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) { @@ -338,36 +327,34 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio if (colObj1->hasContactResponse()) colObj0->activate(); } - if(m_splitIslands) - { + if (m_splitIslands) + { //filtering for response - if (dispatcher->needsResponse(colObj0,colObj1)) + if (dispatcher->needsResponse(colObj0, colObj1)) m_islandmanifold.push_back(manifold); } } } } - - ///@todo: this is random access, it can be walked 'cache friendly'! -void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback) +void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback) { btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - buildIslands(dispatcher,collisionWorld); + buildIslands(dispatcher, collisionWorld); - int endIslandIndex=1; + int endIslandIndex = 1; int startIslandIndex; int numElem = getUnionFind().getNumElements(); BT_PROFILE("processIslands"); - if(!m_splitIslands) + if (!m_splitIslands) { btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); int maxNumManifolds = dispatcher->getNumManifolds(); - callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); + callback->processIsland(&collisionObjects[0], collisionObjects.size(), manifold, maxNumManifolds, -1); } else { @@ -375,11 +362,21 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, // Sort the vector using predicate and std::sort //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); - int numManifolds = int (m_islandmanifold.size()); + int numManifolds = int(m_islandmanifold.size()); //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) @@ -389,55 +386,49 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, //int islandId; - - - // printf("Start Islands\n"); + // printf("Start Islands\n"); //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - for ( startIslandIndex=0;startIslandIndexisActive()) - islandSleeping = false; - } - + for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) + { + int i = getUnionFind().getElement(endIslandIndex).m_sz; + btCollisionObject* colObj0 = collisionObjects[i]; + m_islandBodies.push_back(colObj0); + if (colObj0->isActive()) + islandSleeping = false; + } //find the accompanying contact manifold for this islandId int numIslandManifolds = 0; btPersistentManifold** startManifold = 0; - if (startManifoldIndexprocessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); - // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); + callback->processIsland(&m_islandBodies[0], m_islandBodies.size(), startManifold, numIslandManifolds, islandId); + // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); } - + if (numIslandManifolds) { startManifoldIndex = endManifoldIndex; @@ -445,6 +436,5 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, m_islandBodies.resize(0); } - } // else if(!splitIslands) - + } // else if(!splitIslands) } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index e24c6afec..6c2802141 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -26,45 +26,39 @@ class btCollisionWorld; class btDispatcher; class btPersistentManifold; - ///SimulationIslandManager creates and handles simulation islands, using btUnionFind class btSimulationIslandManager { btUnionFind m_unionFind; - btAlignedObjectArray m_islandmanifold; - btAlignedObjectArray m_islandBodies; - + btAlignedObjectArray m_islandmanifold; + btAlignedObjectArray m_islandBodies; + bool m_splitIslands; - + public: btSimulationIslandManager(); virtual ~btSimulationIslandManager(); + void initUnionFind(int n); - void initUnionFind(int n); - - - btUnionFind& getUnionFind() { return m_unionFind;} + btUnionFind& getUnionFind() { return m_unionFind; } - virtual void updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher); - virtual void storeIslandActivationState(btCollisionWorld* world); + virtual void updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher); + virtual void storeIslandActivationState(btCollisionWorld* world); + void findUnions(btDispatcher* dispatcher, btCollisionWorld* colWorld); - void findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld); - - - - struct IslandCallback + struct IslandCallback { - virtual ~IslandCallback() {}; + virtual ~IslandCallback(){}; - virtual void processIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; + virtual void processIsland(btCollisionObject** bodies, int numBodies, class btPersistentManifold** manifolds, int numManifolds, int islandId) = 0; }; - void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback); + void buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback); - void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); + void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld); bool getSplitIslands() { @@ -74,8 +68,6 @@ public: { m_splitIslands = doSplitIslands; } - }; -#endif //BT_SIMULATION_ISLAND_MANAGER_H - +#endif //BT_SIMULATION_ISLAND_MANAGER_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index e8b567e0e..bc68b285b 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -21,23 +21,22 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" //#include -btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped) -: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf), -m_isSwapped(isSwapped) +btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped) + : btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf), + m_isSwapped(isSwapped) { - const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? col1Wrap : col0Wrap; - const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? col0Wrap : col1Wrap; - - if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject())) + const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? col0Wrap : col1Wrap; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject()); m_ownManifold = true; } } - btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() { if (m_ownManifold) @@ -47,17 +46,15 @@ btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() } } - - -void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) +void btSphereBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)dispatchInfo; (void)resultOut; if (!m_manifoldPtr) return; - const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap; + const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? body0Wrap : body1Wrap; btVector3 pOnBox; @@ -83,10 +80,9 @@ void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWra resultOut->refreshContactPoints(); } } - } -btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; @@ -97,27 +93,26 @@ btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* return btScalar(1.); } - -bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) +bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance) { - const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape(); - btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin(); + const btBoxShape* boxShape = (const btBoxShape*)boxObjWrap->getCollisionShape(); + btVector3 const& boxHalfExtent = boxShape->getHalfExtentsWithoutMargin(); btScalar boxMargin = boxShape->getMargin(); penetrationDepth = 1.0f; // convert the sphere position to the box's local space - btTransform const &m44T = boxObjWrap->getWorldTransform(); + btTransform const& m44T = boxObjWrap->getWorldTransform(); btVector3 sphereRelPos = m44T.invXform(sphereCenter); // Determine the closest point to the sphere center in the box btVector3 closestPoint = sphereRelPos; - closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) ); - closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) ); - closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) ); - closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) ); - closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) ); - closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) ); - + closestPoint.setX(btMin(boxHalfExtent.getX(), closestPoint.getX())); + closestPoint.setX(btMax(-boxHalfExtent.getX(), closestPoint.getX())); + closestPoint.setY(btMin(boxHalfExtent.getY(), closestPoint.getY())); + closestPoint.setY(btMax(-boxHalfExtent.getY(), closestPoint.getY())); + closestPoint.setZ(btMin(boxHalfExtent.getZ(), closestPoint.getZ())); + closestPoint.setZ(btMax(-boxHalfExtent.getZ(), closestPoint.getZ())); + btScalar intersectionDist = fRadius + boxMargin; btScalar contactDist = intersectionDist + maxContactDistance; normal = sphereRelPos - closestPoint; @@ -136,42 +131,42 @@ bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWra { distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal); } - else //compute the penetration details + else //compute the penetration details { distance = normal.length(); normal /= distance; } pointOnBox = closestPoint + normal * boxMargin; -// v3PointOnSphere = sphereRelPos - (normal * fRadius); + // v3PointOnSphere = sphereRelPos - (normal * fRadius); penetrationDepth = distance - intersectionDist; // transform back in world space btVector3 tmp = m44T(pointOnBox); pointOnBox = tmp; -// tmp = m44T(v3PointOnSphere); -// v3PointOnSphere = tmp; + // tmp = m44T(v3PointOnSphere); + // v3PointOnSphere = tmp; tmp = m44T.getBasis() * normal; normal = tmp; return true; } -btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ) +btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal) { //project the center of the sphere on the closest face of the box btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX(); btScalar minDist = faceDist; - closestPoint.setX( boxHalfExtent.getX() ); - normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f)); + closestPoint.setX(boxHalfExtent.getX()); + normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f)); faceDist = boxHalfExtent.getX() + sphereRelPos.getX(); if (faceDist < minDist) { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setX( -boxHalfExtent.getX() ); - normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f)); + closestPoint.setX(-boxHalfExtent.getX()); + normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f)); } faceDist = boxHalfExtent.getY() - sphereRelPos.getY(); @@ -179,8 +174,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setY( boxHalfExtent.getY() ); - normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f)); + closestPoint.setY(boxHalfExtent.getY()); + normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f)); } faceDist = boxHalfExtent.getY() + sphereRelPos.getY(); @@ -188,8 +183,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setY( -boxHalfExtent.getY() ); - normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f)); + closestPoint.setY(-boxHalfExtent.getY()); + normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f)); } faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ(); @@ -197,8 +192,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setZ( boxHalfExtent.getZ() ); - normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f)); + closestPoint.setZ(boxHalfExtent.getZ()); + normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f)); } faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ(); @@ -206,8 +201,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setZ( -boxHalfExtent.getZ() ); - normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f)); + closestPoint.setZ(-boxHalfExtent.getZ()); + normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f)); } return minDist; diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index eefaedc9e..3348bc89a 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -28,21 +28,20 @@ class btPersistentManifold; /// Other features are frame-coherency (persistent data) and collision response. class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_isSwapped; - -public: + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; - btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped); +public: + btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btSphereBoxCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -50,26 +49,25 @@ public: } } - bool getSphereDistance( const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance ); + bool getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance); - btScalar getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + btScalar getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal); + + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm)); if (!m_swapped) { - return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false); - } else + return new (mem) btSphereBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false); + } + else { - return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true); + return new (mem) btSphereBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true); } } }; - }; -#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H - +#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index 27eaec305..7fa0559f9 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -20,14 +20,14 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap) -: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf) +btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap) + : btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf) { if (!m_manifoldPtr) { - m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(),col1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(), col1Wrap->getCollisionObject()); m_ownManifold = true; } } @@ -41,7 +41,7 @@ btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm() } } -void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSphereSphereCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)dispatchInfo; @@ -53,27 +53,27 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject btSphereShape* sphere0 = (btSphereShape*)col0Wrap->getCollisionShape(); btSphereShape* sphere1 = (btSphereShape*)col1Wrap->getCollisionShape(); - btVector3 diff = col0Wrap->getWorldTransform().getOrigin()- col1Wrap->getWorldTransform().getOrigin(); + btVector3 diff = col0Wrap->getWorldTransform().getOrigin() - col1Wrap->getWorldTransform().getOrigin(); btScalar len = diff.length(); btScalar radius0 = sphere0->getRadius(); btScalar radius1 = sphere1->getRadius(); #ifdef CLEAR_MANIFOLD - m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting + m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting #endif ///iff distance positive, don't generate a new contact - if ( len > (radius0+radius1+resultOut->m_closestPointDistanceThreshold)) + if (len > (radius0 + radius1 + resultOut->m_closestPointDistanceThreshold)) { #ifndef CLEAR_MANIFOLD resultOut->refreshContactPoints(); -#endif //CLEAR_MANIFOLD +#endif //CLEAR_MANIFOLD return; } ///distance (negative means penetration) - btScalar dist = len - (radius0+radius1); + btScalar dist = len - (radius0 + radius1); - btVector3 normalOnSurfaceB(1,0,0); + btVector3 normalOnSurfaceB(1, 0, 0); if (len > SIMD_EPSILON) { normalOnSurfaceB = diff / len; @@ -82,20 +82,18 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject ///point on A (worldspace) ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) - btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; + btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1 * normalOnSurfaceB; /// report a contact. internally this will be kept persistent, and contact reduction is done - - - resultOut->addContactPoint(normalOnSurfaceB,pos1,dist); + + resultOut->addContactPoint(normalOnSurfaceB, pos1, dist); #ifndef CLEAR_MANIFOLD resultOut->refreshContactPoints(); -#endif //CLEAR_MANIFOLD - +#endif //CLEAR_MANIFOLD } -btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)col0; (void)col1; diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index 3517a568a..b08d0df76 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -28,39 +28,37 @@ class btPersistentManifold; /// Also provides the most basic sample for custom/user btCollisionAlgorithm class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + public: - btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap); + btSphereSphereCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap); btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { manifoldArray.push_back(m_manifoldPtr); } } - + virtual ~btSphereSphereCollisionAlgorithm(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm)); - return new(mem) btSphereSphereCollisionAlgorithm(0,ci,col0Wrap,col1Wrap); + return new (mem) btSphereSphereCollisionAlgorithm(0, ci, col0Wrap, col1Wrap); } }; - }; -#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H - +#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 86d4e7440..1bc3056c0 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSphereTriangleCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -21,15 +20,15 @@ subject to the following restrictions: #include "SphereTriangleDetector.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf), -m_swapped(swapped) +btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool swapped) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf), + m_swapped(swapped) { if (!m_manifoldPtr) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } } @@ -43,36 +42,35 @@ btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm() } } -void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSphereTriangleCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) return; - const btCollisionObjectWrapper* sphereObjWrap = m_swapped? col1Wrap : col0Wrap; - const btCollisionObjectWrapper* triObjWrap = m_swapped? col0Wrap : col1Wrap; + const btCollisionObjectWrapper* sphereObjWrap = m_swapped ? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* triObjWrap = m_swapped ? col0Wrap : col1Wrap; btSphereShape* sphere = (btSphereShape*)sphereObjWrap->getCollisionShape(); btTriangleShape* triangle = (btTriangleShape*)triObjWrap->getCollisionShape(); - + /// 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()+ resultOut->m_closestPointDistanceThreshold); - + SphereTriangleDetector detector(sphere, triangle, m_manifoldPtr->getContactBreakingThreshold() + resultOut->m_closestPointDistanceThreshold); + btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); ///@todo: tighter bounds input.m_transformA = sphereObjWrap->getWorldTransform(); input.m_transformB = triObjWrap->getWorldTransform(); bool swapResults = m_swapped; - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults); + detector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw, swapResults); if (m_ownManifold) resultOut->refreshContactPoints(); - } -btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index 6b6e39a72..d660222f1 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -27,43 +27,39 @@ class btPersistentManifold; /// Also provides the most basic sample for custom/user btCollisionAlgorithm class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_swapped; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_swapped; + public: - btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped); + btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool swapped); btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { manifoldArray.push_back(m_manifoldPtr); } } - + virtual ~btSphereTriangleCollisionAlgorithm(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm)); - return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_swapped); + return new (mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_swapped); } }; - }; -#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H - +#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index 522293359..816bf1e6a 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -15,68 +15,60 @@ subject to the following restrictions: #include "btUnionFind.h" - - btUnionFind::~btUnionFind() { Free(); - } btUnionFind::btUnionFind() -{ - +{ } -void btUnionFind::allocate(int N) +void btUnionFind::allocate(int N) { m_elements.resize(N); } -void btUnionFind::Free() +void btUnionFind::Free() { m_elements.clear(); } - -void btUnionFind::reset(int N) +void btUnionFind::reset(int N) { allocate(N); - for (int i = 0; i < N; i++) - { - m_elements[i].m_id = i; m_elements[i].m_sz = 1; - } + for (int i = 0; i < N; i++) + { + m_elements[i].m_id = i; + m_elements[i].m_sz = 1; + } } - class btUnionFindElementSortPredicate { - public: - - bool operator() ( const btElement& lhs, const btElement& rhs ) const - { - return lhs.m_id < rhs.m_id; - } +public: + bool operator()(const btElement& lhs, const btElement& rhs) const + { + return lhs.m_id < rhs.m_id; + } }; ///this is a special operation, destroying the content of btUnionFind. ///it sorts the elements, based on island id, in order to make it easy to iterate over islands -void btUnionFind::sortIslands() +void btUnionFind::sortIslands() { - //first store the original body index, and islandId int numElements = m_elements.size(); - - for (int i=0;i m_elements; +{ +private: + btAlignedObjectArray m_elements; - public: - - btUnionFind(); - ~btUnionFind(); +public: + btUnionFind(); + ~btUnionFind(); - - //this is a special operation, destroying the content of btUnionFind. - //it sorts the elements, based on island id, in order to make it easy to iterate over islands - void sortIslands(); + //this is a special operation, destroying the content of btUnionFind. + //it sorts the elements, based on island id, in order to make it easy to iterate over islands + void sortIslands(); - void reset(int N); + void reset(int N); - SIMD_FORCE_INLINE int getNumElements() const - { - return int(m_elements.size()); - } - SIMD_FORCE_INLINE bool isRoot(int x) const - { - return (x == m_elements[x].m_id); - } + SIMD_FORCE_INLINE int getNumElements() const + { + return int(m_elements.size()); + } + SIMD_FORCE_INLINE bool isRoot(int x) const + { + return (x == m_elements[x].m_id); + } - btElement& getElement(int index) - { - return m_elements[index]; - } - const btElement& getElement(int index) const - { - return m_elements[index]; - } - - void allocate(int N); - void Free(); + btElement& getElement(int index) + { + return m_elements[index]; + } + const btElement& getElement(int index) const + { + return m_elements[index]; + } + void allocate(int N); + void Free(); + int find(int p, int q) + { + return (find(p) == find(q)); + } - - int find(int p, int q) - { - return (find(p) == find(q)); - } - - void unite(int p, int q) - { - int i = find(p), j = find(q); - if (i == j) - return; + void unite(int p, int q) + { + int i = find(p), j = find(q); + if (i == j) + return; #ifndef USE_PATH_COMPRESSION - //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) - if (m_elements[i].m_sz < m_elements[j].m_sz) - { - m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; - } - else - { - m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; - } -#else - m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; -#endif //USE_PATH_COMPRESSION + //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) + if (m_elements[i].m_sz < m_elements[j].m_sz) + { + m_elements[i].m_id = j; + m_elements[j].m_sz += m_elements[i].m_sz; } + else + { + m_elements[j].m_id = i; + m_elements[i].m_sz += m_elements[j].m_sz; + } +#else + m_elements[i].m_id = j; + m_elements[j].m_sz += m_elements[i].m_sz; +#endif //USE_PATH_COMPRESSION + } - int find(int x) - { + int find(int x) + { + //btAssert(x < m_N); + //btAssert(x >= 0); + + while (x != m_elements[x].m_id) + { + //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically + +#ifdef USE_PATH_COMPRESSION + const btElement* elementPtr = &m_elements[m_elements[x].m_id]; + m_elements[x].m_id = elementPtr->m_id; + x = elementPtr->m_id; +#else // + x = m_elements[x].m_id; +#endif //btAssert(x < m_N); //btAssert(x >= 0); - - while (x != m_elements[x].m_id) - { - //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically - - #ifdef USE_PATH_COMPRESSION - const btElement* elementPtr = &m_elements[m_elements[x].m_id]; - m_elements[x].m_id = elementPtr->m_id; - x = elementPtr->m_id; - #else// - x = m_elements[x].m_id; - #endif - //btAssert(x < m_N); - //btAssert(x >= 0); - - } - return x; } + return x; + } +}; - - }; - - -#endif //BT_UNION_FIND_H +#endif //BT_UNION_FIND_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.cpp index ecce028c2..a3d8075da 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.cpp @@ -15,28 +15,23 @@ subject to the following restrictions: #include "btBox2dShape.h" +//{ -//{ - - -void btBox2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btBox2dShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); + btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax); } - -void btBox2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btBox2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //btScalar margin = btScalar(0.); btVector3 halfExtents = getHalfExtentsWithMargin(); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); } - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.h index 22bee4f2c..7e085f9e2 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.h @@ -23,9 +23,9 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" ///The btBox2dShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space. -ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) +btBox2dShape : public btPolyhedralConvexShape { - //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead btVector3 m_centroid; @@ -33,79 +33,75 @@ ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape btVector3 m_normals[4]; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 getHalfExtentsWithMargin() const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; return halfExtents; } - + const btVector3& getHalfExtentsWithoutMargin() const { - return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included + return m_implicitShapeDimensions; //changed in Bullet 2.63: assume the scaling and margin are included } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; - + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - - for (int i=0;iboxHalfExtents.getY()) + if (minDimension > boxHalfExtents.getY()) minDimension = boxHalfExtents.getY(); m_shapeType = BOX_2D_SHAPE_PROXYTYPE; - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; setSafeMargin(minDimension); @@ -114,42 +110,34 @@ public: 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; + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; + btConvexInternalShape::setMargin(collisionMargin); + btVector3 newMargin(getMargin(), getMargin(), getMargin()); + m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; } - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; btConvexInternalShape::setLocalScaling(scaling); m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; - } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - - - - - int getVertexCount() const + int getVertexCount() const { return 4; } - virtual int getNumVertices()const + virtual int getNumVertices() const { return 4; } @@ -164,82 +152,70 @@ public: return &m_normals[0]; } - - - - - - - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const { //this plane might not be aligned... - btVector4 plane ; - getPlaneEquation(plane,i); - planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ()); + btVector4 plane; + getPlaneEquation(plane, i); + planeNormal = btVector3(plane.getX(), plane.getY(), plane.getZ()); planeSupport = localGetSupportingVertex(-planeNormal); } - const btVector3& getCentroid() const { return m_centroid; } - + virtual int getNumPlanes() const { return 6; - } - - + } virtual int getNumEdges() const { return 12; } - - virtual void getVertex(int i,btVector3& vtx) const + virtual void getVertex(int i, btVector3& vtx) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); vtx = btVector3( - halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1), - halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1), - halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2)); + halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1), + halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1), + halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2)); } - - virtual void getPlaneEquation(btVector4& plane,int i) const + virtual void getPlaneEquation(btVector4 & plane, int i) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); switch (i) { - case 0: - plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 1: - plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 2: - plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y()); - break; - case 3: - plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y()); - break; - case 4: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z()); - break; - case 5: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z()); - break; - default: - btAssert(0); + case 0: + plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 1: + plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 2: + plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y()); + break; + case 3: + plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y()); + break; + case 4: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z()); + break; + case 5: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z()); + break; + default: + btAssert(0); } } - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const //virtual void getEdge(int i,Edge& edge) const { int edgeVert0 = 0; @@ -247,126 +223,117 @@ public: switch (i) { - case 0: + case 0: edgeVert0 = 0; edgeVert1 = 1; - break; - case 1: + break; + case 1: edgeVert0 = 0; edgeVert1 = 2; - break; - case 2: - edgeVert0 = 1; - edgeVert1 = 3; + break; + case 2: + edgeVert0 = 1; + edgeVert1 = 3; - break; - case 3: - edgeVert0 = 2; - edgeVert1 = 3; - break; - case 4: - edgeVert0 = 0; - edgeVert1 = 4; - break; - case 5: - edgeVert0 = 1; - edgeVert1 = 5; - - break; - case 6: - edgeVert0 = 2; - edgeVert1 = 6; - break; - case 7: - edgeVert0 = 3; - edgeVert1 = 7; - break; - case 8: - edgeVert0 = 4; - edgeVert1 = 5; - break; - case 9: - edgeVert0 = 4; - edgeVert1 = 6; - break; - case 10: - edgeVert0 = 5; - edgeVert1 = 7; - break; - case 11: - edgeVert0 = 6; - edgeVert1 = 7; - break; - default: - btAssert(0); + break; + case 3: + edgeVert0 = 2; + edgeVert1 = 3; + break; + case 4: + edgeVert0 = 0; + edgeVert1 = 4; + break; + case 5: + edgeVert0 = 1; + edgeVert1 = 5; + break; + case 6: + edgeVert0 = 2; + edgeVert1 = 6; + break; + case 7: + edgeVert0 = 3; + edgeVert1 = 7; + break; + case 8: + edgeVert0 = 4; + edgeVert1 = 5; + break; + case 9: + edgeVert0 = 4; + edgeVert1 = 6; + break; + case 10: + edgeVert0 = 5; + edgeVert1 = 7; + break; + case 11: + edgeVert0 = 6; + edgeVert1 = 7; + break; + default: + btAssert(0); } - getVertex(edgeVert0,pa ); - getVertex(edgeVert1,pb ); + getVertex(edgeVert0, pa); + getVertex(edgeVert1, pb); } - - - - - virtual bool isInside(const btVector3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt, btScalar tolerance) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); //btScalar minDist = 2*tolerance; - - bool result = (pt.x() <= (halfExtents.x()+tolerance)) && - (pt.x() >= (-halfExtents.x()-tolerance)) && - (pt.y() <= (halfExtents.y()+tolerance)) && - (pt.y() >= (-halfExtents.y()-tolerance)) && - (pt.z() <= (halfExtents.z()+tolerance)) && - (pt.z() >= (-halfExtents.z()-tolerance)); - + + bool result = (pt.x() <= (halfExtents.x() + tolerance)) && + (pt.x() >= (-halfExtents.x() - tolerance)) && + (pt.y() <= (halfExtents.y() + tolerance)) && + (pt.y() >= (-halfExtents.y() - tolerance)) && + (pt.z() <= (halfExtents.z() + tolerance)) && + (pt.z() >= (-halfExtents.z() - tolerance)); + return result; } - //debugging - virtual const char* getName()const + virtual const char* getName() const { return "Box2d"; } - virtual int getNumPreferredPenetrationDirections() const + virtual int getNumPreferredPenetrationDirections() const { return 6; } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { switch (index) { - case 0: - penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); - break; - case 1: - penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); - break; - case 2: - penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); - break; - case 3: - penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); - break; - case 4: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); - break; - case 5: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); - break; - default: - btAssert(0); + case 0: + penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.)); + break; + case 1: + penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.)); + break; + case 2: + penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.)); + break; + case 3: + penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.)); + break; + case 4: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.)); + break; + case 5: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.)); + break; + default: + btAssert(0); } } - }; -#endif //BT_OBB_BOX_2D_SHAPE_H - - +#endif //BT_OBB_BOX_2D_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBoxShape.cpp index 72eeb3891..cb91d023e 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -14,38 +14,32 @@ subject to the following restrictions: */ #include "btBoxShape.h" -btBoxShape::btBoxShape( const btVector3& boxHalfExtents) -: btPolyhedralConvexShape() +btBoxShape::btBoxShape(const btVector3& boxHalfExtents) + : btPolyhedralConvexShape() { m_shapeType = BOX_SHAPE_PROXYTYPE; - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; setSafeMargin(boxHalfExtents); }; - - - -void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btBoxShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); + btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax); } - -void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btBoxShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //btScalar margin = btScalar(0.); btVector3 halfExtents = getHalfExtentsWithMargin(); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); } - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBoxShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBoxShape.h index 715e3f2ab..3c65505d5 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -23,112 +23,102 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" ///The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space. -ATTRIBUTE_ALIGNED16(class) btBoxShape: public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) +btBoxShape : public btPolyhedralConvexShape { - //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead - public: - -BT_DECLARE_ALIGNED_ALLOCATOR(); + BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 getHalfExtentsWithMargin() const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; return halfExtents; } - + const btVector3& getHalfExtentsWithoutMargin() const { - return m_implicitShapeDimensions;//scaling is included, margin is not + return m_implicitShapeDimensions; //scaling is included, margin is not } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; - + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - - for (int i=0;i>1)) - halfExtents.y() * ((i&2)>>1), - halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2)); + halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1), + halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1), + halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2)); } - - virtual void getPlaneEquation(btVector4& plane,int i) const + virtual void getPlaneEquation(btVector4 & plane, int i) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); switch (i) { - case 0: - plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 1: - plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 2: - plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y()); - break; - case 3: - plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y()); - break; - case 4: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z()); - break; - case 5: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z()); - break; - default: - btAssert(0); + case 0: + plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 1: + plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 2: + plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y()); + break; + case 3: + plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y()); + break; + case 4: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z()); + break; + case 5: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z()); + break; + default: + btAssert(0); } } - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const //virtual void getEdge(int i,Edge& edge) const { int edgeVert0 = 0; @@ -188,127 +175,117 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); switch (i) { - case 0: + case 0: edgeVert0 = 0; edgeVert1 = 1; - break; - case 1: + break; + case 1: edgeVert0 = 0; edgeVert1 = 2; - break; - case 2: - edgeVert0 = 1; - edgeVert1 = 3; + break; + case 2: + edgeVert0 = 1; + edgeVert1 = 3; - break; - case 3: - edgeVert0 = 2; - edgeVert1 = 3; - break; - case 4: - edgeVert0 = 0; - edgeVert1 = 4; - break; - case 5: - edgeVert0 = 1; - edgeVert1 = 5; - - break; - case 6: - edgeVert0 = 2; - edgeVert1 = 6; - break; - case 7: - edgeVert0 = 3; - edgeVert1 = 7; - break; - case 8: - edgeVert0 = 4; - edgeVert1 = 5; - break; - case 9: - edgeVert0 = 4; - edgeVert1 = 6; - break; - case 10: - edgeVert0 = 5; - edgeVert1 = 7; - break; - case 11: - edgeVert0 = 6; - edgeVert1 = 7; - break; - default: - btAssert(0); + break; + case 3: + edgeVert0 = 2; + edgeVert1 = 3; + break; + case 4: + edgeVert0 = 0; + edgeVert1 = 4; + break; + case 5: + edgeVert0 = 1; + edgeVert1 = 5; + break; + case 6: + edgeVert0 = 2; + edgeVert1 = 6; + break; + case 7: + edgeVert0 = 3; + edgeVert1 = 7; + break; + case 8: + edgeVert0 = 4; + edgeVert1 = 5; + break; + case 9: + edgeVert0 = 4; + edgeVert1 = 6; + break; + case 10: + edgeVert0 = 5; + edgeVert1 = 7; + break; + case 11: + edgeVert0 = 6; + edgeVert1 = 7; + break; + default: + btAssert(0); } - getVertex(edgeVert0,pa ); - getVertex(edgeVert1,pb ); + getVertex(edgeVert0, pa); + getVertex(edgeVert1, pb); } - - - - - virtual bool isInside(const btVector3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt, btScalar tolerance) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); //btScalar minDist = 2*tolerance; - - bool result = (pt.x() <= (halfExtents.x()+tolerance)) && - (pt.x() >= (-halfExtents.x()-tolerance)) && - (pt.y() <= (halfExtents.y()+tolerance)) && - (pt.y() >= (-halfExtents.y()-tolerance)) && - (pt.z() <= (halfExtents.z()+tolerance)) && - (pt.z() >= (-halfExtents.z()-tolerance)); - + + bool result = (pt.x() <= (halfExtents.x() + tolerance)) && + (pt.x() >= (-halfExtents.x() - tolerance)) && + (pt.y() <= (halfExtents.y() + tolerance)) && + (pt.y() >= (-halfExtents.y() - tolerance)) && + (pt.z() <= (halfExtents.z() + tolerance)) && + (pt.z() >= (-halfExtents.z() - tolerance)); + return result; } - //debugging - virtual const char* getName()const + virtual const char* getName() const { return "Box"; } - virtual int getNumPreferredPenetrationDirections() const + virtual int getNumPreferredPenetrationDirections() const { return 6; } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { switch (index) { - case 0: - penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); - break; - case 1: - penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); - break; - case 2: - penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); - break; - case 3: - penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); - break; - case 4: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); - break; - case 5: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); - break; - default: - btAssert(0); + case 0: + penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.)); + break; + case 1: + penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.)); + break; + case 2: + penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.)); + break; + case 3: + penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.)); + break; + case 4: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.)); + break; + case 5: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.)); + break; + default: + btAssert(0); } } - }; - -#endif //BT_OBB_BOX_MINKOWSKI_H - - +#endif //BT_OBB_BOX_MINKOWSKI_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index 61f465cb7..d663b3d6d 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -22,11 +22,11 @@ subject to the following restrictions: ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh) -:btTriangleMeshShape(meshInterface), -m_bvh(0), -m_triangleInfoMap(0), -m_useQuantizedAabbCompression(useQuantizedAabbCompression), -m_ownsBvh(false) + : btTriangleMeshShape(meshInterface), + m_bvh(0), + m_triangleInfoMap(0), + m_useQuantizedAabbCompression(useQuantizedAabbCompression), + m_ownsBvh(false) { m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; //construct bvh from meshInterface @@ -37,16 +37,15 @@ m_ownsBvh(false) buildOptimizedBvh(); } -#endif //DISABLE_BVH - +#endif //DISABLE_BVH } -btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,bool buildBvh) -:btTriangleMeshShape(meshInterface), -m_bvh(0), -m_triangleInfoMap(0), -m_useQuantizedAabbCompression(useQuantizedAabbCompression), -m_ownsBvh(false) +btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh) + : btTriangleMeshShape(meshInterface), + m_bvh(0), + m_triangleInfoMap(0), + m_useQuantizedAabbCompression(useQuantizedAabbCompression), + m_ownsBvh(false) { m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; //construct bvh from meshInterface @@ -54,30 +53,28 @@ m_ownsBvh(false) if (buildBvh) { - void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16); + void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16); m_bvh = new (mem) btOptimizedBvh(); - - m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); + + m_bvh->build(meshInterface, m_useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax); m_ownsBvh = true; } -#endif //DISABLE_BVH - +#endif //DISABLE_BVH } -void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax) +void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax) { - m_bvh->refitPartial( m_meshInterface,aabbMin,aabbMax ); - + m_bvh->refitPartial(m_meshInterface, aabbMin, aabbMax); + m_localAabbMin.setMin(aabbMin); m_localAabbMax.setMax(aabbMax); } - -void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin,const btVector3& aabbMax) +void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin, const btVector3& aabbMax) { - m_bvh->refit( m_meshInterface, aabbMin,aabbMax ); - + m_bvh->refit(m_meshInterface, aabbMin, aabbMax); + recalcLocalAabb(); } @@ -90,27 +87,27 @@ btBvhTriangleMeshShape::~btBvhTriangleMeshShape() } } -void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) +void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) { - struct MyNodeOverlapCallback : public btNodeOverlapCallback + struct MyNodeOverlapCallback : public btNodeOverlapCallback { - btStridingMeshInterface* m_meshInterface; + btStridingMeshInterface* m_meshInterface; btTriangleCallback* m_callback; - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(callback) + MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface) + : m_meshInterface(meshInterface), + m_callback(callback) { } - + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { btVector3 m_triangle[3]; - const unsigned char *vertexbase; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; int numfaces; PHY_ScalarType indicestype; @@ -126,60 +123,60 @@ void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const indicestype, nodeSubPart); - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); - + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); + const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - + int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); + + m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ()); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + + m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ()); } } /* Perform ray vs. triangle collision here */ - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex); m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } }; - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface); - m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget); + m_bvh->reportRayOverlappingNodex(&myNodeCallback, raySource, rayTarget); } -void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) +void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) { - struct MyNodeOverlapCallback : public btNodeOverlapCallback + struct MyNodeOverlapCallback : public btNodeOverlapCallback { - btStridingMeshInterface* m_meshInterface; + btStridingMeshInterface* m_meshInterface; btTriangleCallback* m_callback; - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(callback) + MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface) + : m_meshInterface(meshInterface), + m_callback(callback) { } - + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { btVector3 m_triangle[3]; - const unsigned char *vertexbase; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; int numfaces; PHY_ScalarType indicestype; @@ -195,77 +192,74 @@ void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, co indicestype, nodeSubPart); - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); - + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); + const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ()); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + + m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ()); } } /* Perform ray vs. triangle collision here */ - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex); m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } }; - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface); - m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); + m_bvh->reportBoxCastOverlappingNodex(&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); } //perform bvh tree traversal and report overlapping triangles to 'callback' -void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - #ifdef DISABLE_BVH //brute force traverse all triangles - btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax); + btTriangleMeshShape::processAllTriangles(callback, aabbMin, aabbMax); #else //first get all the nodes - - struct MyNodeOverlapCallback : public btNodeOverlapCallback + struct MyNodeOverlapCallback : public btNodeOverlapCallback { - btStridingMeshInterface* m_meshInterface; - btTriangleCallback* m_callback; - btVector3 m_triangle[3]; + 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_numOverlap(0) + MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface) + : m_meshInterface(meshInterface), + m_callback(callback), + m_numOverlap(0) { } - + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { m_numOverlap++; - const unsigned char *vertexbase; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; int numfaces; PHY_ScalarType indicestype; - m_meshInterface->getLockedReadOnlyVertexIndexBase( &vertexbase, @@ -278,67 +272,62 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co indicestype, nodeSubPart); - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT||indicestype==PHY_UCHAR); - - const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) - { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:indicestype==PHY_INTEGER?gfxbase[j]:((unsigned char*)gfxbase)[j]; + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT || indicestype == PHY_UCHAR); + const btVector3& meshScaling = m_meshInterface->getScaling(); + for (int j = 2; j >= 0; j--) + { + int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : indicestype == PHY_INTEGER ? gfxbase[j] : ((unsigned char*)gfxbase)[j]; #ifdef DEBUG_TRIANGLE_MESH - printf("%d ,",graphicsindex); -#endif //DEBUG_TRIANGLE_MESH + printf("%d ,", graphicsindex); +#endif //DEBUG_TRIANGLE_MESH if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); + m_triangle[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); + graphicsbase[0] * meshScaling.getX(), + graphicsbase[1] * meshScaling.getY(), + graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); m_triangle[j] = btVector3( - btScalar(graphicsbase[0])*meshScaling.getX(), - btScalar(graphicsbase[1])*meshScaling.getY(), - btScalar(graphicsbase[2])*meshScaling.getZ()); + btScalar(graphicsbase[0]) * meshScaling.getX(), + btScalar(graphicsbase[1]) * meshScaling.getY(), + btScalar(graphicsbase[2]) * meshScaling.getZ()); } #ifdef DEBUG_TRIANGLE_MESH - printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z()); -#endif //DEBUG_TRIANGLE_MESH + printf("triangle vertices:%f,%f,%f\n", triangle[j].x(), triangle[j].y(), triangle[j].z()); +#endif //DEBUG_TRIANGLE_MESH } - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex); m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } - }; - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); - - m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); - -#endif//DISABLE_BVH + MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface); + m_bvh->reportAabbOverlappingNodex(&myNodeCallback, aabbMin, aabbMax); +#endif //DISABLE_BVH } -void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) { - if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) - { - btTriangleMeshShape::setLocalScaling(scaling); - buildOptimizedBvh(); - } + if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON) + { + btTriangleMeshShape::setLocalScaling(scaling); + buildOptimizedBvh(); + } } -void btBvhTriangleMeshShape::buildOptimizedBvh() +void btBvhTriangleMeshShape::buildOptimizedBvh() { if (m_ownsBvh) { @@ -346,43 +335,39 @@ void btBvhTriangleMeshShape::buildOptimizedBvh() btAlignedFree(m_bvh); } ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work - void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16); - m_bvh = new(mem) btOptimizedBvh(); + void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16); + m_bvh = new (mem) btOptimizedBvh(); //rebuild the bvh... - m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax); + m_bvh->build(m_meshInterface, m_useQuantizedAabbCompression, m_localAabbMin, m_localAabbMax); m_ownsBvh = true; } -void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling) +void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling) { - btAssert(!m_bvh); - btAssert(!m_ownsBvh); + btAssert(!m_bvh); + btAssert(!m_ownsBvh); - m_bvh = bvh; - m_ownsBvh = false; - // update the scaling without rebuilding the bvh - if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) - { - btTriangleMeshShape::setLocalScaling(scaling); - } + m_bvh = bvh; + m_ownsBvh = false; + // update the scaling without rebuilding the bvh + if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON) + { + btTriangleMeshShape::setLocalScaling(scaling); + } } - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*) dataBuffer; + btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*)dataBuffer; - btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer); + btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer); m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer); trimeshData->m_collisionMargin = float(m_collisionMargin); - - - if (m_bvh && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_BVH)) + if (m_bvh && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_BVH)) { void* chunk = serializer->findPointer(m_bvh); if (chunk) @@ -391,48 +376,49 @@ const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* se trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)chunk; trimeshData->m_quantizedFloatBvh = 0; #else - trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk; - trimeshData->m_quantizedDoubleBvh= 0; -#endif //BT_USE_DOUBLE_PRECISION - } else + trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk; + trimeshData->m_quantizedDoubleBvh = 0; +#endif //BT_USE_DOUBLE_PRECISION + } + else { - #ifdef BT_USE_DOUBLE_PRECISION trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); trimeshData->m_quantizedFloatBvh = 0; #else - trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); - trimeshData->m_quantizedDoubleBvh= 0; -#endif //BT_USE_DOUBLE_PRECISION - + trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); + trimeshData->m_quantizedDoubleBvh = 0; +#endif //BT_USE_DOUBLE_PRECISION + int sz = m_bvh->calculateSerializeBufferSizeNew(); - btChunk* chunk = serializer->allocate(sz,1); + btChunk* chunk = serializer->allocate(sz, 1); const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,m_bvh); + serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, m_bvh); } - } else + } + else { trimeshData->m_quantizedFloatBvh = 0; trimeshData->m_quantizedDoubleBvh = 0; } - - - if (m_triangleInfoMap && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_TRIANGLEINFOMAP)) + if (m_triangleInfoMap && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_TRIANGLEINFOMAP)) { void* chunk = serializer->findPointer(m_triangleInfoMap); if (chunk) { trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)chunk; - } else + } + else { trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)serializer->getUniquePointer(m_triangleInfoMap); int sz = m_triangleInfoMap->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(sz,1); + btChunk* chunk = serializer->allocate(sz, 1); const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,m_triangleInfoMap); + serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, m_triangleInfoMap); } - } else + } + else { trimeshData->m_triangleInfoMap = 0; } @@ -443,28 +429,24 @@ const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* se return "btTriangleMeshShapeData"; } -void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const +void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const { if (m_bvh) { - int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place - btChunk* chunk = serializer->allocate(len,1); + int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place + btChunk* chunk = serializer->allocate(len, 1); const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,(void*)m_bvh); + serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, (void*)m_bvh); } } -void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const +void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const { if (m_triangleInfoMap) { int len = m_triangleInfoMap->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,(void*)m_triangleInfoMap); + serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, (void*)m_triangleInfoMap); } } - - - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 1fa4995d1..8b2f2ee85 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -23,103 +23,99 @@ subject to the following restrictions: ///The btBvhTriangleMeshShape is a static-triangle mesh shape, it can only be used for fixed/non-moving objects. ///If you required moving concave triangle meshes, it is recommended to perform convex decomposition -///using HACD, see Bullet/Demos/ConvexDecompositionDemo. +///using HACD, see Bullet/Demos/ConvexDecompositionDemo. ///Alternatively, you can use btGimpactMeshShape for moving concave triangle meshes. -///btBvhTriangleMeshShape has several optimizations, such as bounding volume hierarchy and -///cache friendly traversal for PlayStation 3 Cell SPU. +///btBvhTriangleMeshShape has several optimizations, such as bounding volume hierarchy and +///cache friendly traversal for PlayStation 3 Cell SPU. ///It is recommended to enable useQuantizedAabbCompression for better memory usage. ///It takes a triangle mesh as input, for example a btTriangleMesh or btTriangleIndexVertexArray. The btBvhTriangleMeshShape class allows for triangle mesh deformations by a refit or partialRefit method. ///Instead of building the bounding volume hierarchy acceleration structure, it is also possible to serialize (save) and deserialize (load) the structure from disk. ///See Demos\ConcaveDemo\ConcavePhysicsDemo.cpp for an example. -ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape +ATTRIBUTE_ALIGNED16(class) +btBvhTriangleMeshShape : public btTriangleMeshShape { - - btOptimizedBvh* m_bvh; - btTriangleInfoMap* m_triangleInfoMap; + btOptimizedBvh* m_bvh; + btTriangleInfoMap* m_triangleInfoMap; bool m_useQuantizedAabbCompression; bool m_ownsBvh; #ifdef __clang__ - bool m_pad[11] __attribute__((unused));////need padding due to alignment + bool m_pad[11] __attribute__((unused)); ////need padding due to alignment #else - bool m_pad[11];////need padding due to alignment + bool m_pad[11]; ////need padding due to alignment #endif public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true); + btBvhTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true); ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb - btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true); - + btBvhTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh = true); + virtual ~btBvhTriangleMeshShape(); - bool getOwnsBvh () const + bool getOwnsBvh() const { return m_ownsBvh; } + void performRaycast(btTriangleCallback * callback, const btVector3& raySource, const btVector3& rayTarget); + void performConvexcast(btTriangleCallback * callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax); - - void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget); - void performConvexcast (btTriangleCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax); + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - void refitTree(const btVector3& aabbMin,const btVector3& aabbMax); + void refitTree(const btVector3& aabbMin, const btVector3& aabbMax); ///for a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks - void partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax); + void partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax); //debugging - virtual const char* getName()const {return "BVHTRIANGLEMESH";} + virtual const char* getName() const { return "BVHTRIANGLEMESH"; } + virtual void setLocalScaling(const btVector3& scaling); - virtual void setLocalScaling(const btVector3& scaling); - - btOptimizedBvh* getOptimizedBvh() + btOptimizedBvh* getOptimizedBvh() { return m_bvh; } - void setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1)); + void setOptimizedBvh(btOptimizedBvh * bvh, const btVector3& localScaling = btVector3(1, 1, 1)); - void buildOptimizedBvh(); + void buildOptimizedBvh(); - bool usesQuantizedAabbCompression() const + bool usesQuantizedAabbCompression() const { - return m_useQuantizedAabbCompression; + return m_useQuantizedAabbCompression; } - void setTriangleInfoMap(btTriangleInfoMap* triangleInfoMap) + void setTriangleInfoMap(btTriangleInfoMap * triangleInfoMap) { m_triangleInfoMap = triangleInfoMap; } - const btTriangleInfoMap* getTriangleInfoMap() const - { - return m_triangleInfoMap; - } - - btTriangleInfoMap* getTriangleInfoMap() + const btTriangleInfoMap* getTriangleInfoMap() const { return m_triangleInfoMap; } - virtual int calculateSerializeBufferSize() const; + btTriangleInfoMap* getTriangleInfoMap() + { + return m_triangleInfoMap; + } + + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - virtual void serializeSingleBvh(btSerializer* serializer) const; - - virtual void serializeSingleTriangleInfoMap(btSerializer* serializer) const; + virtual void serializeSingleBvh(btSerializer * serializer) const; + virtual void serializeSingleTriangleInfoMap(btSerializer * serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btTriangleMeshShapeData { @@ -138,12 +134,11 @@ struct btTriangleMeshShapeData }; +// clang-format on -SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const { return sizeof(btTriangleMeshShapeData); } - - -#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H +#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index 0345501ce..7c3377428 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -13,24 +13,21 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btCapsuleShape.h" #include "LinearMath/btQuaternion.h" -btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () +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); + m_implicitShapeDimensions.setValue(radius, 0.5f * height, radius); } - - btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - - btVector3 supVec(0,0,0); + btVector3 supVec(0, 0, 0); btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); @@ -38,20 +35,19 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } btVector3 vtx; btScalar newDot; - - { - btVector3 pos(0,0,0); + btVector3 pos(0, 0, 0); pos[getUpAxis()] = getHalfHeight(); vtx = pos; @@ -63,7 +59,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter } } { - btVector3 pos(0,0,0); + btVector3 pos(0, 0, 0); pos[getUpAxis()] = -getHalfHeight(); vtx = pos; @@ -76,15 +72,11 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter } return supVec; - } - void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - - - - for (int j=0;jm_convexInternalShapeData,serializer); + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); shapeData->m_upAxis = m_upAxis; @@ -178,7 +170,7 @@ SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSeri return "btCapsuleShapeData"; } -SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer) +SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer) { m_implicitShapeDimensions.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_implicitShapeDimensions); m_collisionMargin = dataBuffer->m_convexInternalShapeData.m_collisionMargin; @@ -187,4 +179,4 @@ SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* data m_upAxis = dataBuffer->m_upAxis; } -#endif //BT_CAPSULE_SHAPE_H +#endif //BT_CAPSULE_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionMargin.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionMargin.h index 474bf1fb4..abd8ab3eb 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionMargin.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionMargin.h @@ -19,9 +19,6 @@ subject to the following restrictions: ///The CONVEX_DISTANCE_MARGIN is a default collision margin for convex collision shapes derived from btConvexInternalShape. ///This collision margin is used by Gjk and some other algorithms ///Note that when creating small objects, you need to make sure to set a smaller collision margin, using the 'setMargin' API -#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01) - - - -#endif //BT_COLLISION_MARGIN_H +#define CONVEX_DISTANCE_MARGIN btScalar(0.04) // btScalar(0.1)//;//btScalar(0.01) +#endif //BT_COLLISION_MARGIN_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.cpp index 823e2788f..0b3640a65 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -20,47 +20,44 @@ subject to the following restrictions: can be used by probes that are checking whether the library is actually installed. */ -extern "C" +extern "C" { -void btBulletCollisionProbe (); + void btBulletCollisionProbe(); -void btBulletCollisionProbe () {} + void btBulletCollisionProbe() {} } - - -void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const +void btCollisionShape::getBoundingSphere(btVector3& center, btScalar& radius) const { btTransform tr; tr.setIdentity(); - btVector3 aabbMin,aabbMax; + btVector3 aabbMin, aabbMax; - getAabb(tr,aabbMin,aabbMax); + getAabb(tr, aabbMin, aabbMax); - radius = (aabbMax-aabbMin).length()*btScalar(0.5); - center = (aabbMin+aabbMax)*btScalar(0.5); + radius = (aabbMax - aabbMin).length() * btScalar(0.5); + center = (aabbMin + aabbMax) * btScalar(0.5); } - -btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const +btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const { return getAngularMotionDisc() * defaultContactThreshold; } -btScalar btCollisionShape::getAngularMotionDisc() const +btScalar btCollisionShape::getAngularMotionDisc() const { ///@todo cache this value, to improve performance - btVector3 center; + btVector3 center; btScalar disc; - getBoundingSphere(center,disc); + getBoundingSphere(center, disc); disc += (center).length(); return disc; } -void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const +void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btVector3& temporalAabbMin, btVector3& temporalAabbMax) const { //start with static aabb - getAabb(curTrans,temporalAabbMin,temporalAabbMax); + getAabb(curTrans, temporalAabbMin, temporalAabbMax); btScalar temporalAabbMaxx = temporalAabbMax.getX(); btScalar temporalAabbMaxy = temporalAabbMax.getY(); @@ -70,36 +67,36 @@ void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const b btScalar temporalAabbMinz = temporalAabbMin.getZ(); // add linear motion - btVector3 linMotion = linvel*timeStep; + btVector3 linMotion = linvel * timeStep; ///@todo: simd would have a vector max/min operation, instead of per-element access if (linMotion.x() > btScalar(0.)) - temporalAabbMaxx += linMotion.x(); + temporalAabbMaxx += linMotion.x(); else temporalAabbMinx += linMotion.x(); if (linMotion.y() > btScalar(0.)) - temporalAabbMaxy += linMotion.y(); + temporalAabbMaxy += linMotion.y(); else temporalAabbMiny += linMotion.y(); if (linMotion.z() > btScalar(0.)) - temporalAabbMaxz += linMotion.z(); + temporalAabbMaxz += linMotion.z(); else temporalAabbMinz += linMotion.z(); //add conservative angular motion btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep; - btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); - temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); - temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); + btVector3 angularMotion3d(angularMotion, angularMotion, angularMotion); + temporalAabbMin = btVector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); + temporalAabbMax = btVector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); temporalAabbMin -= angularMotion3d; temporalAabbMax += angularMotion3d; } ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btCollisionShapeData* shapeData = (btCollisionShapeData*) dataBuffer; - char* name = (char*) serializer->findNameForPointer(this); + btCollisionShapeData* shapeData = (btCollisionShapeData*)dataBuffer; + char* name = (char*)serializer->findNameForPointer(this); shapeData->m_name = (char*)serializer->getUniquePointer(name); if (shapeData->m_name) { @@ -113,10 +110,10 @@ const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializ return "btCollisionShapeData"; } -void btCollisionShape::serializeSingleShape(btSerializer* serializer) const +void btCollisionShape::serializeSingleShape(btSerializer* serializer) const { int len = calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,(void*)this); + serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, (void*)this); } \ No newline at end of file diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.h index 6c4916fbd..c80e105a4 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -19,12 +19,12 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types class btSerializer; - ///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects. -ATTRIBUTE_ALIGNED16(class) btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btCollisionShape { protected: int m_shapeType; @@ -32,10 +32,9 @@ protected: int m_userIndex; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1) + btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1) { } @@ -44,50 +43,47 @@ public: } ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0; - virtual void getBoundingSphere(btVector3& center,btScalar& radius) const; + virtual void getBoundingSphere(btVector3 & center, btScalar & radius) const; ///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; + virtual btScalar getAngularMotionDisc() const; + virtual btScalar getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const; ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative - void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const; + void calculateTemporalAabb(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btVector3& temporalAabbMin, btVector3& temporalAabbMax) const; - - - SIMD_FORCE_INLINE bool isPolyhedral() const + SIMD_FORCE_INLINE bool isPolyhedral() const { return btBroadphaseProxy::isPolyhedral(getShapeType()); } - SIMD_FORCE_INLINE bool isConvex2d() const + SIMD_FORCE_INLINE bool isConvex2d() const { return btBroadphaseProxy::isConvex2d(getShapeType()); } - SIMD_FORCE_INLINE bool isConvex() const + SIMD_FORCE_INLINE bool isConvex() const { return btBroadphaseProxy::isConvex(getShapeType()); } - SIMD_FORCE_INLINE bool isNonMoving() const + SIMD_FORCE_INLINE bool isNonMoving() const { return btBroadphaseProxy::isNonMoving(getShapeType()); } - SIMD_FORCE_INLINE bool isConcave() const + SIMD_FORCE_INLINE bool isConcave() const { return btBroadphaseProxy::isConcave(getShapeType()); } - SIMD_FORCE_INLINE bool isCompound() const + SIMD_FORCE_INLINE bool isCompound() const { return btBroadphaseProxy::isCompound(getShapeType()); } - SIMD_FORCE_INLINE bool isSoftBody() const + SIMD_FORCE_INLINE bool isSoftBody() const { return btBroadphaseProxy::isSoftBody(getShapeType()); } @@ -99,35 +95,35 @@ public: } #ifndef __SPU__ - virtual void setLocalScaling(const btVector3& scaling) =0; - virtual const btVector3& getLocalScaling() const =0; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0; + virtual void setLocalScaling(const btVector3& scaling) = 0; + virtual const btVector3& getLocalScaling() const = 0; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const = 0; + //debugging support + virtual const char* getName() const = 0; +#endif //__SPU__ -//debugging support - virtual const char* getName()const =0 ; -#endif //__SPU__ - - - int getShapeType() const { return m_shapeType; } + int getShapeType() const + { + return m_shapeType; + } ///the getAnisotropicRollingFrictionDirection can be used in combination with setAnisotropicFriction ///See Bullet/Demos/RollingFrictionDemo for an example - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - return btVector3(1,1,1); + return btVector3(1, 1, 1); } - virtual void setMargin(btScalar margin) = 0; - virtual btScalar getMargin() const = 0; + virtual void setMargin(btScalar margin) = 0; + virtual btScalar getMargin() const = 0; - ///optional user data pointer - void setUserPointer(void* userPtr) + void setUserPointer(void* userPtr) { m_userPointer = userPtr; } - void* getUserPointer() const + void* getUserPointer() const { return m_userPointer; } @@ -141,16 +137,16 @@ public: return m_userIndex; } - - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - virtual void serializeSingleShape(btSerializer* serializer) const; - -}; + virtual void serializeSingleShape(btSerializer * serializer) const; +}; +// clang-format off +// parser needs * with the name ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btCollisionShapeData { @@ -158,13 +154,10 @@ struct btCollisionShapeData int m_shapeType; char m_padding[4]; }; - -SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const +// clang-format on +SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const { return sizeof(btCollisionShapeData); } - - -#endif //BT_COLLISION_SHAPE_H - +#endif //BT_COLLISION_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index e8c8c336c..fd7828b10 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -19,26 +19,25 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity) -: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)), -m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)), -m_dynamicAabbTree(0), -m_updateRevision(1), -m_collisionMargin(btScalar(0.)), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) + : m_localAabbMin(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)), + m_localAabbMax(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)), + m_dynamicAabbTree(0), + m_updateRevision(1), + m_collisionMargin(btScalar(0.)), + m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)) { m_shapeType = COMPOUND_SHAPE_PROXYTYPE; if (enableDynamicAabbTree) { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); + void* mem = btAlignedAlloc(sizeof(btDbvt), 16); + m_dynamicAabbTree = new (mem) btDbvt(); + btAssert(mem == m_dynamicAabbTree); } m_children.reserve(initialChildCapacity); } - btCompoundShape::~btCompoundShape() { if (m_dynamicAabbTree) @@ -48,7 +47,7 @@ btCompoundShape::~btCompoundShape() } } -void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) +void btCompoundShape::addChildShape(const btTransform& localTransform, btCollisionShape* shape) { m_updateRevision++; //m_childTransforms.push_back(localTransform); @@ -60,11 +59,10 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio child.m_childShapeType = shape->getShapeType(); child.m_childMargin = shape->getMargin(); - //extend the local aabbMin/aabbMax - btVector3 localAabbMin,localAabbMax; - shape->getAabb(localTransform,localAabbMin,localAabbMax); - for (int i=0;i<3;i++) + btVector3 localAabbMin, localAabbMax; + shape->getAabb(localTransform, localAabbMin, localAabbMax); + for (int i = 0; i < 3; i++) { if (m_localAabbMin[i] > localAabbMin[i]) { @@ -74,31 +72,30 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio { m_localAabbMax[i] = localAabbMax[i]; } - } if (m_dynamicAabbTree) { - const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); size_t index = m_children.size(); - child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast(index) ); + child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast(index)); } m_children.push_back(child); - } -void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb) +void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb) { m_children[childIndex].m_transform = newChildTransform; if (m_dynamicAabbTree) { ///update the dynamic aabb tree - btVector3 localAabbMin,localAabbMax; - m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax); - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + btVector3 localAabbMin, localAabbMax; + m_children[childIndex].m_childShape->getAabb(newChildTransform, localAabbMin, localAabbMax); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); //int index = m_children.size()-1; - m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds); + m_dynamicAabbTree->update(m_children[childIndex].m_node, bounds); } if (shouldRecalculateLocalAabb) @@ -110,35 +107,30 @@ void btCompoundShape::updateChildTransform(int childIndex, const btTransform& ne void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) { m_updateRevision++; - btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size()); + btAssert(childShapeIndex >= 0 && childShapeIndex < m_children.size()); if (m_dynamicAabbTree) { m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node); } - m_children.swap(childShapeIndex,m_children.size()-1); - if (m_dynamicAabbTree) + m_children.swap(childShapeIndex, m_children.size() - 1); + if (m_dynamicAabbTree) m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex; m_children.pop_back(); - } - - void btCompoundShape::removeChildShape(btCollisionShape* shape) { m_updateRevision++; // Find the children containing the shape specified, and remove those children. //note: there might be multiple children using the same shape! - for(int i = m_children.size()-1; i >= 0 ; i--) + for (int i = m_children.size() - 1; i >= 0; i--) { - if(m_children[i].m_childShape == shape) + if (m_children[i].m_childShape == shape) { removeChildShapeByIndex(i); } } - - recalculateLocalAabb(); } @@ -147,15 +139,15 @@ void btCompoundShape::recalculateLocalAabb() // Recalculate the local aabb // Brute force, it iterates over all the shapes left. - m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); //extend the local aabbMin/aabbMax for (int j = 0; j < m_children.size(); j++) { - btVector3 localAabbMin,localAabbMax; + btVector3 localAabbMin, localAabbMax; m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { if (m_localAabbMin[i] > localAabbMin[i]) m_localAabbMin[i] = localAabbMin[i]; @@ -166,54 +158,48 @@ void btCompoundShape::recalculateLocalAabb() } ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btCompoundShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); - btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); - + btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin); + btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin); + //avoid an illegal AABB when there are no children if (!m_children.size()) { - localHalfExtents.setValue(0,0,0); - localCenter.setValue(0,0,0); + localHalfExtents.setValue(0, 0, 0); + localCenter.setValue(0, 0, 0); } - localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); - + localHalfExtents += btVector3(getMargin(), getMargin(), getMargin()); - btMatrix3x3 abs_b = trans.getBasis().absolute(); + btMatrix3x3 abs_b = trans.getBasis().absolute(); btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - aabbMin = center-extent; - aabbMax = center+extent; - + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + aabbMin = center - extent; + aabbMax = center + extent; } -void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btCompoundShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //approximation: take the inertia from the aabb for now btTransform ident; ident.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(ident,aabbMin,aabbMax); + btVector3 aabbMin, aabbMax; + getAabb(ident, aabbMin, aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz); - inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz); - inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia[0] = mass / (btScalar(12.0)) * (ly * ly + lz * lz); + inertia[1] = mass / (btScalar(12.0)) * (lx * lx + lz * lz); + inertia[2] = mass / (btScalar(12.0)) * (lx * lx + ly * ly); } - - - -void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const +void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const { int n = m_children.size(); @@ -223,18 +209,18 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf for (k = 0; k < n; k++) { - btAssert(masses[k]>0); + btAssert(masses[k] > 0); center += m_children[k].m_transform.getOrigin() * masses[k]; totalMass += masses[k]; } - btAssert(totalMass>0); + btAssert(totalMass > 0); center /= totalMass; principal.setOrigin(center); btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0); - for ( k = 0; k < n; k++) + for (k = 0; k < n; k++) { btVector3 i; m_children[k].m_childShape->calculateLocalInertia(masses[k], i); @@ -259,8 +245,8 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf j[0].setValue(o2, 0, 0); j[1].setValue(0, o2, 0); j[2].setValue(0, 0, o2); - j[0] += o * -o.x(); - j[1] += o * -o.y(); + j[0] += o * -o.x(); + j[1] += o * -o.y(); j[2] += o * -o.z(); //add inertia tensor of pointmass @@ -273,59 +259,50 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]); } - - - - void btCompoundShape::setLocalScaling(const btVector3& scaling) { - - for(int i = 0; i < m_children.size(); i++) + for (int i = 0; i < m_children.size(); i++) { btTransform childTrans = getChildTransform(i); btVector3 childScale = m_children[i].m_childShape->getLocalScaling(); -// childScale = childScale * (childTrans.getBasis() * scaling); + // childScale = childScale * (childTrans.getBasis() * scaling); childScale = childScale * scaling / m_localScaling; m_children[i].m_childShape->setLocalScaling(childScale); childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling); - updateChildTransform(i, childTrans,false); + updateChildTransform(i, childTrans, false); } - + m_localScaling = scaling; recalculateLocalAabb(); - } - void btCompoundShape::createAabbTreeFromChildren() { - if ( !m_dynamicAabbTree ) - { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); + if (!m_dynamicAabbTree) + { + void* mem = btAlignedAlloc(sizeof(btDbvt), 16); + m_dynamicAabbTree = new (mem) btDbvt(); + btAssert(mem == m_dynamicAabbTree); - for ( int index = 0; index < m_children.size(); index++ ) - { - btCompoundShapeChild &child = m_children[index]; + for (int index = 0; index < m_children.size(); index++) + { + btCompoundShapeChild& child = m_children[index]; - //extend the local aabbMin/aabbMax - btVector3 localAabbMin,localAabbMax; - child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax); + //extend the local aabbMin/aabbMax + btVector3 localAabbMin, localAabbMax; + child.m_childShape->getAabb(child.m_transform, localAabbMin, localAabbMax); - const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); size_t index2 = index; - child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast(index2) ); - } - } + child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast(index2)); + } + } } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const { - - btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer; + btCompoundShapeData* shapeData = (btCompoundShapeData*)dataBuffer; btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); shapeData->m_collisionMargin = float(m_collisionMargin); @@ -333,27 +310,26 @@ const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serialize shapeData->m_childShapePtr = 0; if (shapeData->m_numChildShapes) { - btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes); + btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData), shapeData->m_numChildShapes); btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr; shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr); - for (int i=0;im_numChildShapes;i++,memPtr++) + for (int i = 0; i < shapeData->m_numChildShapes; i++, memPtr++) { memPtr->m_childMargin = float(m_children[i].m_childMargin); memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape); //don't serialize shapes that already have been serialized if (!serializer->findPointer(m_children[i].m_childShape)) { - btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1); - const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer); - serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape); - } + btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(), 1); + const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, m_children[i].m_childShape); + } memPtr->m_childShapeType = m_children[i].m_childShapeType; m_children[i].m_transform.serializeFloat(memPtr->m_transform); } - serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btCompoundShapeChildData", BT_ARRAY_CODE, chunk->m_oldPtr); } return "btCompoundShapeData"; } - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.h index 4eef8dba3..7e2d0eb81 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -27,45 +27,47 @@ subject to the following restrictions: //class btOptimizedBvh; struct btDbvt; -ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild +ATTRIBUTE_ALIGNED16(struct) +btCompoundShapeChild { BT_DECLARE_ALIGNED_ALLOCATOR(); - btTransform m_transform; - btCollisionShape* m_childShape; - int m_childShapeType; - btScalar m_childMargin; - struct btDbvtNode* m_node; + btTransform m_transform; + btCollisionShape* m_childShape; + int m_childShapeType; + btScalar m_childMargin; + struct btDbvtNode* m_node; }; SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompoundShapeChild& c2) { - return ( c1.m_transform == c2.m_transform && - c1.m_childShape == c2.m_childShape && - c1.m_childShapeType == c2.m_childShapeType && - c1.m_childMargin == c2.m_childMargin ); + return (c1.m_transform == c2.m_transform && + c1.m_childShape == c2.m_childShape && + c1.m_childShapeType == c2.m_childShapeType && + c1.m_childMargin == c2.m_childMargin); } /// The btCompoundShape allows to store multiple other btCollisionShapes /// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape. -/// It has an (optional) dynamic aabb tree to accelerate early rejection tests. +/// It has an (optional) dynamic aabb tree to accelerate early rejection tests. /// @todo: This aabb tree can also be use to speed up ray tests on btCompoundShape, see http://code.google.com/p/bullet/issues/detail?id=25 /// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape) -ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btCompoundShape : public btCollisionShape { protected: btAlignedObjectArray m_children; - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; - btDbvt* m_dynamicAabbTree; + btDbvt* m_dynamicAabbTree; ///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated - int m_updateRevision; + int m_updateRevision; - btScalar m_collisionMargin; + btScalar m_collisionMargin; - btVector3 m_localScaling; + btVector3 m_localScaling; public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -74,17 +76,16 @@ public: virtual ~btCompoundShape(); - void addChildShape(const btTransform& localTransform,btCollisionShape* shape); + void addChildShape(const btTransform& localTransform, btCollisionShape* shape); /// Remove all children shapes that contain the specified shape - virtual void removeChildShape(btCollisionShape* shape); + virtual void removeChildShape(btCollisionShape * shape); void removeChildShapeByIndex(int childShapeindex); - - int getNumChildShapes() const + int getNumChildShapes() const { - return int (m_children.size()); + return int(m_children.size()); } btCollisionShape* getChildShape(int index) @@ -96,18 +97,17 @@ public: return m_children[index].m_childShape; } - btTransform& getChildTransform(int index) + btTransform& getChildTransform(int index) { return m_children[index].m_transform; } - const btTransform& getChildTransform(int index) const + const btTransform& getChildTransform(int index) const { return m_children[index].m_transform; } ///set a new transform for a child, and update internal data structures (local aabb and dynamic tree) - void updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true); - + void updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true); btCompoundShapeChild* getChildList() { @@ -115,40 +115,40 @@ public: } ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; /** Re-calculate the local Aabb. Is called at the end of removeChildShapes. Use this yourself if you modify the children or their transforms. */ - virtual void recalculateLocalAabb(); + virtual void recalculateLocalAabb(); - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const + virtual const btVector3& getLocalScaling() const { return m_localScaling; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { m_collisionMargin = margin; } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { return m_collisionMargin; } - virtual const char* getName()const + virtual const char* getName() const { return "Compound"; } - const btDbvt* getDynamicAabbTree() const + const btDbvt* getDynamicAabbTree() const { return m_dynamicAabbTree; } - - btDbvt* getDynamicAabbTree() + + btDbvt* getDynamicAabbTree() { return m_dynamicAabbTree; } @@ -160,21 +160,21 @@ public: ///"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 + int getUpdateRevision() const { return m_updateRevision; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btCompoundShapeChildData { @@ -197,16 +197,11 @@ struct btCompoundShapeData }; +// clang-format on -SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const { return sizeof(btCompoundShapeData); } - - - - - - -#endif //BT_COMPOUND_SHAPE_H +#endif //BT_COMPOUND_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.cpp index 58ff84a5b..5d396844d 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.cpp @@ -13,15 +13,12 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btConcaveShape.h" btConcaveShape::btConcaveShape() : m_collisionMargin(btScalar(0.)) { - } btConcaveShape::~btConcaveShape() { - } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.h index 2917cc5b6..716624e18 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -17,12 +17,13 @@ subject to the following restrictions: #define BT_CONCAVE_SHAPE_H #include "btCollisionShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "btTriangleCallback.h" /// PHY_ScalarType enumerates possible scalar types. /// See the btStridingMeshInterface or btHeightfieldTerrainShape for its use -typedef enum PHY_ScalarType { +typedef enum PHY_ScalarType +{ PHY_FLOAT, PHY_DOUBLE, PHY_INTEGER, @@ -33,30 +34,29 @@ typedef enum PHY_ScalarType { ///The btConcaveShape class provides an interface for non-moving (static) concave shapes. ///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape. -ATTRIBUTE_ALIGNED16(class) btConcaveShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btConcaveShape : public btCollisionShape { protected: btScalar m_collisionMargin; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btConcaveShape(); virtual ~btConcaveShape(); - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const = 0; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const = 0; - virtual btScalar getMargin() const { + virtual btScalar getMargin() const + { return m_collisionMargin; } virtual void setMargin(btScalar collisionMargin) { m_collisionMargin = collisionMargin; } - - - }; -#endif //BT_CONCAVE_SHAPE_H +#endif //BT_CONCAVE_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConeShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConeShape.cpp index 2d83c8bfb..64a6f272c 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -15,11 +15,9 @@ subject to the following restrictions: #include "btConeShape.h" - - -btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (), -m_radius (radius), -m_height(height) +btConeShape::btConeShape(btScalar radius, btScalar height) : btConvexInternalShape(), + m_radius(radius), + m_height(height) { m_shapeType = CONE_SHAPE_PROXYTYPE; setConeUpIndex(1); @@ -27,42 +25,40 @@ m_height(height) m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height)); } -btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height): -btConeShape(radius,height) +btConeShapeZ::btConeShapeZ(btScalar radius, btScalar height) : btConeShape(radius, height) { setConeUpIndex(2); } -btConeShapeX::btConeShapeX (btScalar radius,btScalar height): -btConeShape(radius,height) +btConeShapeX::btConeShapeX(btScalar radius, btScalar height) : btConeShape(radius, height) { setConeUpIndex(0); } ///choose upAxis index -void btConeShape::setConeUpIndex(int upIndex) +void btConeShape::setConeUpIndex(int upIndex) { switch (upIndex) { - case 0: + case 0: m_coneIndices[0] = 1; m_coneIndices[1] = 0; m_coneIndices[2] = 2; - break; - case 1: + break; + case 1: m_coneIndices[0] = 0; m_coneIndices[1] = 1; m_coneIndices[2] = 2; - break; - case 2: + break; + case 2: m_coneIndices[0] = 0; m_coneIndices[1] = 2; m_coneIndices[2] = 1; - break; - default: - btAssert(0); + break; + default: + btAssert(0); }; - + m_implicitShapeDimensions[m_coneIndices[0]] = m_radius; m_implicitShapeDimensions[m_coneIndices[1]] = m_height; m_implicitShapeDimensions[m_coneIndices[2]] = m_radius; @@ -70,72 +66,71 @@ void btConeShape::setConeUpIndex(int upIndex) btVector3 btConeShape::coneLocalSupport(const btVector3& v) const { - btScalar halfHeight = m_height * btScalar(0.5); - if (v[m_coneIndices[1]] > v.length() * m_sinAngle) - { - btVector3 tmp; - - tmp[m_coneIndices[0]] = btScalar(0.); - tmp[m_coneIndices[1]] = halfHeight; - tmp[m_coneIndices[2]] = btScalar(0.); - return tmp; - } - else { - btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]); - if (s > SIMD_EPSILON) { - btScalar d = m_radius / s; - btVector3 tmp; - tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d; - tmp[m_coneIndices[1]] = -halfHeight; - tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d; - return tmp; - } - else { + if (v[m_coneIndices[1]] > v.length() * m_sinAngle) + { btVector3 tmp; + tmp[m_coneIndices[0]] = btScalar(0.); - tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[1]] = halfHeight; tmp[m_coneIndices[2]] = btScalar(0.); return tmp; } - } - + else + { + btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]); + if (s > SIMD_EPSILON) + { + btScalar d = m_radius / s; + btVector3 tmp; + tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d; + tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d; + return tmp; + } + else + { + btVector3 tmp; + tmp[m_coneIndices[0]] = btScalar(0.); + tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[2]] = btScalar(0.); + return tmp; + } + } } -btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const +btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return coneLocalSupport(vec); + return coneLocalSupport(vec); } -void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;im_convexInternalShapeData,serializer); + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); shapeData->m_upIndex = m_coneIndices[1]; @@ -182,5 +172,4 @@ SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSeriali return "btConeShapeData"; } -#endif //BT_CONE_MINKOWSKI_H - +#endif //BT_CONE_MINKOWSKI_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp index 10ea3e981..7d3d1d362 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp @@ -15,54 +15,48 @@ subject to the following restrictions: #include "btConvex2dShape.h" -btConvex2dShape::btConvex2dShape( btConvexShape* convexChildShape): -btConvexShape (), m_childConvexShape(convexChildShape) +btConvex2dShape::btConvex2dShape(btConvexShape* convexChildShape) : btConvexShape(), m_childConvexShape(convexChildShape) { m_shapeType = CONVEX_2D_SHAPE_PROXYTYPE; } - + btConvex2dShape::~btConvex2dShape() { } - - -btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { return m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); } -void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); + m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors, supportVerticesOut, numVectors); } - -btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec) const { return m_childConvexShape->localGetSupportingVertex(vec); } - -void btConvex2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btConvex2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { ///this linear upscaling is not realistic, but we don't deal with large mass ratios... - m_childConvexShape->calculateLocalInertia(mass,inertia); + m_childConvexShape->calculateLocalInertia(mass, inertia); } - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btConvex2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btConvex2dShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - m_childConvexShape->getAabb(t,aabbMin,aabbMax); + m_childConvexShape->getAabb(t, aabbMin, aabbMax); } -void btConvex2dShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btConvex2dShape::getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax); + m_childConvexShape->getAabbSlow(t, aabbMin, aabbMax); } -void btConvex2dShape::setLocalScaling(const btVector3& scaling) +void btConvex2dShape::setLocalScaling(const btVector3& scaling) { m_childConvexShape->setLocalScaling(scaling); } @@ -72,21 +66,21 @@ const btVector3& btConvex2dShape::getLocalScaling() const return m_childConvexShape->getLocalScaling(); } -void btConvex2dShape::setMargin(btScalar margin) +void btConvex2dShape::setMargin(btScalar margin) { m_childConvexShape->setMargin(margin); } -btScalar btConvex2dShape::getMargin() const +btScalar btConvex2dShape::getMargin() const { return m_childConvexShape->getMargin(); } -int btConvex2dShape::getNumPreferredPenetrationDirections() const +int btConvex2dShape::getNumPreferredPenetrationDirections() const { return m_childConvexShape->getNumPreferredPenetrationDirections(); } - -void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + +void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { - m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); + m_childConvexShape->getPreferredPenetrationDirection(index, penetrationVector); } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.h index bbd1caf42..cd4f1ef7b 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.h @@ -17,66 +17,61 @@ subject to the following restrictions: #define BT_CONVEX_2D_SHAPE_H #include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///The btConvex2dShape allows to use arbitrary convex shapes as 2d convex shapes, with the Z component assumed to be 0. ///For 2d boxes, the btBox2dShape is recommended. -ATTRIBUTE_ALIGNED16(class) btConvex2dShape : public btConvexShape +ATTRIBUTE_ALIGNED16(class) +btConvex2dShape : public btConvexShape { - btConvexShape* m_childConvexShape; + btConvexShape* m_childConvexShape; - public: - +public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvex2dShape( btConvexShape* convexChildShape); - + + btConvex2dShape(btConvexShape * convexChildShape); + virtual ~btConvex2dShape(); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - btConvexShape* getChildShape() + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; + + btConvexShape* getChildShape() { return m_childConvexShape; } - const btConvexShape* getChildShape() const + const btConvexShape* getChildShape() const { return m_childConvexShape; } - virtual const char* getName()const + virtual const char* getName() const { return "Convex2dShape"; } - - /////////////////////////// - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling) ; - virtual const btVector3& getLocalScaling() const ; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; - virtual void setMargin(btScalar margin); - virtual btScalar getMargin() const; - - virtual int getNumPreferredPenetrationDirections() const; - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + virtual int getNumPreferredPenetrationDirections() const; + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; }; -#endif //BT_CONVEX_2D_SHAPE_H +#endif //BT_CONVEX_2D_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index a7a959840..703de4592 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#if defined (_WIN32) || defined (__i386__) +#if defined(_WIN32) || defined(__i386__) #define BT_USE_SSE_IN_API #endif @@ -25,14 +25,14 @@ subject to the following restrictions: #include "btConvexPolyhedron.h" #include "LinearMath/btConvexHullComputer.h" -btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape () +btConvexHullShape ::btConvexHullShape(const btScalar* points, int numPoints, int stride) : btPolyhedralConvexAabbCachingShape() { m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE; m_unscaledPoints.resize(numPoints); unsigned char* pointsAddress = (unsigned char*)points; - for (int i=0;im_convexInternalShapeData, serializer); int numElem = m_unscaledPoints.size(); shapeData->m_numUnscaledPoints = numElem; #ifdef BT_USE_DOUBLE_PRECISION shapeData->m_unscaledPointsFloatPtr = 0; - shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]): 0; + shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0; #else - shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]): 0; + shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0; shapeData->m_unscaledPointsDoublePtr = 0; #endif - + if (numElem) { int sz = sizeof(btVector3Data); - // int sz2 = sizeof(btVector3DoubleData); - // int sz3 = sizeof(btVector3FloatData); - btChunk* chunk = serializer->allocate(sz,numElem); + // int sz2 = sizeof(btVector3DoubleData); + // int sz3 = sizeof(btVector3FloatData); + btChunk* chunk = serializer->allocate(sz, numElem); btVector3Data* memPtr = (btVector3Data*)chunk->m_oldPtr; - for (int i=0;ifinalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]); + serializer->finalizeChunk(chunk, btVector3DataName, BT_ARRAY_CODE, (void*)&m_unscaledPoints[0]); } // Fill padding with zeros to appease msan. @@ -218,45 +204,41 @@ const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* seriali return "btConvexHullShapeData"; } -void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const +void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const { #if 1 minProj = FLT_MAX; maxProj = -FLT_MAX; int numVerts = m_unscaledPoints.size(); - for(int i=0;i maxProj) + if (dp > maxProj) { maxProj = dp; - witnesPtMax=pt; + witnesPtMax = pt; } } #else - btVector3 localAxis = dir*trans.getBasis(); - witnesPtMin = trans(localGetSupportingVertex(localAxis)); + btVector3 localAxis = dir * trans.getBasis(); + witnesPtMin = trans(localGetSupportingVertex(localAxis)); witnesPtMax = trans(localGetSupportingVertex(-localAxis)); minProj = witnesPtMin.dot(dir); maxProj = witnesPtMax.dot(dir); #endif - if(minProj>maxProj) + if (minProj > maxProj) { - btSwap(minProj,maxProj); - btSwap(witnesPtMin,witnesPtMax); + btSwap(minProj, maxProj); + btSwap(witnesPtMin, witnesPtMax); } - - } - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h index 0c12aeef1..96136d7dd 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -17,28 +17,26 @@ subject to the following restrictions: #define BT_CONVEX_HULL_SHAPE_H #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btAlignedObjectArray.h" - ///The btConvexHullShape implements an implicit convex hull of an array of vertices. ///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex. -ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btConvexHullShape : public btPolyhedralConvexAabbCachingShape { - btAlignedObjectArray m_unscaledPoints; + btAlignedObjectArray m_unscaledPoints; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive btScalar (x,y,z), the striding defines the number of bytes between each point, in memory. ///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint. ///btConvexHullShape make an internal copy of the points. - btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btVector3)); + btConvexHullShape(const btScalar* points = 0, int numPoints = 0, int stride = sizeof(btVector3)); void addPoint(const btVector3& point, bool recalculateLocalAabb = true); - btVector3* getUnscaledPoints() { return &m_unscaledPoints[0]; @@ -55,48 +53,46 @@ public: return getUnscaledPoints(); } - void optimizeConvexHull(); - - SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const + void optimizeConvexHull(); + + SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const { return m_unscaledPoints[i] * m_localScaling; } - SIMD_FORCE_INLINE int getNumPoints() const + SIMD_FORCE_INLINE int getNumPoints() const { return m_unscaledPoints.size(); } - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - - virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const; //debugging - virtual const char* getName()const {return "Convex";} + virtual const char* getName() const { return "Convex"; } - - virtual int getNumVertices() const; + virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; + virtual void getVertex(int i, btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; ///in case we receive negative scaling - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btConvexHullShapeData { @@ -110,12 +106,11 @@ struct btConvexHullShapeData }; +// clang-format on -SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const { return sizeof(btConvexHullShapeData); } - -#endif //BT_CONVEX_HULL_SHAPE_H - +#endif //BT_CONVEX_HULL_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp index 083d60b1b..4d598b1aa 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp @@ -13,139 +13,125 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btConvexInternalShape.h" - - btConvexInternalShape::btConvexInternalShape() -: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), -m_collisionMargin(CONVEX_DISTANCE_MARGIN) + : m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)), + m_collisionMargin(CONVEX_DISTANCE_MARGIN) { } - -void btConvexInternalShape::setLocalScaling(const btVector3& scaling) +void btConvexInternalShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling.absolute(); } - - -void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const +void btConvexInternalShape::getAabbSlow(const btTransform& trans, btVector3& minAabb, btVector3& maxAabb) const { #ifndef __SPU__ //use localGetSupportingVertexWithoutMargin? btScalar margin = getMargin(); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); - btVector3 sv = localGetSupportingVertex(vec*trans.getBasis()); + btVector3 sv = localGetSupportingVertex(vec * trans.getBasis()); btVector3 tmp = trans(sv); - maxAabb[i] = tmp[i]+margin; + maxAabb[i] = tmp[i] + margin; vec[i] = btScalar(-1.); - tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); - minAabb[i] = tmp[i]-margin; + tmp = trans(localGetSupportingVertex(vec * trans.getBasis())); + minAabb[i] = tmp[i] - margin; } #endif } - - -btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec) const { #ifndef __SPU__ - btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; #else btAssert(0); - return btVector3(0,0,0); -#endif //__SPU__ - - } - + return btVector3(0, 0, 0); +#endif //__SPU__ +} btConvexInternalAabbCachingShape::btConvexInternalAabbCachingShape() - : btConvexInternalShape(), -m_localAabbMin(1,1,1), -m_localAabbMax(-1,-1,-1), -m_isLocalAabbValid(false) + : btConvexInternalShape(), + m_localAabbMin(1, 1, 1), + m_localAabbMax(-1, -1, -1), + m_isLocalAabbValid(false) { } - -void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); + getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin()); } -void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling) +void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling) { btConvexInternalShape::setLocalScaling(scaling); recalcLocalAabb(); } - -void btConvexInternalAabbCachingShape::recalcLocalAabb() +void btConvexInternalAabbCachingShape::recalcLocalAabb() { m_isLocalAabbValid = true; - - #if 1 + +#if 1 static const btVector3 _directions[] = - { - btVector3( 1., 0., 0.), - btVector3( 0., 1., 0.), - btVector3( 0., 0., 1.), - btVector3( -1., 0., 0.), - btVector3( 0., -1., 0.), - btVector3( 0., 0., -1.) - }; - + { + btVector3(1., 0., 0.), + btVector3(0., 1., 0.), + btVector3(0., 0., 1.), + btVector3(-1., 0., 0.), + btVector3(0., -1., 0.), + btVector3(0., 0., -1.)}; + btVector3 _supporting[] = - { - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.) - }; - + { + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.)}; + batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); - - for ( int i = 0; i < 3; ++i ) + + for (int i = 0; i < 3; ++i) { m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin; m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin; } - - #else - for (int i=0;i<3;i++) +#else + + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]+m_collisionMargin; + m_localAabbMax[i] = tmp[i] + m_collisionMargin; vec[i] = btScalar(-1.); tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]-m_collisionMargin; + m_localAabbMin[i] = tmp[i] - m_collisionMargin; } - #endif +#endif } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.h index 1213b82fb..a28c57de4 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -19,39 +19,35 @@ subject to the following restrictions: #include "btConvexShape.h" #include "LinearMath/btAabbUtil2.h" - ///The btConvexInternalShape is an internal base class, shared by most convex shape implementations. ///The btConvexInternalShape uses a default collision margin set to CONVEX_DISTANCE_MARGIN. ///This collision margin used by Gjk and some other algorithms, see also btCollisionMargin.h -///Note that when creating small shapes (derived from btConvexInternalShape), +///Note that when creating small shapes (derived from btConvexInternalShape), ///you need to make sure to set a smaller collision margin, using the 'setMargin' API ///There is a automatic mechanism 'setSafeMargin' used by btBoxShape and btCylinderShape -ATTRIBUTE_ALIGNED16(class) btConvexInternalShape : public btConvexShape +ATTRIBUTE_ALIGNED16(class) +btConvexInternalShape : public btConvexShape { - - protected: - +protected: //local scaling. collisionMargin is not scaled ! - btVector3 m_localScaling; + btVector3 m_localScaling; - btVector3 m_implicitShapeDimensions; - - btScalar m_collisionMargin; + btVector3 m_implicitShapeDimensions; - btScalar m_padding; + btScalar m_collisionMargin; + + btScalar m_padding; btConvexInternalShape(); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); virtual ~btConvexInternalShape() { - } - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; const btVector3& getImplicitShapeDimensions() const { @@ -62,110 +58,102 @@ public: ///changing a collision shape while the body is in the world is not recommended, ///it is best to remove the body from the world, then make the change, and re-add it ///alternatively flush the contact points, see documentation for 'cleanProxyFromPairs' - void setImplicitShapeDimensions(const btVector3& dimensions) + void setImplicitShapeDimensions(const btVector3& dimensions) { m_implicitShapeDimensions = dimensions; } - void setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f) + void setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f) { - btScalar safeMargin = defaultMarginMultiplier*minDimension; + btScalar safeMargin = defaultMarginMultiplier * minDimension; if (safeMargin < getMargin()) { setMargin(safeMargin); } } - void setSafeMargin(const btVector3& halfExtents, btScalar defaultMarginMultiplier = 0.1f) + void setSafeMargin(const btVector3& halfExtents, btScalar defaultMarginMultiplier = 0.1f) { //see http://code.google.com/p/bullet/issues/detail?id=349 //this margin check could could be added to other collision shapes too, //or add some assert/warning somewhere - btScalar minDimension=halfExtents[halfExtents.minAxis()]; + btScalar minDimension = halfExtents[halfExtents.minAxis()]; setSafeMargin(minDimension, defaultMarginMultiplier); } ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - getAabbSlow(t,aabbMin,aabbMax); + getAabbSlow(t, aabbMin, aabbMax); } + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - - virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const { return m_localScaling; } - const btVector3& getLocalScalingNV() const + const btVector3& getLocalScalingNV() const { return m_localScaling; } - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { m_collisionMargin = margin; } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { return m_collisionMargin; } - btScalar getMarginNV() const + btScalar getMarginNV() const { return m_collisionMargin; } - virtual int getNumPreferredPenetrationDirections() const + virtual int getNumPreferredPenetrationDirections() const { return 0; } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { (void)penetrationVector; (void)index; btAssert(0); } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btConvexInternalShapeData +struct btConvexInternalShapeData { - btCollisionShapeData m_collisionShapeData; + btCollisionShapeData m_collisionShapeData; - btVector3FloatData m_localScaling; + btVector3FloatData m_localScaling; - btVector3FloatData m_implicitShapeDimensions; - - float m_collisionMargin; + btVector3FloatData m_implicitShapeDimensions; - int m_padding; + float m_collisionMargin; + int m_padding; }; - - -SIMD_FORCE_INLINE int btConvexInternalShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btConvexInternalShape::calculateSerializeBufferSize() const { return sizeof(btConvexInternalShapeData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*) dataBuffer; + btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*)dataBuffer; btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); m_implicitShapeDimensions.serializeFloat(shapeData->m_implicitShapeDimensions); @@ -178,50 +166,43 @@ SIMD_FORCE_INLINE const char* btConvexInternalShape::serialize(void* dataBuffer, return "btConvexInternalShapeData"; } - - - ///btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive bounding box calculations class btConvexInternalAabbCachingShape : public btConvexInternalShape { - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - bool m_isLocalAabbValid; - + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; + protected: - btConvexInternalAabbCachingShape(); - - void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax) + + void setCachedLocalAabb(const btVector3& aabbMin, const btVector3& aabbMax) { m_isLocalAabbValid = true; m_localAabbMin = aabbMin; m_localAabbMax = aabbMax; } - inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const + inline void getCachedLocalAabb(btVector3& aabbMin, btVector3& aabbMax) const { btAssert(m_isLocalAabbValid); aabbMin = m_localAabbMin; aabbMax = m_localAabbMax; } - inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const + inline void getNonvirtualAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax, btScalar margin) const { - //lazy evaluation of local aabb btAssert(m_isLocalAabbValid); - btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax); + btTransformAabb(m_localAabbMin, m_localAabbMax, margin, trans, aabbMin, aabbMax); } - + public: - - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - void recalcLocalAabb(); + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + void recalcLocalAabb(); }; -#endif //BT_CONVEX_INTERNAL_SHAPE_H +#endif //BT_CONVEX_INTERNAL_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp index ad1d1bf78..f00a440fa 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp @@ -25,81 +25,73 @@ void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling) } #ifndef __SPU__ -btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.)); btScalar maxDot = btScalar(-BT_LARGE_FLOAT); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } - - if( m_numPoints > 0 ) - { - // Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically. - // btVector3 scaled = vec * m_localScaling; - int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot); //FIXME: may violate encapsulation of m_unscaledPoints - return getScaledPoint(index); - } + + if (m_numPoints > 0) + { + // Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically. + // btVector3 scaled = vec * m_localScaling; + int index = (int)vec.maxDot(&m_unscaledPoints[0], m_numPoints, maxDot); //FIXME: may violate encapsulation of m_unscaledPoints + return getScaledPoint(index); + } return supVec; } -void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for( int j = 0; j < numVectors; j++ ) - { - const btVector3& vec = vectors[j] * m_localScaling; // dot( a*c, b) = dot(a, b*c) - btScalar maxDot; - int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot); - supportVerticesOut[j][3] = btScalar(-BT_LARGE_FLOAT); - if( 0 <= index ) - { - //WARNING: don't swap next lines, the w component would get overwritten! - supportVerticesOut[j] = getScaledPoint(index); - supportVerticesOut[j][3] = maxDot; - } - } - + for (int j = 0; j < numVectors; j++) + { + const btVector3& vec = vectors[j] * m_localScaling; // dot( a*c, b) = dot(a, b*c) + btScalar maxDot; + int index = (int)vec.maxDot(&m_unscaledPoints[0], m_numPoints, maxDot); + supportVerticesOut[j][3] = btScalar(-BT_LARGE_FLOAT); + if (0 <= index) + { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j] = getScaledPoint(index); + supportVerticesOut[j][3] = maxDot; + } + } } - - -btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - #endif - - - - - //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo -int btConvexPointCloudShape::getNumVertices() const +int btConvexPointCloudShape::getNumVertices() const { return m_numPoints; } @@ -109,31 +101,29 @@ int btConvexPointCloudShape::getNumEdges() const return 0; } -void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const +void btConvexPointCloudShape::getEdge(int i, btVector3& pa, btVector3& pb) const { - btAssert (0); + btAssert(0); } -void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const +void btConvexPointCloudShape::getVertex(int i, btVector3& vtx) const { - vtx = m_unscaledPoints[i]*m_localScaling; + vtx = m_unscaledPoints[i] * m_localScaling; } -int btConvexPointCloudShape::getNumPlanes() const +int btConvexPointCloudShape::getNumPlanes() const { return 0; } -void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const +void btConvexPointCloudShape::getPlane(btVector3&, btVector3&, int) const { - btAssert(0); } //not yet -bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const +bool btConvexPointCloudShape::isInside(const btVector3&, btScalar) const { btAssert(0); return false; } - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h index 54b5afac3..c7d554a4d 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h @@ -17,11 +17,12 @@ subject to the following restrictions: #define BT_CONVEX_POINT_CLOUD_SHAPE_H #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btAlignedObjectArray.h" ///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices. -ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape { btVector3* m_unscaledPoints; int m_numPoints; @@ -31,13 +32,13 @@ public: btConvexPointCloudShape() { - m_localScaling.setValue(1.f,1.f,1.f); + m_localScaling.setValue(1.f, 1.f, 1.f); m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; m_unscaledPoints = 0; m_numPoints = 0; } - btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true) + btConvexPointCloudShape(btVector3 * points, int numPoints, const btVector3& localScaling, bool computeAabb = true) { m_localScaling = localScaling; m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; @@ -48,7 +49,7 @@ public: recalcLocalAabb(); } - void setPoints (btVector3* points, int numPoints, bool computeAabb = true,const btVector3& localScaling=btVector3(1.f,1.f,1.f)) + void setPoints(btVector3 * points, int numPoints, bool computeAabb = true, const btVector3& localScaling = btVector3(1.f, 1.f, 1.f)) { m_unscaledPoints = points; m_numPoints = numPoints; @@ -58,48 +59,45 @@ public: recalcLocalAabb(); } - SIMD_FORCE_INLINE btVector3* getUnscaledPoints() + SIMD_FORCE_INLINE btVector3* getUnscaledPoints() { return m_unscaledPoints; } - SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const + SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const { return m_unscaledPoints; } - SIMD_FORCE_INLINE int getNumPoints() const + SIMD_FORCE_INLINE int getNumPoints() const { return m_numPoints; } - SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const + SIMD_FORCE_INLINE btVector3 getScaledPoint(int index) const { return m_unscaledPoints[index] * m_localScaling; } #ifndef __SPU__ - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; #endif - //debugging - virtual const char* getName()const {return "ConvexPointCloud";} + virtual const char* getName() const { return "ConvexPointCloud"; } - virtual int getNumVertices() const; + virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; + virtual void getVertex(int i, btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; ///in case we receive negative scaling - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); }; - -#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H - +#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index 4f45319a8..9694f4ddb 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///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 @@ -21,49 +20,45 @@ subject to the following restrictions: #include "btConvexPolyhedron.h" #include "LinearMath/btHashMap.h" - btConvexPolyhedron::btConvexPolyhedron() { - } btConvexPolyhedron::~btConvexPolyhedron() { - } - -inline bool IsAlmostZero(const btVector3& v) +inline bool IsAlmostZero1(const btVector3& v) { - if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6) return false; + if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false; return true; } struct btInternalVertexPair { - btInternalVertexPair(short int v0,short int v1) - :m_v0(v0), - m_v1(v1) + btInternalVertexPair(short int v0, short int v1) + : m_v0(v0), + m_v1(v1) { - if (m_v1>m_v0) - btSwap(m_v0,m_v1); + if (m_v1 > m_v0) + btSwap(m_v0, m_v1); } short int m_v0; short int m_v1; int getHash() const { - return m_v0+(m_v1<<16); + return m_v0 + (m_v1 << 16); } bool equals(const btInternalVertexPair& other) const { - return m_v0==other.m_v0 && m_v1==other.m_v1; + return m_v0 == other.m_v0 && m_v1 == other.m_v1; } }; struct btInternalEdge { btInternalEdge() - :m_face0(-1), - m_face1(-1) + : m_face0(-1), + m_face1(-1) { } short int m_face0; @@ -75,23 +70,31 @@ struct btInternalEdge #ifdef TEST_INTERNAL_OBJECTS bool btConvexPolyhedron::testContainment() const { - for(int p=0;p<8;p++) + for (int p = 0; p < 8; p++) { btVector3 LocalPt; - if(p==0) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]); - else if(p==1) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]); - else if(p==2) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]); - else if(p==3) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]); - else if(p==4) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]); - else if(p==5) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]); - else if(p==6) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]); - else if(p==7) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]); + if (p == 0) + LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]); + else if (p == 1) + LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]); + else if (p == 2) + LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]); + else if (p == 3) + LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]); + else if (p == 4) + LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]); + else if (p == 5) + LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]); + else if (p == 6) + LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]); + else if (p == 7) + LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]); - for(int i=0;i0.0f) + if (d > 0.0f) return false; } } @@ -99,33 +102,28 @@ bool btConvexPolyhedron::testContainment() const } #endif -void btConvexPolyhedron::initialize() +void btConvexPolyhedron::initialize() { + btHashMap edges; - btHashMap edges; - - btScalar TotalArea = 0.0f; - - m_localCenter.setValue(0, 0, 0); - for(int i=0;im_face0>=0); - btAssert(edptr->m_face1<0); + btAssert(edptr->m_face0 >= 0); + btAssert(edptr->m_face1 < 0); edptr->m_face1 = i; - } else + } + else { btInternalEdge ed; ed.m_face0 = i; - edges.insert(vp,ed); + edges.insert(vp, ed); } } } #ifdef USE_CONNECTED_FACES - for(int i=0;im_face0>=0); - btAssert(edptr->m_face1>=0); + btAssert(edptr->m_face0 >= 0); + btAssert(edptr->m_face1 >= 0); - int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0; + int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0; m_faces[i].m_connectedFaces[j] = connectedFace; } } -#endif//USE_CONNECTED_FACES +#endif //USE_CONNECTED_FACES - for(int i=0;iMaxX) MaxX = pt.x(); - if(pt.y()MaxY) MaxY = pt.y(); - if(pt.z()MaxZ) MaxZ = pt.z(); + if (pt.x() < MinX) MinX = pt.x(); + if (pt.x() > MaxX) MaxX = pt.x(); + if (pt.y() < MinY) MinY = pt.y(); + if (pt.y() > MaxY) MaxY = pt.y(); + if (pt.z() < MinZ) MinZ = pt.z(); + if (pt.z() > MaxZ) MaxZ = pt.z(); } - mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ); - mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ); + mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ); + mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ); - - -// const btScalar r = m_radius / sqrtf(2.0f); + // const btScalar r = m_radius / sqrtf(2.0f); const btScalar r = m_radius / sqrtf(3.0f); const int LargestExtent = mE.maxAxis(); - const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f; + const btScalar 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; + m_extents[LargestExtent] = mE[LargestExtent] * 0.5f; bool FoundBox = false; - for(int j=0;j<1024;j++) + for (int j = 0; j < 1024; j++) { - if(testContainment()) + if (testContainment()) { FoundBox = true; break; @@ -245,25 +245,25 @@ void btConvexPolyhedron::initialize() m_extents[LargestExtent] -= Step; } - if(!FoundBox) + if (!FoundBox) { m_extents[0] = m_extents[1] = m_extents[2] = r; } else { // Refine the box - const btScalar Step = (m_radius - r)/1024.0f; - const int e0 = (1< maxProj) + if (dp > maxProj) { maxProj = dp; witnesPtMax = pt; } } - if(minProj>maxProj) + if (minProj > maxProj) { - btSwap(minProj,maxProj); - btSwap(witnesPtMin,witnesPtMax); + btSwap(minProj, maxProj); + btSwap(witnesPtMin, witnesPtMax); } } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h index d3cd066ac..638aa9b3d 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h @@ -13,10 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans - #ifndef _BT_POLYHEDRAL_FEATURES_H #define _BT_POLYHEDRAL_FEATURES_H @@ -25,41 +23,37 @@ subject to the following restrictions: #define TEST_INTERNAL_OBJECTS 1 - struct btFace { - btAlignedObjectArray m_indices; -// btAlignedObjectArray m_connectedFaces; - btScalar m_plane[4]; + btAlignedObjectArray m_indices; + // btAlignedObjectArray m_connectedFaces; + btScalar m_plane[4]; }; - -ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron +ATTRIBUTE_ALIGNED16(class) +btConvexPolyhedron { - public: - +public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvexPolyhedron(); - virtual ~btConvexPolyhedron(); - btAlignedObjectArray m_vertices; - btAlignedObjectArray m_faces; + btConvexPolyhedron(); + virtual ~btConvexPolyhedron(); + + btAlignedObjectArray m_vertices; + btAlignedObjectArray m_faces; btAlignedObjectArray m_uniqueEdges; - btVector3 m_localCenter; - btVector3 m_extents; - btScalar m_radius; - btVector3 mC; - btVector3 mE; + btVector3 m_localCenter; + btVector3 m_extents; + btScalar m_radius; + btVector3 mC; + btVector3 mE; - void initialize(); + void initialize(); + void initialize2(); bool testContainment() const; - void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; + void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const; }; - -#endif //_BT_POLYHEDRAL_FEATURES_H - - +#endif //_BT_POLYHEDRAL_FEATURES_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 8d7fb054d..f8fb0aa9f 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#if defined (_WIN32) || defined (__i386__) +#if defined(_WIN32) || defined(__i386__) #define BT_USE_SSE_IN_API #endif @@ -27,30 +27,28 @@ subject to the following restrictions: #include "btConvexPointCloudShape.h" ///not supported on IBM SDK, until we fix the alignment of btVector3 -#if defined (__CELLOS_LV2__) && defined (__SPU__) +#if defined(__CELLOS_LV2__) && defined(__SPU__) #include -static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) +static inline vec_float4 vec_dot3(vec_float4 vec0, vec_float4 vec1) { - vec_float4 result; - result = spu_mul( vec0, vec1 ); - result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result ); - return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result ); + vec_float4 result; + result = spu_mul(vec0, vec1); + result = spu_madd(spu_rlqwbyte(vec0, 4), spu_rlqwbyte(vec1, 4), result); + return spu_madd(spu_rlqwbyte(vec0, 8), spu_rlqwbyte(vec1, 8), result); } -#endif //__SPU__ +#endif //__SPU__ -btConvexShape::btConvexShape () +btConvexShape::btConvexShape() { } btConvexShape::~btConvexShape() { - } - -void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin,btVector3& witnesPtMax) const +void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin, btVector3& witnesPtMax) const { - btVector3 localAxis = dir*trans.getBasis(); + btVector3 localAxis = dir * trans.getBasis(); btVector3 vtx1 = trans(localGetSupportingVertex(localAxis)); btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis)); @@ -58,8 +56,8 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc max = vtx2.dot(dir); witnesPtMax = vtx2; witnesPtMin = vtx1; - - if(min>max) + + if (min > max) { btScalar tmp = min; min = max; @@ -69,387 +67,392 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc } } - -static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) -{ - +static btVector3 convexHullSupport(const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) +{ btVector3 vec = localDirOrg * localScaling; -#if defined (__CELLOS_LV2__) && defined (__SPU__) +#if defined(__CELLOS_LV2__) && defined(__SPU__) btVector3 localDir = vec; - vec_float4 v_distMax = {-FLT_MAX,0,0,0}; - vec_int4 v_idxMax = {-999,0,0,0}; - int v=0; + vec_float4 v_distMax = {-FLT_MAX, 0, 0, 0}; + vec_int4 v_idxMax = {-999, 0, 0, 0}; + int v = 0; int numverts = numPoints; - for(;v<(int)numverts-4;v+=4) { - vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); - vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); - vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); - vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); - const vec_int4 i0 = {v ,0,0,0}; - const vec_int4 i1 = {v+1,0,0,0}; - const vec_int4 i2 = {v+2,0,0,0}; - const vec_int4 i3 = {v+3,0,0,0}; - vec_uint4 retGt01 = spu_cmpgt(p0,p1); - vec_float4 pmax01 = spu_sel(p1,p0,retGt01); - vec_int4 imax01 = spu_sel(i1,i0,retGt01); - vec_uint4 retGt23 = spu_cmpgt(p2,p3); - vec_float4 pmax23 = spu_sel(p3,p2,retGt23); - vec_int4 imax23 = spu_sel(i3,i2,retGt23); - vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); - vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); - vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); - vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); - v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); - v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); + for (; v < (int)numverts - 4; v += 4) + { + vec_float4 p0 = vec_dot3(points[v].get128(), localDir.get128()); + vec_float4 p1 = vec_dot3(points[v + 1].get128(), localDir.get128()); + vec_float4 p2 = vec_dot3(points[v + 2].get128(), localDir.get128()); + vec_float4 p3 = vec_dot3(points[v + 3].get128(), localDir.get128()); + const vec_int4 i0 = {v, 0, 0, 0}; + const vec_int4 i1 = {v + 1, 0, 0, 0}; + const vec_int4 i2 = {v + 2, 0, 0, 0}; + const vec_int4 i3 = {v + 3, 0, 0, 0}; + vec_uint4 retGt01 = spu_cmpgt(p0, p1); + vec_float4 pmax01 = spu_sel(p1, p0, retGt01); + vec_int4 imax01 = spu_sel(i1, i0, retGt01); + vec_uint4 retGt23 = spu_cmpgt(p2, p3); + vec_float4 pmax23 = spu_sel(p3, p2, retGt23); + vec_int4 imax23 = spu_sel(i3, i2, retGt23); + vec_uint4 retGt0123 = spu_cmpgt(pmax01, pmax23); + vec_float4 pmax0123 = spu_sel(pmax23, pmax01, retGt0123); + vec_int4 imax0123 = spu_sel(imax23, imax01, retGt0123); + vec_uint4 retGtMax = spu_cmpgt(v_distMax, pmax0123); + v_distMax = spu_sel(pmax0123, v_distMax, retGtMax); + v_idxMax = spu_sel(imax0123, v_idxMax, retGtMax); } - for(;v<(int)numverts;v++) { - vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); - const vec_int4 i = {v,0,0,0}; - vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); - v_distMax = spu_sel(p,v_distMax,retGtMax); - v_idxMax = spu_sel(i,v_idxMax,retGtMax); + for (; v < (int)numverts; v++) + { + vec_float4 p = vec_dot3(points[v].get128(), localDir.get128()); + const vec_int4 i = {v, 0, 0, 0}; + vec_uint4 retGtMax = spu_cmpgt(v_distMax, p); + v_distMax = spu_sel(p, v_distMax, retGtMax); + v_idxMax = spu_sel(i, v_idxMax, retGtMax); } - int ptIndex = spu_extract(v_idxMax,0); - const btVector3& supVec= points[ptIndex] * localScaling; + int ptIndex = spu_extract(v_idxMax, 0); + const btVector3& supVec = points[ptIndex] * localScaling; return supVec; #else - btScalar maxDot; - long ptIndex = vec.maxDot( points, numPoints, maxDot); + btScalar maxDot; + long ptIndex = vec.maxDot(points, numPoints, maxDot); btAssert(ptIndex >= 0); + if (ptIndex < 0) + { + ptIndex = 0; + } btVector3 supVec = points[ptIndex] * localScaling; return supVec; -#endif //__SPU__ +#endif //__SPU__ } -btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const +btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual(const btVector3& localDir) const { switch (m_shapeType) { - case SPHERE_SHAPE_PROXYTYPE: - { - return btVector3(0,0,0); - } - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); + case SPHERE_SHAPE_PROXYTYPE: + { + return btVector3(0, 0, 0); + } + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* convexShape = (btBoxShape*)this; + const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); -#if defined( __APPLE__ ) && (defined( BT_USE_SSE )||defined( BT_USE_NEON )) - #if defined( BT_USE_SSE ) - return btVector3( _mm_xor_ps( _mm_and_ps( localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f }), halfExtents.mVec128 )); - #elif defined( BT_USE_NEON ) - return btVector3( (float32x4_t) (((uint32x4_t) localDir.mVec128 & (uint32x4_t){ 0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t) halfExtents.mVec128 )); - #else - #error unknown vector arch - #endif +#if defined(__APPLE__) && (defined(BT_USE_SSE) || defined(BT_USE_NEON)) +#if defined(BT_USE_SSE) + return btVector3(_mm_xor_ps(_mm_and_ps(localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f}), halfExtents.mVec128)); +#elif defined(BT_USE_NEON) + return btVector3((float32x4_t)(((uint32x4_t)localDir.mVec128 & (uint32x4_t){0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t)halfExtents.mVec128)); #else - return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), - btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), - btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); +#error unknown vector arch #endif - } - case TRIANGLE_SHAPE_PROXYTYPE: - { - btTriangleShape* triangleShape = (btTriangleShape*)this; - btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3* vertices = &triangleShape->m_vertices1[0]; - btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]); - btVector3 sup = vertices[dots.maxAxis()]; - return btVector3(sup.getX(),sup.getY(),sup.getZ()); - } - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShape* cylShape = (btCylinderShape*)this; - //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) - - btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); - btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); - int cylinderUpAxis = cylShape->getUpAxis(); - int XX(1),YY(0),ZZ(2); - - switch (cylinderUpAxis) - { - case 0: - { - XX = 1; - YY = 0; - ZZ = 2; - } - break; - case 1: - { - XX = 0; - YY = 1; - ZZ = 2; - } - break; - case 2: - { - XX = 0; - YY = 2; - ZZ = 1; - - } - break; - default: - btAssert(0); - break; - }; - - btScalar radius = halfExtents[XX]; - btScalar halfHeight = halfExtents[cylinderUpAxis]; - - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) - { - d = radius / s; - tmp[XX] = v[XX] * d; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = v[ZZ] * d; - return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); - } else { - tmp[XX] = radius; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = btScalar(0.0); - return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); - } - } - case CAPSULE_SHAPE_PROXYTYPE: - { - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); - - btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - btScalar halfHeight = capsuleShape->getHalfHeight(); - int capsuleUpAxis = capsuleShape->getUpAxis(); - - btVector3 supVec(0,0,0); - - btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < SIMD_EPSILON*SIMD_EPSILON) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - btVector3 vtx; - btScalar newDot; - { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = halfHeight; - - vtx = pos; - newDot = vec.dot(vtx); - - - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = -halfHeight; - - vtx = pos; - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); - } - case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: - { - btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; - btVector3* points = convexPointCloudShape->getUnscaledPoints (); - int numPoints = convexPointCloudShape->getNumPoints (); - return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV()); - } - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - btConvexHullShape* convexHullShape = (btConvexHullShape*)this; - btVector3* points = convexHullShape->getUnscaledPoints(); - int numPoints = convexHullShape->getNumPoints (); - return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV()); - } - default: -#ifndef __SPU__ - return this->localGetSupportingVertexWithoutMargin (localDir); #else - btAssert (0); + return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), + btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), + btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); +#endif + } + case TRIANGLE_SHAPE_PROXYTYPE: + { + btTriangleShape* triangleShape = (btTriangleShape*)this; + btVector3 dir(localDir.getX(), localDir.getY(), localDir.getZ()); + btVector3* vertices = &triangleShape->m_vertices1[0]; + btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]); + btVector3 sup = vertices[dots.maxAxis()]; + return btVector3(sup.getX(), sup.getY(), sup.getZ()); + } + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShape* cylShape = (btCylinderShape*)this; + //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) + + btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); + btVector3 v(localDir.getX(), localDir.getY(), localDir.getZ()); + int cylinderUpAxis = cylShape->getUpAxis(); + int XX(1), YY(0), ZZ(2); + + switch (cylinderUpAxis) + { + case 0: + { + XX = 1; + YY = 0; + ZZ = 2; + } + break; + case 1: + { + XX = 0; + YY = 1; + ZZ = 2; + } + break; + case 2: + { + XX = 0; + YY = 2; + ZZ = 1; + } + break; + default: + btAssert(0); + break; + }; + + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; + + btVector3 tmp; + btScalar d; + + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) + { + d = radius / s; + tmp[XX] = v[XX] * d; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = v[ZZ] * d; + return btVector3(tmp.getX(), tmp.getY(), tmp.getZ()); + } + else + { + tmp[XX] = radius; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = btScalar(0.0); + return btVector3(tmp.getX(), tmp.getY(), tmp.getZ()); + } + } + case CAPSULE_SHAPE_PROXYTYPE: + { + btVector3 vec0(localDir.getX(), localDir.getY(), localDir.getZ()); + + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + btScalar halfHeight = capsuleShape->getHalfHeight(); + int capsuleUpAxis = capsuleShape->getUpAxis(); + + btVector3 supVec(0, 0, 0); + + btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < SIMD_EPSILON * SIMD_EPSILON) + { + vec.setValue(1, 0, 0); + } + else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); + vec *= rlen; + } + btVector3 vtx; + btScalar newDot; + { + btVector3 pos(0, 0, 0); + pos[capsuleUpAxis] = halfHeight; + + vtx = pos; + newDot = vec.dot(vtx); + + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + { + btVector3 pos(0, 0, 0); + pos[capsuleUpAxis] = -halfHeight; + + vtx = pos; + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return btVector3(supVec.getX(), supVec.getY(), supVec.getZ()); + } + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + { + btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; + btVector3* points = convexPointCloudShape->getUnscaledPoints(); + int numPoints = convexPointCloudShape->getNumPoints(); + return convexHullSupport(localDir, points, numPoints, convexPointCloudShape->getLocalScalingNV()); + } + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btConvexHullShape* convexHullShape = (btConvexHullShape*)this; + btVector3* points = convexHullShape->getUnscaledPoints(); + int numPoints = convexHullShape->getNumPoints(); + return convexHullSupport(localDir, points, numPoints, convexHullShape->getLocalScalingNV()); + } + default: +#ifndef __SPU__ + return this->localGetSupportingVertexWithoutMargin(localDir); +#else + btAssert(0); #endif } // should never reach here - btAssert (0); - return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); + btAssert(0); + return btVector3(btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); } -btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const +btVector3 btConvexShape::localGetSupportVertexNonVirtual(const btVector3& localDir) const { btVector3 localDirNorm = localDir; - if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (localDirNorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); + localDirNorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); } - localDirNorm.normalize (); + localDirNorm.normalize(); - return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm; + return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm) + getMarginNonVirtual() * localDirNorm; } /* TODO: This should be bumped up to btCollisionShape () */ -btScalar btConvexShape::getMarginNonVirtual () const +btScalar btConvexShape::getMarginNonVirtual() const { switch (m_shapeType) { - case SPHERE_SHAPE_PROXYTYPE: - { - btSphereShape* sphereShape = (btSphereShape*)this; - return sphereShape->getRadius (); - } - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - return convexShape->getMarginNV (); - } - case TRIANGLE_SHAPE_PROXYTYPE: - { - btTriangleShape* triangleShape = (btTriangleShape*)this; - return triangleShape->getMarginNV (); - } - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShape* cylShape = (btCylinderShape*)this; - return cylShape->getMarginNV(); - } - case CONE_SHAPE_PROXYTYPE: - { - btConeShape* conShape = (btConeShape*)this; - return conShape->getMarginNV(); - } - case CAPSULE_SHAPE_PROXYTYPE: - { - btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - return capsuleShape->getMarginNV(); - } - case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: - /* fall through */ - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; - return convexHullShape->getMarginNV(); - } - default: + case SPHERE_SHAPE_PROXYTYPE: + { + btSphereShape* sphereShape = (btSphereShape*)this; + return sphereShape->getRadius(); + } + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* convexShape = (btBoxShape*)this; + return convexShape->getMarginNV(); + } + case TRIANGLE_SHAPE_PROXYTYPE: + { + btTriangleShape* triangleShape = (btTriangleShape*)this; + return triangleShape->getMarginNV(); + } + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShape* cylShape = (btCylinderShape*)this; + return cylShape->getMarginNV(); + } + case CONE_SHAPE_PROXYTYPE: + { + btConeShape* conShape = (btConeShape*)this; + return conShape->getMarginNV(); + } + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + return capsuleShape->getMarginNV(); + } + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + /* fall through */ + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; + return convexHullShape->getMarginNV(); + } + default: #ifndef __SPU__ - return this->getMargin (); + return this->getMargin(); #else - btAssert (0); + btAssert(0); #endif } // should never reach here - btAssert (0); + btAssert(0); return btScalar(0.0f); } #ifndef __SPU__ -void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const +void btConvexShape::getAabbNonVirtual(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { switch (m_shapeType) { - case SPHERE_SHAPE_PROXYTYPE: - { - btSphereShape* sphereShape = (btSphereShape*)this; - btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); - btScalar margin = radius + sphereShape->getMarginNonVirtual(); - const btVector3& center = t.getOrigin(); - btVector3 extent(margin,margin,margin); - aabbMin = center - extent; - aabbMax = center + extent; - } - break; - case CYLINDER_SHAPE_PROXYTYPE: - /* fall through */ - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - btScalar margin=convexShape->getMarginNonVirtual(); - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - halfExtents += btVector3(margin,margin,margin); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - - aabbMin = center - extent; - aabbMax = center + extent; - break; - } - case TRIANGLE_SHAPE_PROXYTYPE: - { - btTriangleShape* triangleShape = (btTriangleShape*)this; - btScalar margin = triangleShape->getMarginNonVirtual(); - for (int i=0;i<3;i++) + case SPHERE_SHAPE_PROXYTYPE: { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); - vec[i] = btScalar(1.); + btSphereShape* sphereShape = (btSphereShape*)this; + btScalar radius = sphereShape->getImplicitShapeDimensions().getX(); // * convexShape->getLocalScaling().getX(); + btScalar margin = radius + sphereShape->getMarginNonVirtual(); + const btVector3& center = t.getOrigin(); + btVector3 extent(margin, margin, margin); + aabbMin = center - extent; + aabbMax = center + extent; + } + break; + case CYLINDER_SHAPE_PROXYTYPE: + /* fall through */ + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* convexShape = (btBoxShape*)this; + btScalar margin = convexShape->getMarginNonVirtual(); + btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); + halfExtents += btVector3(margin, margin, margin); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()); + aabbMin = center - extent; + aabbMax = center + extent; + break; + } + case TRIANGLE_SHAPE_PROXYTYPE: + { + btTriangleShape* triangleShape = (btTriangleShape*)this; + btScalar margin = triangleShape->getMarginNonVirtual(); + for (int i = 0; i < 3; i++) + { + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); + vec[i] = btScalar(1.); - btVector3 tmp = t(sv); - aabbMax[i] = tmp[i]+margin; - vec[i] = btScalar(-1.); - tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis())); - aabbMin[i] = tmp[i]-margin; - } - } - break; - case CAPSULE_SHAPE_PROXYTYPE: - { - btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius()); - int m_upAxis = capsuleShape->getUpAxis(); - halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - aabbMin = center - extent; - aabbMax = center + extent; - } - break; - case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this; - btScalar margin = convexHullShape->getMarginNonVirtual(); - convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin); - } - break; - default: + btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec * t.getBasis()); + + btVector3 tmp = t(sv); + aabbMax[i] = tmp[i] + margin; + vec[i] = btScalar(-1.); + tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec * t.getBasis())); + aabbMin[i] = tmp[i] - margin; + } + } + break; + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + btVector3 halfExtents(capsuleShape->getRadius(), capsuleShape->getRadius(), capsuleShape->getRadius()); + int m_upAxis = capsuleShape->getUpAxis(); + halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + aabbMin = center - extent; + aabbMax = center + extent; + } + break; + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this; + btScalar margin = convexHullShape->getMarginNonVirtual(); + convexHullShape->getNonvirtualAabb(t, aabbMin, aabbMax, margin); + } + break; + default: #ifndef __SPU__ - this->getAabb (t, aabbMin, aabbMax); + this->getAabb(t, aabbMin, aabbMax); #else - btAssert (0); + btAssert(0); #endif - break; + break; } // should never reach here - btAssert (0); + btAssert(0); } -#endif //__SPU__ +#endif //__SPU__ diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexShape.h index 875f2ac19..d3b3ed816 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -28,58 +28,48 @@ subject to the following restrictions: /// The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape, btConvexHullShape etc. /// It describes general convex shapes using the localGetSupportingVertex interface, used by collision detectors such as btGjkPairDetector. -ATTRIBUTE_ALIGNED16(class) btConvexShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btConvexShape : public btCollisionShape { - - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btConvexShape (); + btConvexShape(); virtual ~btConvexShape(); - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const = 0; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const = 0; - //////// - #ifndef __SPU__ - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const=0; - #endif //#ifndef __SPU__ +//////// +#ifndef __SPU__ + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const = 0; +#endif //#ifndef __SPU__ - btVector3 localGetSupportVertexWithoutMarginNonVirtual (const btVector3& vec) const; - btVector3 localGetSupportVertexNonVirtual (const btVector3& vec) const; - btScalar getMarginNonVirtual () const; - void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3& vec) const; + btVector3 localGetSupportVertexNonVirtual(const btVector3& vec) const; + btScalar getMarginNonVirtual() const; + void getAabbNonVirtual(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const; - virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; - - //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const = 0; ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0; - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0; - virtual void setLocalScaling(const btVector3& scaling) =0; - virtual const btVector3& getLocalScaling() const =0; + virtual void setLocalScaling(const btVector3& scaling) = 0; + virtual const btVector3& getLocalScaling() const = 0; - virtual void setMargin(btScalar margin)=0; + virtual void setMargin(btScalar margin) = 0; - virtual btScalar getMargin() const=0; + virtual btScalar getMargin() const = 0; - virtual int getNumPreferredPenetrationDirections() const=0; - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0; + virtual int getNumPreferredPenetrationDirections() const = 0; - - - + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const = 0; }; - - -#endif //BT_CONVEX_SHAPE_INTERFACE1 +#endif //BT_CONVEX_SHAPE_INTERFACE1 diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index 0f9ced554..f6987cc76 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -19,42 +19,37 @@ subject to the following restrictions: #include "LinearMath/btQuaternion.h" #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" - -btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) -: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) +btConvexTriangleMeshShape ::btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb) + : btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) { m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; - if ( calcAabb ) + if (calcAabb) recalcLocalAabb(); } - - - ///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once ///but then we are duplicating -class LocalSupportVertexCallback: public btInternalTriangleIndexCallback +class LocalSupportVertexCallback : public btInternalTriangleIndexCallback { - btVector3 m_supportVertexLocal; -public: +public: btScalar m_maxDot; btVector3 m_supportVecLocal; LocalSupportVertexCallback(const btVector3& supportVecLocal) - : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), - m_maxDot(btScalar(-BT_LARGE_FLOAT)), - m_supportVecLocal(supportVecLocal) + : m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)), + m_maxDot(btScalar(-BT_LARGE_FLOAT)), + m_supportVecLocal(supportVecLocal) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { (void)triangleIndex; (void)partId; - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { btScalar dot = m_supportVecLocal.dot(triangle[i]); if (dot > m_maxDot) @@ -64,99 +59,82 @@ public: } } } - - btVector3 GetSupportVertexLocal() + + btVector3 GetSupportVertexLocal() { return m_supportVertexLocal; } - }; - - - - -btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } - LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + LocalSupportVertexCallback supportCallback(vec); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax); supVec = supportCallback.GetSupportVertexLocal(); return supVec; } -void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { //use 'w' component of supportVerticesOut? { - for (int i=0;iInternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + LocalSupportVertexCallback supportCallback(vec); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax); supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); } - } - - -btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - - - - - - - - //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo -int btConvexTriangleMeshShape::getNumVertices() const +int btConvexTriangleMeshShape::getNumVertices() const { //cache this? return 0; - } int btConvexTriangleMeshShape::getNumEdges() const @@ -164,43 +142,39 @@ int btConvexTriangleMeshShape::getNumEdges() const return 0; } -void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const -{ - btAssert(0); -} - -void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const +void btConvexTriangleMeshShape::getEdge(int, btVector3&, btVector3&) const { btAssert(0); } -int btConvexTriangleMeshShape::getNumPlanes() const +void btConvexTriangleMeshShape::getVertex(int, btVector3&) const +{ + btAssert(0); +} + +int btConvexTriangleMeshShape::getNumPlanes() const { return 0; } -void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const +void btConvexTriangleMeshShape::getPlane(btVector3&, btVector3&, int) const { btAssert(0); } //not yet -bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const +bool btConvexTriangleMeshShape::isInside(const btVector3&, btScalar) const { btAssert(0); return false; } - - -void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) +void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_stridingMesh->setScaling(scaling); - - recalcLocalAabb(); - -} + recalcLocalAabb(); +} const btVector3& btConvexTriangleMeshShape::getLocalScaling() const { @@ -209,107 +183,101 @@ const btVector3& btConvexTriangleMeshShape::getLocalScaling() const void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const { - class CenterCallback: public btInternalTriangleIndexCallback - { - bool first; - btVector3 ref; - btVector3 sum; - btScalar volume; + class CenterCallback : public btInternalTriangleIndexCallback + { + bool first; + btVector3 ref; + btVector3 sum; + btScalar volume; - public: + public: + CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) + { + } - CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) - { - } + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + (void)triangleIndex; + (void)partId; + if (first) + { + ref = triangle[0]; + first = false; + } + else + { + btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); + sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); + volume += vol; + } + } - virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) - { - (void) triangleIndex; - (void) partId; - if (first) - { - ref = triangle[0]; - first = false; - } - else - { - btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); - sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); - volume += vol; - } - } - - btVector3 getCenter() - { - return (volume > 0) ? sum / volume : ref; - } + btVector3 getCenter() + { + return (volume > 0) ? sum / volume : ref; + } - btScalar getVolume() - { - return volume * btScalar(1. / 6); - } + btScalar getVolume() + { + return volume * btScalar(1. / 6); + } + }; - }; + class InertiaCallback : public btInternalTriangleIndexCallback + { + btMatrix3x3 sum; + btVector3 center; - class InertiaCallback: public btInternalTriangleIndexCallback - { - btMatrix3x3 sum; - btVector3 center; + public: + InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) + { + } - public: + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + (void)triangleIndex; + (void)partId; + btMatrix3x3 i; + btVector3 a = triangle[0] - center; + btVector3 b = triangle[1] - center; + btVector3 c = triangle[2] - center; + btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); + for (int j = 0; j < 3; j++) + { + for (int k = 0; k <= j; k++) + { + i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j])); + } + } + btScalar i00 = -i[0][0]; + btScalar i11 = -i[1][1]; + btScalar i22 = -i[2][2]; + i[0][0] = i11 + i22; + i[1][1] = i22 + i00; + i[2][2] = i00 + i11; + sum[0] += i[0]; + sum[1] += i[1]; + sum[2] += i[2]; + } - InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) - { - } + btMatrix3x3& getInertia() + { + return sum; + } + }; - virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) - { - (void) triangleIndex; - (void) partId; - btMatrix3x3 i; - btVector3 a = triangle[0] - center; - btVector3 b = triangle[1] - center; - btVector3 c = triangle[2] - center; - btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); - for (int j = 0; j < 3; j++) - { - for (int k = 0; k <= j; k++) - { - i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) - + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j])); - } - } - btScalar i00 = -i[0][0]; - btScalar i11 = -i[1][1]; - btScalar i22 = -i[2][2]; - i[0][0] = i11 + i22; - i[1][1] = i22 + i00; - i[2][2] = i00 + i11; - sum[0] += i[0]; - sum[1] += i[1]; - sum[2] += i[2]; - } - - btMatrix3x3& getInertia() - { - return sum; - } + CenterCallback centerCallback; + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); + btVector3 center = centerCallback.getCenter(); + principal.setOrigin(center); + volume = centerCallback.getVolume(); - }; + InertiaCallback inertiaCallback(center); + m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); - CenterCallback centerCallback; - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); - btVector3 center = centerCallback.getCenter(); - principal.setOrigin(center); - volume = centerCallback.getVolume(); - - InertiaCallback inertiaCallback(center); - m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); - - btMatrix3x3& i = inertiaCallback.getInertia(); - i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); - inertia.setValue(i[0][0], i[1][1], i[2][2]); - inertia /= volume; + btMatrix3x3& i = inertiaCallback.getInertia(); + i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); + inertia.setValue(i[0][0], i[1][1], i[2][2]); + inertia /= volume; } - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h index f338865ca..6dac9fff0 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -15,24 +15,22 @@ subject to the following restrictions: #ifndef BT_CONVEX_TRIANGLEMESH_SHAPE_H #define BT_CONVEX_TRIANGLEMESH_SHAPE_H - #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types - +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types /// The btConvexTriangleMeshShape is a convex hull of a triangle mesh, but the performance is not as good as btConvexHullShape. /// A small benefit of this class is that it uses the btStridingMeshInterface, so you can avoid the duplication of the triangle mesh data. Nevertheless, most users should use the much better performing btConvexHullShape instead. -ATTRIBUTE_ALIGNED16(class) btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape { - - class btStridingMeshInterface* m_stridingMesh; + class btStridingMeshInterface* m_stridingMesh; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb = true); - class btStridingMeshInterface* getMeshInterface() + btConvexTriangleMeshShape(btStridingMeshInterface * meshInterface, bool calcAabb = true); + + class btStridingMeshInterface* getMeshInterface() { return m_stridingMesh; } @@ -40,24 +38,23 @@ public: { return m_stridingMesh; } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - //debugging - virtual const char* getName()const {return "ConvexTrimesh";} - - virtual int getNumVertices() const; - virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; - - virtual void setLocalScaling(const btVector3& scaling); + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + + //debugging + virtual const char* getName() const { return "ConvexTrimesh"; } + + virtual int getNumVertices() const; + virtual int getNumEdges() const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; + virtual void getVertex(int i, btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; + + virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia @@ -65,13 +62,7 @@ public: ///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the ///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. This method also computes the volume of the convex mesh. - void calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const; - + void calculatePrincipalAxisTransform(btTransform & principal, btVector3 & inertia, btScalar & volume) const; }; - - -#endif //BT_CONVEX_TRIANGLEMESH_SHAPE_H - - - +#endif //BT_CONVEX_TRIANGLEMESH_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 604b3fc77..66dbb8e53 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -15,11 +15,11 @@ subject to the following restrictions: #include "btCylinderShape.h" -btCylinderShape::btCylinderShape (const btVector3& halfExtents) -:btConvexInternalShape(), -m_upAxis(1) +btCylinderShape::btCylinderShape(const btVector3& halfExtents) + : btConvexInternalShape(), + m_upAxis(1) { - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin; setSafeMargin(halfExtents); @@ -27,30 +27,25 @@ m_upAxis(1) m_shapeType = CYLINDER_SHAPE_PROXYTYPE; } - -btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents) -:btCylinderShape(halfExtents) +btCylinderShapeX::btCylinderShapeX(const btVector3& halfExtents) + : btCylinderShape(halfExtents) { m_upAxis = 0; - } - -btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents) -:btCylinderShape(halfExtents) +btCylinderShapeZ::btCylinderShapeZ(const btVector3& halfExtents) + : btCylinderShape(halfExtents) { m_upAxis = 2; - } -void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btCylinderShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); + btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax); } -void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btCylinderShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { - //Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility //#define USE_BOX_INERTIA_APPROXIMATION 1 #ifndef USE_BOX_INERTIA_APPROXIMATION @@ -64,25 +59,25 @@ void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co * */ - btScalar radius2; // square of cylinder radius - btScalar height2; // square of cylinder height - btVector3 halfExtents = getHalfExtentsWithMargin(); // get cylinder dimension + btScalar radius2; // square of cylinder radius + btScalar height2; // square of cylinder height + btVector3 halfExtents = getHalfExtentsWithMargin(); // get cylinder dimension btScalar div12 = mass / 12.f; btScalar div4 = mass / 4.f; btScalar div2 = mass / 2.f; int idxRadius, idxHeight; - switch (m_upAxis) // get indices of radius and height of cylinder + switch (m_upAxis) // get indices of radius and height of cylinder { - case 0: // cylinder is aligned along x + case 0: // cylinder is aligned along x idxRadius = 1; idxHeight = 0; break; - case 2: // cylinder is aligned along z + case 2: // cylinder is aligned along z idxRadius = 0; idxHeight = 2; break; - default: // cylinder is aligned along y + default: // cylinder is aligned along y idxRadius = 0; idxHeight = 1; } @@ -95,188 +90,164 @@ void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co btScalar t1 = div12 * height2 + div4 * radius2; btScalar t2 = div2 * radius2; - switch (m_upAxis) // set diagonal elements of inertia tensor + switch (m_upAxis) // set diagonal elements of inertia tensor { - case 0: // cylinder is aligned along x - inertia.setValue(t2,t1,t1); + case 0: // cylinder is aligned along x + inertia.setValue(t2, t1, t1); break; - case 2: // cylinder is aligned along z - inertia.setValue(t1,t1,t2); + case 2: // cylinder is aligned along z + inertia.setValue(t1, t1, t2); break; - default: // cylinder is aligned along y - inertia.setValue(t1,t2,t1); + default: // cylinder is aligned along y + inertia.setValue(t1, t2, t1); } -#else //USE_BOX_INERTIA_APPROXIMATION +#else //USE_BOX_INERTIA_APPROXIMATION //approximation of box shape btVector3 halfExtents = getHalfExtentsWithMargin(); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); -#endif //USE_BOX_INERTIA_APPROXIMATION + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); +#endif //USE_BOX_INERTIA_APPROXIMATION } - -SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) +SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents, const btVector3& v) { -const int cylinderUpAxis = 0; -const int XX = 1; -const int YY = 0; -const int ZZ = 2; + const int cylinderUpAxis = 0; + const int XX = 1; + const int YY = 0; + const int ZZ = 2; //mapping depends on how cylinder local orientation is // extents of the cylinder is: X,Y is for radius, and Z for height - btScalar radius = halfExtents[XX]; btScalar halfHeight = halfExtents[cylinderUpAxis]; + btVector3 tmp; + btScalar d; - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) { - d = radius / s; + d = radius / s; tmp[XX] = v[XX] * d; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = v[ZZ] * d; return tmp; } - else + else { - tmp[XX] = radius; + tmp[XX] = radius; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = btScalar(0.0); return tmp; - } - - + } } - - - - - -inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v) +inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents, const btVector3& v) { - -const int cylinderUpAxis = 1; -const int XX = 0; -const int YY = 1; -const int ZZ = 2; - + const int cylinderUpAxis = 1; + const int XX = 0; + const int YY = 1; + const int ZZ = 2; btScalar radius = halfExtents[XX]; btScalar halfHeight = halfExtents[cylinderUpAxis]; + btVector3 tmp; + btScalar d; - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) { - d = radius / s; + d = radius / s; tmp[XX] = v[XX] * d; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = v[ZZ] * d; return tmp; } - else + else { - tmp[XX] = radius; + tmp[XX] = radius; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = btScalar(0.0); return tmp; - } - + } } -inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v) +inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents, const btVector3& v) { -const int cylinderUpAxis = 2; -const int XX = 0; -const int YY = 2; -const int ZZ = 1; + const int cylinderUpAxis = 2; + const int XX = 0; + const int YY = 2; + const int ZZ = 1; //mapping depends on how cylinder local orientation is // extents of the cylinder is: X,Y is for radius, and Z for height - btScalar radius = halfExtents[XX]; btScalar halfHeight = halfExtents[cylinderUpAxis]; + btVector3 tmp; + btScalar d; - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) { - d = radius / s; + d = radius / s; tmp[XX] = v[XX] * d; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = v[ZZ] * d; return tmp; } - else + else { - tmp[XX] = radius; + tmp[XX] = radius; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = btScalar(0.0); return tmp; - } - - -} - -btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const -{ - return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec); -} - - -btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const -{ - return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec); -} -btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const -{ - return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec); -} - -void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - for (int i=0;im_convexInternalShapeData,serializer); + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); shapeData->m_upAxis = m_upAxis; @@ -213,7 +203,4 @@ SIMD_FORCE_INLINE const char* btCylinderShape::serialize(void* dataBuffer, btSer return "btCylinderShapeData"; } - - -#endif //BT_CYLINDER_MINKOWSKI_H - +#endif //BT_CYLINDER_MINKOWSKI_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.cpp index a9e6df5c5..4699555bd 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.cpp @@ -15,36 +15,28 @@ subject to the following restrictions: #include "btEmptyShape.h" - #include "btCollisionShape.h" - -btEmptyShape::btEmptyShape() : btConcaveShape () +btEmptyShape::btEmptyShape() : btConcaveShape() { m_shapeType = EMPTY_SHAPE_PROXYTYPE; } - btEmptyShape::~btEmptyShape() { } - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btEmptyShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); aabbMin = t.getOrigin() - margin; aabbMax = t.getOrigin() + margin; - } -void btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) const +void btEmptyShape::calculateLocalInertia(btScalar, btVector3&) const { btAssert(0); } - - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.h index 069a79402..d2e21173b 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.h @@ -23,50 +23,43 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" #include "btCollisionMargin.h" - - - /// The btEmptyShape is a collision shape without actual collision detection shape, so most users should ignore this class. /// It can be replaced by another shape during runtime, but the inertia tensor should be recomputed. -ATTRIBUTE_ALIGNED16(class) btEmptyShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btEmptyShape : public btConcaveShape { public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btEmptyShape(); virtual ~btEmptyShape(); - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } - virtual const btVector3& getLocalScaling() const + virtual const btVector3& getLocalScaling() const { return m_localScaling; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual const char* getName()const + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; + + virtual const char* getName() const { return "Empty"; } - virtual void processAllTriangles(btTriangleCallback* ,const btVector3& ,const btVector3& ) const + virtual void processAllTriangles(btTriangleCallback*, const btVector3&, const btVector3&) const { } protected: - btVector3 m_localScaling; - + btVector3 m_localScaling; }; - - -#endif //BT_EMPTY_SHAPE_H +#endif //BT_EMPTY_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 441a89c6b..34ec2d8c4 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -17,23 +17,17 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" - - -btHeightfieldTerrainShape::btHeightfieldTerrainShape -( -int heightStickWidth, int heightStickLength, const void* heightfieldData, -btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis, -PHY_ScalarType hdt, bool flipQuadEdges -) +btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, const void* heightfieldData, + btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType hdt, bool flipQuadEdges) { initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); } - - -btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,const void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) +btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges) { // legacy constructor: support only float or unsigned char, // and min height is zero @@ -45,27 +39,23 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int h btScalar heightScale = maxHeight / 65535; initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); } - - -void btHeightfieldTerrainShape::initialize -( -int heightStickWidth, int heightStickLength, const void* heightfieldData, -btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, -PHY_ScalarType hdt, bool flipQuadEdges -) +void btHeightfieldTerrainShape::initialize( + int heightStickWidth, int heightStickLength, const void* heightfieldData, + btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType hdt, bool flipQuadEdges) { // validation - btAssert(heightStickWidth > 1);// && "bad width"); - btAssert(heightStickLength > 1);// && "bad length"); - btAssert(heightfieldData);// && "null heightfield data"); + btAssert(heightStickWidth > 1); // && "bad width"); + btAssert(heightStickLength > 1); // && "bad length"); + btAssert(heightfieldData); // && "null heightfield data"); // btAssert(heightScale) -- do we care? Trust caller here - btAssert(minHeight <= maxHeight);// && "bad min/max height"); - btAssert(upAxis >= 0 && upAxis < 3);// && "bad upAxis--should be in range [0,2]"); - btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum"); + btAssert(minHeight <= maxHeight); // && "bad min/max height"); + btAssert(upAxis >= 0 && upAxis < 3); // && "bad upAxis--should be in range [0,2]"); + btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT); // && "Bad height data type enum"); // initialize member variables m_shapeType = TERRAIN_SHAPE_PROXYTYPE; @@ -73,42 +63,47 @@ PHY_ScalarType hdt, bool flipQuadEdges m_heightStickLength = heightStickLength; m_minHeight = minHeight; m_maxHeight = maxHeight; - m_width = (btScalar) (heightStickWidth - 1); - m_length = (btScalar) (heightStickLength - 1); + m_width = (btScalar)(heightStickWidth - 1); + m_length = (btScalar)(heightStickLength - 1); m_heightScale = heightScale; m_heightfieldDataUnknown = heightfieldData; m_heightDataType = hdt; m_flipQuadEdges = flipQuadEdges; m_useDiamondSubdivision = false; m_useZigzagSubdivision = false; + m_flipTriangleWinding = false; m_upAxis = upAxis; m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); + + m_vboundsChunkSize = 0; + m_vboundsGridWidth = 0; + m_vboundsGridLength = 0; // determine min/max axis-aligned bounding box (aabb) values switch (m_upAxis) { - case 0: + case 0: { m_localAabbMin.setValue(m_minHeight, 0, 0); m_localAabbMax.setValue(m_maxHeight, m_width, m_length); break; } - case 1: + case 1: { m_localAabbMin.setValue(0, m_minHeight, 0); m_localAabbMax.setValue(m_width, m_maxHeight, m_length); break; }; - case 2: + case 2: { m_localAabbMin.setValue(0, 0, m_minHeight); m_localAabbMax.setValue(m_width, m_length, m_maxHeight); break; } - default: + default: { //need to get valid m_upAxis - btAssert(0);// && "Bad m_upAxis"); + btAssert(0); // && "Bad m_upAxis"); } } @@ -116,62 +111,58 @@ PHY_ScalarType hdt, bool flipQuadEdges m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); } - - btHeightfieldTerrainShape::~btHeightfieldTerrainShape() { + clearAccelerator(); } - - -void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); + btVector3 halfExtents = (m_localAabbMax - m_localAabbMin) * m_localScaling * btScalar(0.5); btVector3 localOrigin(0, 0, 0); localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); localOrigin *= m_localScaling; - btMatrix3x3 abs_b = t.getBasis().absolute(); + btMatrix3x3 abs_b = t.getBasis().absolute(); btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - extent += btVector3(getMargin(),getMargin(),getMargin()); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + extent += btVector3(getMargin(), getMargin(), getMargin()); aabbMin = center - extent; aabbMax = center + extent; } - /// This returns the "raw" (user's initial) height, not the actual height. /// The actual height needs to be adjusted to be relative to the center /// of the heightfield's AABB. btScalar -btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const +btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const { btScalar val = 0.f; switch (m_heightDataType) { - case PHY_FLOAT: + case PHY_FLOAT: { - val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; + val = m_heightfieldDataFloat[(y * m_heightStickWidth) + x]; break; } - case PHY_UCHAR: + case PHY_UCHAR: { - unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; + unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x]; val = heightFieldValue * m_heightScale; break; } - case PHY_SHORT: + case PHY_SHORT: { short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; val = hfValue * m_heightScale; break; } - default: + default: { btAssert(!"Bad m_heightDataType"); } @@ -180,74 +171,63 @@ btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const return val; } - - - /// this returns the vertex in bullet-local coordinates -void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const +void btHeightfieldTerrainShape::getVertex(int x, int y, btVector3& vertex) const { - btAssert(x>=0); - btAssert(y>=0); - btAssert(x= 0); + btAssert(y >= 0); + btAssert(x < m_heightStickWidth); + btAssert(y < m_heightStickLength); - btScalar height = getRawHeightFieldValue(x,y); + btScalar height = getRawHeightFieldValue(x, y); switch (m_upAxis) { - case 0: - { - vertex.setValue( - height - m_localOrigin.getX(), - (-m_width/btScalar(2.0)) + x, - (-m_length/btScalar(2.0) ) + y - ); - break; - } - case 1: + case 0: { vertex.setValue( - (-m_width/btScalar(2.0)) + x, - height - m_localOrigin.getY(), - (-m_length/btScalar(2.0)) + y - ); + height - m_localOrigin.getX(), + (-m_width / btScalar(2.0)) + x, + (-m_length / btScalar(2.0)) + y); + break; + } + case 1: + { + vertex.setValue( + (-m_width / btScalar(2.0)) + x, + height - m_localOrigin.getY(), + (-m_length / btScalar(2.0)) + y); break; }; - case 2: + case 2: { vertex.setValue( - (-m_width/btScalar(2.0)) + x, - (-m_length/btScalar(2.0)) + y, - height - m_localOrigin.getZ() - ); + (-m_width / btScalar(2.0)) + x, + (-m_length / btScalar(2.0)) + y, + height - m_localOrigin.getZ()); break; } - default: + default: { //need to get valid m_upAxis btAssert(0); } } - vertex*=m_localScaling; + vertex *= m_localScaling; } - - static inline int -getQuantized -( -btScalar x -) +getQuantized( + btScalar x) { - if (x < 0.0) { - return (int) (x - 0.5); + if (x < 0.0) + { + return (int)(x - 0.5); } - return (int) (x + 0.5); + return (int)(x + 0.5); } - - /// given input vector, return quantized version /** This routine is basically determining the gridpoint indices for a given @@ -257,7 +237,7 @@ btScalar x "with clamp" means that we restrict the point to be in the heightfield's axis-aligned bounding box. */ -void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const +void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point, int /*isMax*/) const { btVector3 clampedPoint(point); clampedPoint.setMax(m_localAabbMin); @@ -266,11 +246,8 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi out[0] = getQuantized(clampedPoint.getX()); out[1] = getQuantized(clampedPoint.getY()); out[2] = getQuantized(clampedPoint.getZ()); - } - - /// process all triangles within the provided axis-aligned bounding box /** basic algorithm: @@ -278,128 +255,132 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi - convert input aabb to a range of heightfield grid points (quantize) - iterate over all triangles in that subset of the grid */ -void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { // scale down the input aabb's so they are in local (non-scaled) coordinates - btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); - btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); + btVector3 localAabbMin = aabbMin * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]); + btVector3 localAabbMax = aabbMax * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]); // account for local origin localAabbMin += m_localOrigin; localAabbMax += m_localOrigin; //quantize the aabbMin and aabbMax, and adjust the start/end ranges - int quantizedAabbMin[3]; - int quantizedAabbMax[3]; - quantizeWithClamp(quantizedAabbMin, localAabbMin,0); - quantizeWithClamp(quantizedAabbMax, localAabbMax,1); - + int quantizedAabbMin[3]; + int quantizedAabbMax[3]; + quantizeWithClamp(quantizedAabbMin, localAabbMin, 0); + quantizeWithClamp(quantizedAabbMax, localAabbMax, 1); + // expand the min/max quantized values // this is to catch the case where the input aabb falls between grid points! - for (int i = 0; i < 3; ++i) { + for (int i = 0; i < 3; ++i) + { quantizedAabbMin[i]--; quantizedAabbMax[i]++; - } + } - int startX=0; - int endX=m_heightStickWidth-1; - int startJ=0; - int endJ=m_heightStickLength-1; + int startX = 0; + int endX = m_heightStickWidth - 1; + int startJ = 0; + int endJ = m_heightStickLength - 1; switch (m_upAxis) { - case 0: + case 0: { - if (quantizedAabbMin[1]>startX) + if (quantizedAabbMin[1] > startX) startX = quantizedAabbMin[1]; - if (quantizedAabbMax[1]startJ) + if (quantizedAabbMin[2] > startJ) startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]startX) + if (quantizedAabbMin[0] > startX) startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]startJ) + if (quantizedAabbMin[2] > startJ) startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]startX) + if (quantizedAabbMin[0] > startX) startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]startJ) + if (quantizedAabbMin[1] > startJ) startJ = quantizedAabbMin[1]; - if (quantizedAabbMax[1]processTriangle(vertices,x,j); - //second triangle - // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman - getVertex(x+1,j+1,vertices[1]); - getVertex(x + 1, j, vertices[2]); - callback->processTriangle(vertices, x, j); + indices[0] = 2; + indices[2] = 0; + } - } else + if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1))) { - //first triangle - getVertex(x,j,vertices[0]); - getVertex(x,j+1,vertices[1]); - getVertex(x+1,j,vertices[2]); - callback->processTriangle(vertices,x,j); - //second triangle - getVertex(x+1,j,vertices[0]); - //getVertex(x,j+1,vertices[1]); - getVertex(x+1,j+1,vertices[2]); - callback->processTriangle(vertices,x,j); + //first triangle + getVertex(x, j, vertices[indices[0]]); + getVertex(x, j + 1, vertices[indices[1]]); + getVertex(x + 1, j + 1, vertices[indices[2]]); + callback->processTriangle(vertices, x, j); + //second triangle + // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman + getVertex(x + 1, j + 1, vertices[indices[1]]); + getVertex(x + 1, j, vertices[indices[2]]); + callback->processTriangle(vertices, x, j); + } + else + { + //first triangle + getVertex(x, j, vertices[indices[0]]); + getVertex(x, j + 1, vertices[indices[1]]); + getVertex(x + 1, j, vertices[indices[2]]); + callback->processTriangle(vertices, x, j); + //second triangle + getVertex(x + 1, j, vertices[indices[0]]); + //getVertex(x,j+1,vertices[1]); + getVertex(x + 1, j + 1, vertices[indices[2]]); + callback->processTriangle(vertices, x, j); } } } - - - } -void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const +void btHeightfieldTerrainShape::calculateLocalInertia(btScalar, btVector3& inertia) const { //moving concave objects not supported - - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } -void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) +void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } @@ -407,3 +388,458 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const { return m_localScaling; } + +namespace +{ + struct GridRaycastState + { + int x; // Next quad coords + int z; + int prev_x; // Previous quad coords + int prev_z; + btScalar param; // Exit param for previous quad + btScalar prevParam; // Enter param for previous quad + btScalar maxDistanceFlat; + btScalar maxDistance3d; + }; +} + +// TODO Does it really need to take 3D vectors? +/// Iterates through a virtual 2D grid of unit-sized square cells, +/// and executes an action on each cell intersecting the given segment, ordered from begin to end. +/// Initially inspired by http://www.cse.yorku.ca/~amana/research/grid.pdf +template +void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector3& endPos, int indices[3]) +{ + GridRaycastState rs; + rs.maxDistance3d = beginPos.distance(endPos); + if (rs.maxDistance3d < 0.0001) + { + // Consider the ray is too small to hit anything + return; + } + + + btScalar rayDirectionFlatX = endPos[indices[0]] - beginPos[indices[0]]; + btScalar rayDirectionFlatZ = endPos[indices[2]] - beginPos[indices[2]]; + rs.maxDistanceFlat = btSqrt(rayDirectionFlatX * rayDirectionFlatX + rayDirectionFlatZ * rayDirectionFlatZ); + + if (rs.maxDistanceFlat < 0.0001) + { + // Consider the ray vertical + rayDirectionFlatX = 0; + rayDirectionFlatZ = 0; + } + else + { + rayDirectionFlatX /= rs.maxDistanceFlat; + rayDirectionFlatZ /= rs.maxDistanceFlat; + } + + const int xiStep = rayDirectionFlatX > 0 ? 1 : rayDirectionFlatX < 0 ? -1 : 0; + const int ziStep = rayDirectionFlatZ > 0 ? 1 : rayDirectionFlatZ < 0 ? -1 : 0; + + const float infinite = 9999999; + const btScalar paramDeltaX = xiStep != 0 ? 1.f / btFabs(rayDirectionFlatX) : infinite; + const btScalar paramDeltaZ = ziStep != 0 ? 1.f / btFabs(rayDirectionFlatZ) : infinite; + + // pos = param * dir + btScalar paramCrossX; // At which value of `param` we will cross a x-axis lane? + btScalar paramCrossZ; // At which value of `param` we will cross a z-axis lane? + + // paramCrossX and paramCrossZ are initialized as being the first cross + // X initialization + if (xiStep != 0) + { + if (xiStep == 1) + { + paramCrossX = (ceil(beginPos[indices[0]]) - beginPos[indices[0]]) * paramDeltaX; + } + else + { + paramCrossX = (beginPos[indices[0]] - floor(beginPos[indices[0]])) * paramDeltaX; + } + } + else + { + paramCrossX = infinite; // Will never cross on X + } + + // Z initialization + if (ziStep != 0) + { + if (ziStep == 1) + { + paramCrossZ = (ceil(beginPos[indices[2]]) - beginPos[indices[2]]) * paramDeltaZ; + } + else + { + paramCrossZ = (beginPos[indices[2]] - floor(beginPos[indices[2]])) * paramDeltaZ; + } + } + else + { + paramCrossZ = infinite; // Will never cross on Z + } + + rs.x = static_cast(floor(beginPos[indices[0]])); + rs.z = static_cast(floor(beginPos[indices[2]])); + + // Workaround cases where the ray starts at an integer position + if (paramCrossX == 0.0) + { + paramCrossX += paramDeltaX; + // If going backwards, we should ignore the position we would get by the above flooring, + // because the ray is not heading in that direction + if (xiStep == -1) + { + rs.x -= 1; + } + } + + if (paramCrossZ == 0.0) + { + paramCrossZ += paramDeltaZ; + if (ziStep == -1) + rs.z -= 1; + } + + rs.prev_x = rs.x; + rs.prev_z = rs.z; + rs.param = 0; + + while (true) + { + rs.prev_x = rs.x; + rs.prev_z = rs.z; + rs.prevParam = rs.param; + + if (paramCrossX < paramCrossZ) + { + // X lane + rs.x += xiStep; + // Assign before advancing the param, + // to be in sync with the initialization step + rs.param = paramCrossX; + paramCrossX += paramDeltaX; + } + else + { + // Z lane + rs.z += ziStep; + rs.param = paramCrossZ; + paramCrossZ += paramDeltaZ; + } + + if (rs.param > rs.maxDistanceFlat) + { + rs.param = rs.maxDistanceFlat; + quadAction(rs); + break; + } + else + { + quadAction(rs); + } + } +} + +struct ProcessTrianglesAction +{ + const btHeightfieldTerrainShape* shape; + bool flipQuadEdges; + bool useDiamondSubdivision; + int width; + int length; + btTriangleCallback* callback; + + void exec(int x, int z) const + { + if (x < 0 || z < 0 || x >= width || z >= length) + { + return; + } + + btVector3 vertices[3]; + + // TODO Since this is for raycasts, we could greatly benefit from an early exit on the first hit + + // Check quad + if (flipQuadEdges || (useDiamondSubdivision && (((z + x) & 1) > 0))) + { + // First triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x + 1, z, vertices[1]); + shape->getVertex(x + 1, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + + // Second triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x + 1, z + 1, vertices[1]); + shape->getVertex(x, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + } + else + { + // First triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x, z + 1, vertices[1]); + shape->getVertex(x + 1, z, vertices[2]); + callback->processTriangle(vertices, x, z); + + // Second triangle + shape->getVertex(x + 1, z, vertices[0]); + shape->getVertex(x, z + 1, vertices[1]); + shape->getVertex(x + 1, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + } + } + + void operator()(const GridRaycastState& bs) const + { + exec(bs.prev_x, bs.prev_z); + } +}; + +struct ProcessVBoundsAction +{ + const btAlignedObjectArray& vbounds; + int width; + int length; + int chunkSize; + + btVector3 rayBegin; + btVector3 rayEnd; + btVector3 rayDir; + + int* m_indices; + ProcessTrianglesAction processTriangles; + + ProcessVBoundsAction(const btAlignedObjectArray& bnd, int* indices) + : vbounds(bnd), + m_indices(indices) + { + } + void operator()(const GridRaycastState& rs) const + { + int x = rs.prev_x; + int z = rs.prev_z; + + if (x < 0 || z < 0 || x >= width || z >= length) + { + return; + } + + const btHeightfieldTerrainShape::Range chunk = vbounds[x + z * width]; + + btVector3 enterPos; + btVector3 exitPos; + + if (rs.maxDistanceFlat > 0.0001) + { + btScalar flatTo3d = chunkSize * rs.maxDistance3d / rs.maxDistanceFlat; + btScalar enterParam3d = rs.prevParam * flatTo3d; + btScalar exitParam3d = rs.param * flatTo3d; + enterPos = rayBegin + rayDir * enterParam3d; + exitPos = rayBegin + rayDir * exitParam3d; + + // We did enter the flat projection of the AABB, + // but we have to check if we intersect it on the vertical axis + if (enterPos[1] > chunk.max && exitPos[m_indices[1]] > chunk.max) + { + return; + } + if (enterPos[1] < chunk.min && exitPos[m_indices[1]] < chunk.min) + { + return; + } + } + else + { + // Consider the ray vertical + // (though we shouldn't reach this often because there is an early check up-front) + enterPos = rayBegin; + exitPos = rayEnd; + } + + gridRaycast(processTriangles, enterPos, exitPos, m_indices); + // Note: it could be possible to have more than one grid at different levels, + // to do this there would be a branch using a pointer to another ProcessVBoundsAction + } +}; + +// TODO How do I interrupt the ray when there is a hit? `callback` does not return any result +/// Performs a raycast using a hierarchical Bresenham algorithm. +/// Does not allocate any memory by itself. +void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) const +{ + // Transform to cell-local + btVector3 beginPos = raySource / m_localScaling; + btVector3 endPos = rayTarget / m_localScaling; + beginPos += m_localOrigin; + endPos += m_localOrigin; + + ProcessTrianglesAction processTriangles; + processTriangles.shape = this; + processTriangles.flipQuadEdges = m_flipQuadEdges; + processTriangles.useDiamondSubdivision = m_useDiamondSubdivision; + processTriangles.callback = callback; + processTriangles.width = m_heightStickWidth - 1; + processTriangles.length = m_heightStickLength - 1; + + // TODO Transform vectors to account for m_upAxis + int indices[3] = { 0, 1, 2 }; + if (m_upAxis == 2) + { + indices[1] = 2; + indices[2] = 1; + } + int iBeginX = static_cast(floor(beginPos[indices[0]])); + int iBeginZ = static_cast(floor(beginPos[indices[2]])); + int iEndX = static_cast(floor(endPos[indices[0]])); + int iEndZ = static_cast(floor(endPos[indices[2]])); + + if (iBeginX == iEndX && iBeginZ == iEndZ) + { + // The ray will never cross quads within the plane, + // so directly process triangles within one quad + // (typically, vertical rays should end up here) + processTriangles.exec(iBeginX, iEndZ); + return; + } + + + + if (m_vboundsGrid.size()==0) + { + // Process all quads intersecting the flat projection of the ray + gridRaycast(processTriangles, beginPos, endPos, &indices[0]); + } + else + { + btVector3 rayDiff = endPos - beginPos; + btScalar flatDistance2 = rayDiff[indices[0]] * rayDiff[indices[0]] + rayDiff[indices[2]] * rayDiff[indices[2]]; + if (flatDistance2 < m_vboundsChunkSize * m_vboundsChunkSize) + { + // Don't use chunks, the ray is too short in the plane + gridRaycast(processTriangles, beginPos, endPos, &indices[0]); + } + + ProcessVBoundsAction processVBounds(m_vboundsGrid, &indices[0]); + processVBounds.width = m_vboundsGridWidth; + processVBounds.length = m_vboundsGridLength; + processVBounds.rayBegin = beginPos; + processVBounds.rayEnd = endPos; + processVBounds.rayDir = rayDiff.normalized(); + processVBounds.processTriangles = processTriangles; + processVBounds.chunkSize = m_vboundsChunkSize; + // The ray is long, run raycast on a higher-level grid + gridRaycast(processVBounds, beginPos / m_vboundsChunkSize, endPos / m_vboundsChunkSize, indices); + } +} + +/// Builds a grid data structure storing the min and max heights of the terrain in chunks. +/// if chunkSize is zero, that accelerator is removed. +/// If you modify the heights, you need to rebuild this accelerator. +void btHeightfieldTerrainShape::buildAccelerator(int chunkSize) +{ + if (chunkSize <= 0) + { + clearAccelerator(); + return; + } + + m_vboundsChunkSize = chunkSize; + int nChunksX = m_heightStickWidth / chunkSize; + int nChunksZ = m_heightStickLength / chunkSize; + + if (m_heightStickWidth % chunkSize > 0) + { + ++nChunksX; // In case terrain size isn't dividable by chunk size + } + if (m_heightStickLength % chunkSize > 0) + { + ++nChunksZ; + } + + if (m_vboundsGridWidth != nChunksX || m_vboundsGridLength != nChunksZ) + { + clearAccelerator(); + m_vboundsGridWidth = nChunksX; + m_vboundsGridLength = nChunksZ; + } + + if (nChunksX == 0 || nChunksZ == 0) + { + return; + } + + // This data structure is only reallocated if the required size changed + m_vboundsGrid.resize(nChunksX * nChunksZ); + + // Compute min and max height for all chunks + for (int cz = 0; cz < nChunksZ; ++cz) + { + int z0 = cz * chunkSize; + + for (int cx = 0; cx < nChunksX; ++cx) + { + int x0 = cx * chunkSize; + + Range r; + + r.min = getRawHeightFieldValue(x0, z0); + r.max = r.min; + + // Compute min and max height for this chunk. + // We have to include one extra cell to account for neighbors. + // Here is why: + // Say we have a flat terrain, and a plateau that fits a chunk perfectly. + // + // Left Right + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // x + // + // If the AABB for the Left chunk did not share vertices with the Right, + // then we would fail collision tests at x due to a gap. + // + for (int z = z0; z < z0 + chunkSize + 1; ++z) + { + if (z >= m_heightStickLength) + { + continue; + } + + for (int x = x0; x < x0 + chunkSize + 1; ++x) + { + if (x >= m_heightStickWidth) + { + continue; + } + + btScalar height = getRawHeightFieldValue(x, z); + + if (height < r.min) + { + r.min = height; + } + else if (height > r.max) + { + r.max = height; + } + } + } + + m_vboundsGrid[cx + cz * nChunksX] = r; + } + } +} + +void btHeightfieldTerrainShape::clearAccelerator() +{ + m_vboundsGrid.clear(); +} \ No newline at end of file diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 4a7a4a4bd..43e1d25e3 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -17,6 +17,7 @@ subject to the following restrictions: #define BT_HEIGHTFIELD_TERRAIN_SHAPE_H #include "btConcaveShape.h" +#include "LinearMath/btAlignedObjectArray.h" ///btHeightfieldTerrainShape simulates a 2D heightfield terrain /** @@ -68,43 +69,53 @@ subject to the following restrictions: For usage and testing see the TerrainDemo. */ -ATTRIBUTE_ALIGNED16(class) btHeightfieldTerrainShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btHeightfieldTerrainShape : public btConcaveShape { +public: + struct Range + { + btScalar min; + btScalar max; + }; + protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - btVector3 m_localOrigin; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + btVector3 m_localOrigin; ///terrain data - int m_heightStickWidth; + int m_heightStickWidth; int m_heightStickLength; - btScalar m_minHeight; - btScalar m_maxHeight; + btScalar m_minHeight; + btScalar m_maxHeight; btScalar m_width; btScalar m_length; btScalar m_heightScale; - union - { - const unsigned char* m_heightfieldDataUnsignedChar; - const short* m_heightfieldDataShort; - const btScalar* m_heightfieldDataFloat; - const void* m_heightfieldDataUnknown; + union { + const unsigned char* m_heightfieldDataUnsignedChar; + const short* m_heightfieldDataShort; + const btScalar* m_heightfieldDataFloat; + const void* m_heightfieldDataUnknown; }; - PHY_ScalarType m_heightDataType; - bool m_flipQuadEdges; - bool m_useDiamondSubdivision; + PHY_ScalarType m_heightDataType; + bool m_flipQuadEdges; + bool m_useDiamondSubdivision; bool m_useZigzagSubdivision; + bool m_flipTriangleWinding; + int m_upAxis; - int m_upAxis; - - btVector3 m_localScaling; - - virtual btScalar getRawHeightFieldValue(int x,int y) const; - void quantizeWithClamp(int* out, const btVector3& point,int isMax) const; - void getVertex(int x,int y,btVector3& vertex) const; + btVector3 m_localScaling; + // Accelerator + btAlignedObjectArray m_vboundsGrid; + int m_vboundsGridWidth; + int m_vboundsGridLength; + int m_vboundsChunkSize; + virtual btScalar getRawHeightFieldValue(int x, int y) const; + void quantizeWithClamp(int* out, const btVector3& point, int isMax) const; /// protected initialization /** @@ -112,25 +123,24 @@ protected: backwards-compatible without a lot of copy/paste. */ void initialize(int heightStickWidth, int heightStickLength, - const void* heightfieldData, btScalar heightScale, - btScalar minHeight, btScalar maxHeight, int upAxis, - PHY_ScalarType heightDataType, bool flipQuadEdges); + const void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType heightDataType, bool flipQuadEdges); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - + /// preferred constructor /** This constructor supports a range of heightfield data types, and allows for a non-zero minimum height value. heightScale is needed for any integer-based heightfield data types. */ - btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength, - const void* heightfieldData, btScalar heightScale, - btScalar minHeight, btScalar maxHeight, - int upAxis, PHY_ScalarType heightDataType, - bool flipQuadEdges); + btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, + const void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, + int upAxis, PHY_ScalarType heightDataType, + bool flipQuadEdges); /// legacy constructor /** @@ -139,29 +149,42 @@ public: compatibility reasons, heightScale is calculated as maxHeight / 65535 (and is only used when useFloatData = false). */ - btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,const void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); + btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges); virtual ~btHeightfieldTerrainShape(); + void setUseDiamondSubdivision(bool useDiamondSubdivision = true) { m_useDiamondSubdivision = useDiamondSubdivision; } - void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} + ///could help compatibility with Ogre heightfields. See https://code.google.com/p/bullet/issues/detail?id=625 + void setUseZigzagSubdivision(bool useZigzagSubdivision = true) { m_useZigzagSubdivision = useZigzagSubdivision; } - ///could help compatibility with Ogre heightfields. See https://code.google.com/p/bullet/issues/detail?id=625 - void setUseZigzagSubdivision(bool useZigzagSubdivision=true) { m_useZigzagSubdivision = useZigzagSubdivision;} + void setFlipTriangleWinding(bool flipTriangleWinding) + { + m_flipTriangleWinding = flipTriangleWinding; + } + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void setLocalScaling(const btVector3& scaling); - virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const; - - //debugging - virtual const char* getName()const {return "HEIGHTFIELD";} + void getVertex(int x, int y, btVector3& vertex) const; + + void performRaycast(btTriangleCallback * callback, const btVector3& raySource, const btVector3& rayTarget) const; + + void buildAccelerator(int chunkSize = 16); + void clearAccelerator(); + + int getUpAxis() const + { + return m_upAxis; + } + //debugging + virtual const char* getName() const { return "HEIGHTFIELD"; } }; -#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H +#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H \ No newline at end of file diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMaterial.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMaterial.h index 866f9b4da..c9a436bf2 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMaterial.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMaterial.h @@ -21,15 +21,18 @@ subject to the following restrictions: // Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties class btMaterial { - // public members so that materials can change due to world events + // public members so that materials can change due to world events public: - btScalar m_friction; - btScalar m_restitution; - int pad[2]; + btScalar m_friction; + btScalar m_restitution; + int pad[2]; - btMaterial(){} - btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; } + btMaterial() {} + btMaterial(btScalar fric, btScalar rest) + { + m_friction = fric; + m_restitution = rest; + } }; -#endif // BT_MATERIAL_H - +#endif // BT_MATERIAL_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.cpp new file mode 100644 index 000000000..13c0a343f --- /dev/null +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.cpp @@ -0,0 +1,522 @@ +#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 < n_nodes0; i++) + { + unsigned long long int n_nodes1; + ds.read(n_nodes1); + btAlignedObjectArray& nodes = m_nodes[i]; + nodes.resize(n_nodes1); + for (int j = 0; j < nodes.size(); j++) + { + double& node = nodes[j]; + ds.read(node); + } + } + + unsigned long long int n_cells0; + ds.read(n_cells0); + m_cells.resize(n_cells0); + for (int i = 0; i < n_cells0; i++) + { + unsigned long long int n_cells1; + btAlignedObjectArray& cells = m_cells[i]; + ds.read(n_cells1); + cells.resize(n_cells1); + for (int j = 0; j < n_cells1; j++) + { + btCell32& cell = cells[j]; + ds.read(cell); + } + } + + { + unsigned long long int n_cell_maps0; + ds.read(n_cell_maps0); + + m_cell_map.resize(n_cell_maps0); + for (int i = 0; i < n_cell_maps0; i++) + { + unsigned long long int n_cell_maps1; + btAlignedObjectArray& cell_maps = m_cell_map[i]; + ds.read(n_cell_maps1); + cell_maps.resize(n_cell_maps1); + for (int j = 0; j < n_cell_maps1; j++) + { + unsigned int& cell_map = cell_maps[j]; + ds.read(cell_map); + } + } + } + + m_isValid = (ds.m_currentOffset == ds.m_size); + return m_isValid; +} + +unsigned int btMiniSDF::multiToSingleIndex(btMultiIndex const& ijk) const +{ + return m_resolution[1] * m_resolution[0] * ijk.ijk[2] + m_resolution[0] * ijk.ijk[1] + ijk.ijk[0]; +} + +btAlignedBox3d +btMiniSDF::subdomain(btMultiIndex const& ijk) const +{ + btAssert(m_isValid); + btVector3 tmp; + tmp.m_floats[0] = m_cell_size[0] * (double)ijk.ijk[0]; + tmp.m_floats[1] = m_cell_size[1] * (double)ijk.ijk[1]; + tmp.m_floats[2] = m_cell_size[2] * (double)ijk.ijk[2]; + + btVector3 origin = m_domain.min() + tmp; + + btAlignedBox3d box = btAlignedBox3d(origin, origin + m_cell_size); + return box; +} + +btMultiIndex +btMiniSDF::singleToMultiIndex(unsigned int l) const +{ + btAssert(m_isValid); + unsigned int n01 = m_resolution[0] * m_resolution[1]; + unsigned int k = l / n01; + unsigned int temp = l % n01; + unsigned int j = temp / m_resolution[0]; + unsigned int i = temp % m_resolution[0]; + btMultiIndex mi; + mi.ijk[0] = i; + mi.ijk[1] = j; + mi.ijk[2] = k; + return mi; +} + +btAlignedBox3d +btMiniSDF::subdomain(unsigned int l) const +{ + btAssert(m_isValid); + return subdomain(singleToMultiIndex(l)); +} + +btShapeMatrix +btMiniSDF::shape_function_(btVector3 const& xi, btShapeGradients* gradient) const +{ + btAssert(m_isValid); + btShapeMatrix res; + + btScalar x = xi[0]; + btScalar y = xi[1]; + btScalar z = xi[2]; + + btScalar x2 = x * x; + btScalar y2 = y * y; + btScalar z2 = z * z; + + btScalar _1mx = 1.0 - x; + btScalar _1my = 1.0 - y; + btScalar _1mz = 1.0 - z; + + btScalar _1px = 1.0 + x; + btScalar _1py = 1.0 + y; + btScalar _1pz = 1.0 + z; + + btScalar _1m3x = 1.0 - 3.0 * x; + btScalar _1m3y = 1.0 - 3.0 * y; + btScalar _1m3z = 1.0 - 3.0 * z; + + btScalar _1p3x = 1.0 + 3.0 * x; + btScalar _1p3y = 1.0 + 3.0 * y; + btScalar _1p3z = 1.0 + 3.0 * z; + + btScalar _1mxt1my = _1mx * _1my; + btScalar _1mxt1py = _1mx * _1py; + btScalar _1pxt1my = _1px * _1my; + btScalar _1pxt1py = _1px * _1py; + + btScalar _1mxt1mz = _1mx * _1mz; + btScalar _1mxt1pz = _1mx * _1pz; + btScalar _1pxt1mz = _1px * _1mz; + btScalar _1pxt1pz = _1px * _1pz; + + btScalar _1myt1mz = _1my * _1mz; + btScalar _1myt1pz = _1my * _1pz; + btScalar _1pyt1mz = _1py * _1mz; + btScalar _1pyt1pz = _1py * _1pz; + + btScalar _1mx2 = 1.0 - x2; + btScalar _1my2 = 1.0 - y2; + btScalar _1mz2 = 1.0 - z2; + + // Corner nodes. + btScalar fac = 1.0 / 64.0 * (9.0 * (x2 + y2 + z2) - 19.0); + res[0] = fac * _1mxt1my * _1mz; + res[1] = fac * _1pxt1my * _1mz; + res[2] = fac * _1mxt1py * _1mz; + res[3] = fac * _1pxt1py * _1mz; + res[4] = fac * _1mxt1my * _1pz; + res[5] = fac * _1pxt1my * _1pz; + res[6] = fac * _1mxt1py * _1pz; + res[7] = fac * _1pxt1py * _1pz; + + // Edge nodes. + + fac = 9.0 / 64.0 * _1mx2; + btScalar fact1m3x = fac * _1m3x; + btScalar fact1p3x = fac * _1p3x; + res[8] = fact1m3x * _1myt1mz; + res[9] = fact1p3x * _1myt1mz; + res[10] = fact1m3x * _1myt1pz; + res[11] = fact1p3x * _1myt1pz; + res[12] = fact1m3x * _1pyt1mz; + res[13] = fact1p3x * _1pyt1mz; + res[14] = fact1m3x * _1pyt1pz; + res[15] = fact1p3x * _1pyt1pz; + + fac = 9.0 / 64.0 * _1my2; + btScalar fact1m3y = fac * _1m3y; + btScalar fact1p3y = fac * _1p3y; + res[16] = fact1m3y * _1mxt1mz; + res[17] = fact1p3y * _1mxt1mz; + res[18] = fact1m3y * _1pxt1mz; + res[19] = fact1p3y * _1pxt1mz; + res[20] = fact1m3y * _1mxt1pz; + res[21] = fact1p3y * _1mxt1pz; + res[22] = fact1m3y * _1pxt1pz; + res[23] = fact1p3y * _1pxt1pz; + + fac = 9.0 / 64.0 * _1mz2; + btScalar fact1m3z = fac * _1m3z; + btScalar fact1p3z = fac * _1p3z; + res[24] = fact1m3z * _1mxt1my; + res[25] = fact1p3z * _1mxt1my; + res[26] = fact1m3z * _1mxt1py; + res[27] = fact1p3z * _1mxt1py; + res[28] = fact1m3z * _1pxt1my; + res[29] = fact1p3z * _1pxt1my; + res[30] = fact1m3z * _1pxt1py; + res[31] = fact1p3z * _1pxt1py; + + if (gradient) + { + btShapeGradients& dN = *gradient; + + btScalar _9t3x2py2pz2m19 = 9.0 * (3.0 * x2 + y2 + z2) - 19.0; + btScalar _9tx2p3y2pz2m19 = 9.0 * (x2 + 3.0 * y2 + z2) - 19.0; + btScalar _9tx2py2p3z2m19 = 9.0 * (x2 + y2 + 3.0 * z2) - 19.0; + btScalar _18x = 18.0 * x; + btScalar _18y = 18.0 * y; + btScalar _18z = 18.0 * z; + + btScalar _3m9x2 = 3.0 - 9.0 * x2; + btScalar _3m9y2 = 3.0 - 9.0 * y2; + btScalar _3m9z2 = 3.0 - 9.0 * z2; + + btScalar _2x = 2.0 * x; + btScalar _2y = 2.0 * y; + btScalar _2z = 2.0 * z; + + btScalar _18xm9t3x2py2pz2m19 = _18x - _9t3x2py2pz2m19; + btScalar _18xp9t3x2py2pz2m19 = _18x + _9t3x2py2pz2m19; + btScalar _18ym9tx2p3y2pz2m19 = _18y - _9tx2p3y2pz2m19; + btScalar _18yp9tx2p3y2pz2m19 = _18y + _9tx2p3y2pz2m19; + btScalar _18zm9tx2py2p3z2m19 = _18z - _9tx2py2p3z2m19; + btScalar _18zp9tx2py2p3z2m19 = _18z + _9tx2py2p3z2m19; + + dN(0, 0) = _18xm9t3x2py2pz2m19 * _1myt1mz; + dN(0, 1) = _1mxt1mz * _18ym9tx2p3y2pz2m19; + dN(0, 2) = _1mxt1my * _18zm9tx2py2p3z2m19; + dN(1, 0) = _18xp9t3x2py2pz2m19 * _1myt1mz; + dN(1, 1) = _1pxt1mz * _18ym9tx2p3y2pz2m19; + dN(1, 2) = _1pxt1my * _18zm9tx2py2p3z2m19; + dN(2, 0) = _18xm9t3x2py2pz2m19 * _1pyt1mz; + dN(2, 1) = _1mxt1mz * _18yp9tx2p3y2pz2m19; + dN(2, 2) = _1mxt1py * _18zm9tx2py2p3z2m19; + dN(3, 0) = _18xp9t3x2py2pz2m19 * _1pyt1mz; + dN(3, 1) = _1pxt1mz * _18yp9tx2p3y2pz2m19; + dN(3, 2) = _1pxt1py * _18zm9tx2py2p3z2m19; + dN(4, 0) = _18xm9t3x2py2pz2m19 * _1myt1pz; + dN(4, 1) = _1mxt1pz * _18ym9tx2p3y2pz2m19; + dN(4, 2) = _1mxt1my * _18zp9tx2py2p3z2m19; + dN(5, 0) = _18xp9t3x2py2pz2m19 * _1myt1pz; + dN(5, 1) = _1pxt1pz * _18ym9tx2p3y2pz2m19; + dN(5, 2) = _1pxt1my * _18zp9tx2py2p3z2m19; + dN(6, 0) = _18xm9t3x2py2pz2m19 * _1pyt1pz; + dN(6, 1) = _1mxt1pz * _18yp9tx2p3y2pz2m19; + dN(6, 2) = _1mxt1py * _18zp9tx2py2p3z2m19; + dN(7, 0) = _18xp9t3x2py2pz2m19 * _1pyt1pz; + dN(7, 1) = _1pxt1pz * _18yp9tx2p3y2pz2m19; + dN(7, 2) = _1pxt1py * _18zp9tx2py2p3z2m19; + + dN.topRowsDivide(8, 64.0); + + btScalar _m3m9x2m2x = -_3m9x2 - _2x; + btScalar _p3m9x2m2x = _3m9x2 - _2x; + btScalar _1mx2t1m3x = _1mx2 * _1m3x; + btScalar _1mx2t1p3x = _1mx2 * _1p3x; + dN(8, 0) = _m3m9x2m2x * _1myt1mz, + dN(8, 1) = -_1mx2t1m3x * _1mz, + dN(8, 2) = -_1mx2t1m3x * _1my; + dN(9, 0) = _p3m9x2m2x * _1myt1mz, + dN(9, 1) = -_1mx2t1p3x * _1mz, + dN(9, 2) = -_1mx2t1p3x * _1my; + dN(10, 0) = _m3m9x2m2x * _1myt1pz, + dN(10, 1) = -_1mx2t1m3x * _1pz, + dN(10, 2) = _1mx2t1m3x * _1my; + dN(11, 0) = _p3m9x2m2x * _1myt1pz, + dN(11, 1) = -_1mx2t1p3x * _1pz, + dN(11, 2) = _1mx2t1p3x * _1my; + dN(12, 0) = _m3m9x2m2x * _1pyt1mz, + dN(12, 1) = _1mx2t1m3x * _1mz, + dN(12, 2) = -_1mx2t1m3x * _1py; + dN(13, 0) = _p3m9x2m2x * _1pyt1mz, + dN(13, 1) = _1mx2t1p3x * _1mz, + dN(13, 2) = -_1mx2t1p3x * _1py; + dN(14, 0) = _m3m9x2m2x * _1pyt1pz, + dN(14, 1) = _1mx2t1m3x * _1pz, + dN(14, 2) = _1mx2t1m3x * _1py; + dN(15, 0) = _p3m9x2m2x * _1pyt1pz, + dN(15, 1) = _1mx2t1p3x * _1pz, + dN(15, 2) = _1mx2t1p3x * _1py; + + btScalar _m3m9y2m2y = -_3m9y2 - _2y; + btScalar _p3m9y2m2y = _3m9y2 - _2y; + btScalar _1my2t1m3y = _1my2 * _1m3y; + btScalar _1my2t1p3y = _1my2 * _1p3y; + dN(16, 0) = -_1my2t1m3y * _1mz, + dN(16, 1) = _m3m9y2m2y * _1mxt1mz, + dN(16, 2) = -_1my2t1m3y * _1mx; + dN(17, 0) = -_1my2t1p3y * _1mz, + dN(17, 1) = _p3m9y2m2y * _1mxt1mz, + dN(17, 2) = -_1my2t1p3y * _1mx; + dN(18, 0) = _1my2t1m3y * _1mz, + dN(18, 1) = _m3m9y2m2y * _1pxt1mz, + dN(18, 2) = -_1my2t1m3y * _1px; + dN(19, 0) = _1my2t1p3y * _1mz, + dN(19, 1) = _p3m9y2m2y * _1pxt1mz, + dN(19, 2) = -_1my2t1p3y * _1px; + dN(20, 0) = -_1my2t1m3y * _1pz, + dN(20, 1) = _m3m9y2m2y * _1mxt1pz, + dN(20, 2) = _1my2t1m3y * _1mx; + dN(21, 0) = -_1my2t1p3y * _1pz, + dN(21, 1) = _p3m9y2m2y * _1mxt1pz, + dN(21, 2) = _1my2t1p3y * _1mx; + dN(22, 0) = _1my2t1m3y * _1pz, + dN(22, 1) = _m3m9y2m2y * _1pxt1pz, + dN(22, 2) = _1my2t1m3y * _1px; + dN(23, 0) = _1my2t1p3y * _1pz, + dN(23, 1) = _p3m9y2m2y * _1pxt1pz, + dN(23, 2) = _1my2t1p3y * _1px; + + btScalar _m3m9z2m2z = -_3m9z2 - _2z; + btScalar _p3m9z2m2z = _3m9z2 - _2z; + btScalar _1mz2t1m3z = _1mz2 * _1m3z; + btScalar _1mz2t1p3z = _1mz2 * _1p3z; + dN(24, 0) = -_1mz2t1m3z * _1my, + dN(24, 1) = -_1mz2t1m3z * _1mx, + dN(24, 2) = _m3m9z2m2z * _1mxt1my; + dN(25, 0) = -_1mz2t1p3z * _1my, + dN(25, 1) = -_1mz2t1p3z * _1mx, + dN(25, 2) = _p3m9z2m2z * _1mxt1my; + dN(26, 0) = -_1mz2t1m3z * _1py, + dN(26, 1) = _1mz2t1m3z * _1mx, + dN(26, 2) = _m3m9z2m2z * _1mxt1py; + dN(27, 0) = -_1mz2t1p3z * _1py, + dN(27, 1) = _1mz2t1p3z * _1mx, + dN(27, 2) = _p3m9z2m2z * _1mxt1py; + dN(28, 0) = _1mz2t1m3z * _1my, + dN(28, 1) = -_1mz2t1m3z * _1px, + dN(28, 2) = _m3m9z2m2z * _1pxt1my; + dN(29, 0) = _1mz2t1p3z * _1my, + dN(29, 1) = -_1mz2t1p3z * _1px, + dN(29, 2) = _p3m9z2m2z * _1pxt1my; + dN(30, 0) = _1mz2t1m3z * _1py, + dN(30, 1) = _1mz2t1m3z * _1px, + dN(30, 2) = _m3m9z2m2z * _1pxt1py; + dN(31, 0) = _1mz2t1p3z * _1py, + dN(31, 1) = _1mz2t1p3z * _1px, + dN(31, 2) = _p3m9z2m2z * _1pxt1py; + + dN.bottomRowsMul(32u - 8u, 9.0 / 64.0); + } + + return res; +} + +bool btMiniSDF::interpolate(unsigned int field_id, double& dist, btVector3 const& x, + btVector3* gradient) const +{ + btAssert(m_isValid); + if (!m_isValid) + return false; + + if (!m_domain.contains(x)) + return false; + + btVector3 tmpmi = ((x - m_domain.min()) * (m_inv_cell_size)); //.cast().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/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.h new file mode 100644 index 000000000..b60fd102f --- /dev/null +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.h @@ -0,0 +1,127 @@ +#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 < row; i++) + { + m_vec[i] /= denom; + } + } + + void bottomRowsMul(int row, double val) + { + for (int i = 32 - row; i < 32; i++) + { + m_vec[i] *= val; + } + } + + inline btScalar& operator()(int i, int j) + { + return m_vec[i][j]; + } +}; + +struct btCell32 +{ + unsigned int m_cells[32]; +}; + +struct btMiniSDF +{ + btAlignedBox3d m_domain; + unsigned int m_resolution[3]; + btVector3 m_cell_size; + btVector3 m_inv_cell_size; + std::size_t m_n_cells; + std::size_t m_n_fields; + bool m_isValid; + + btAlignedObjectArray > 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/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 899ef5005..d4b6a651d 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -13,65 +13,59 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btMinkowskiSumShape.h" - -btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB) -: btConvexInternalShape (), -m_shapeA(shapeA), -m_shapeB(shapeB) +btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA, const btConvexShape* shapeB) + : btConvexInternalShape(), + m_shapeA(shapeA), + m_shapeB(shapeB) { m_shapeType = MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE; m_transA.setIdentity(); m_transB.setIdentity(); } -btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis())); - btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec*m_transB.getBasis())); - return supVertexA - supVertexB; + btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec * m_transA.getBasis())); + btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec * m_transB.getBasis())); + return supVertexA - supVertexB; } -void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { ///@todo: could make recursive use of batching. probably this shape is not used frequently. - for (int i=0;igetMargin() + m_shapeB->getMargin(); } - -void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btMinkowskiSumShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { (void)mass; //inertia of the AABB of the Minkowski sum btTransform identity; identity.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(identity,aabbMin,aabbMax); + btVector3 aabbMin, aabbMax; + getAabb(identity, aabbMin, aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); + 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; + 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)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index a3f9a4723..3b5150f6d 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -17,46 +17,43 @@ subject to the following restrictions: #define BT_MINKOWSKI_SUM_SHAPE_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types /// The btMinkowskiSumShape is only for advanced users. This shape represents implicit based minkowski sum of two convex implicit shapes. -ATTRIBUTE_ALIGNED16(class) btMinkowskiSumShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btMinkowskiSumShape : public btConvexInternalShape { - - btTransform m_transA; - btTransform m_transB; - const btConvexShape* m_shapeA; - const btConvexShape* m_shapeB; + btTransform m_transA; + btTransform m_transB; + const btConvexShape* m_shapeA; + const btConvexShape* m_shapeB; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); -BT_DECLARE_ALIGNED_ALLOCATOR(); + btMinkowskiSumShape(const btConvexShape* shapeA, const btConvexShape* shapeB); - btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB); + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; + void setTransformA(const btTransform& transA) { m_transA = transA; } + void setTransformB(const btTransform& transB) { m_transB = transB; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + const btTransform& getTransformA() const { return m_transA; } + const btTransform& GetTransformB() const { return m_transB; } - void setTransformA(const btTransform& transA) { m_transA = transA;} - void setTransformB(const btTransform& transB) { m_transB = transB;} + virtual btScalar getMargin() const; - const btTransform& getTransformA()const { return m_transA;} - const btTransform& GetTransformB()const { return m_transB;} + const btConvexShape* getShapeA() const { return m_shapeA; } + const btConvexShape* getShapeB() const { return m_shapeB; } - - virtual btScalar getMargin() const; - - const btConvexShape* getShapeA() const { return m_shapeA;} - const btConvexShape* getShapeB() const { return m_shapeB;} - - virtual const char* getName()const + virtual const char* getName() const { return "MinkowskiSum"; } }; -#endif //BT_MINKOWSKI_SUM_SHAPE_H +#endif //BT_MINKOWSKI_SUM_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index 4195fa313..c0cc55dfb 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#if defined (_WIN32) || defined (__i386__) +#if defined(_WIN32) || defined(__i386__) #define BT_USE_SSE_IN_API #endif @@ -22,43 +22,41 @@ subject to the following restrictions: #include "LinearMath/btQuaternion.h" #include "LinearMath/btSerializer.h" -btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres) -:btConvexInternalAabbCachingShape () +btMultiSphereShape::btMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres) + : btConvexInternalAabbCachingShape() { m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE; //btScalar startMargin = btScalar(BT_LARGE_FLOAT); m_localPositionArray.resize(numSpheres); m_radiArray.resize(numSpheres); - for (int i=0;i maxDot ) + int inner_count = MIN(numSpheres - k, 128); + for (long i = 0; i < inner_count; i++) + { + temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin(); + pos++; + rad++; + } + long i = vec.maxDot(temp, inner_count, newDot); + if (newDot > maxDot) { maxDot = newDot; supVec = temp[i]; } - } + } return supVec; - } - void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - - for (int j=0;j maxDot ) - { - maxDot = newDot; - supportVerticesOut[j] = temp[i]; - } - } - + for (int k = 0; k < numSpheres; k += 128) + { + btVector3 temp[128]; + int inner_count = MIN(numSpheres - k, 128); + for (long i = 0; i < inner_count; i++) + { + temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin(); + pos++; + rad++; + } + long i = vec.maxDot(temp, inner_count, newDot); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = temp[i]; + } + } } } - - - - - - - -void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btMultiSphereShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //as an approximation, take the inertia of the box that bounds the spheres - btVector3 localAabbMin,localAabbMax; - getCachedLocalAabb(localAabbMin,localAabbMax); - btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5); + btVector3 localAabbMin, localAabbMax; + getCachedLocalAabb(localAabbMin, localAabbMax); + btVector3 halfExtents = (localAabbMax - localAabbMin) * btScalar(0.5); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer; + btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*)dataBuffer; btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); int numElem = m_localPositionArray.size(); - shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]): 0; - + shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]) : 0; + shapeData->m_localPositionArraySize = numElem; if (numElem) { - btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem); + btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius), numElem); btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr; - for (int i=0;im_pos); memPtr->m_radius = float(m_radiArray[i]); } - serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]); + serializer->finalizeChunk(chunk, "btPositionAndRadius", BT_ARRAY_CODE, (void*)&m_localPositionArray[0]); } // Fill padding with zeros to appease msan. @@ -181,5 +167,3 @@ const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serial return "btMultiSphereShapeData"; } - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index 5d3b40268..2d79c07ca 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -17,69 +17,65 @@ subject to the following restrictions: #define BT_MULTI_SPHERE_MINKOWSKI_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAabbUtil2.h" - - ///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes. ///It is possible to animate the spheres for deformation, but call 'recalcLocalAabb' after changing any sphere position/radius -ATTRIBUTE_ALIGNED16(class) btMultiSphereShape : public btConvexInternalAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btMultiSphereShape : public btConvexInternalAabbCachingShape { - btAlignedObjectArray m_localPositionArray; - btAlignedObjectArray m_radiArray; - + btAlignedObjectArray m_radiArray; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres); + + btMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres); ///CollisionShape Interface - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; /// btConvexShape Interface - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - int getSphereCount() const + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + + int getSphereCount() const { return m_localPositionArray.size(); } - const btVector3& getSpherePosition(int index) const + const btVector3& getSpherePosition(int index) const { return m_localPositionArray[index]; } - btScalar getSphereRadius(int index) const + btScalar getSphereRadius(int index) const { return m_radiArray[index]; } - - virtual const char* getName()const + virtual const char* getName() const { return "MultiSphere"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; - -struct btPositionAndRadius +struct btPositionAndRadius { - btVector3FloatData m_pos; - float m_radius; + btVector3FloatData m_pos; + float m_radius; }; +// clang-format off + struct btMultiSphereShapeData { btConvexInternalShapeData m_convexInternalShapeData; @@ -89,13 +85,11 @@ struct btMultiSphereShapeData char m_padding[4]; }; +// clang-format on - -SIMD_FORCE_INLINE int btMultiSphereShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btMultiSphereShape::calculateSerializeBufferSize() const { return sizeof(btMultiSphereShapeData); } - - -#endif //BT_MULTI_SPHERE_MINKOWSKI_H +#endif //BT_MULTI_SPHERE_MINKOWSKI_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp index 58799ac96..30108c9e7 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp @@ -19,27 +19,25 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h" //#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" - ///Obtains the material for a specific triangle -const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) +const btMaterial *btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) { - const unsigned char * materialBase = 0; - int numMaterials; - PHY_ScalarType materialType; - int materialStride; - const unsigned char * triangleMaterialBase = 0; - int numTriangles; - int triangleMaterialStride; - PHY_ScalarType triangleType; + const unsigned char *materialBase = 0; + int numMaterials; + PHY_ScalarType materialType; + int materialStride; + const unsigned char *triangleMaterialBase = 0; + int numTriangles; + int triangleMaterialStride; + PHY_ScalarType triangleType; - ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, - &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); + ((btTriangleIndexVertexMaterialArray *)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); - // return the pointer to the place with the friction for the triangle - // TODO: This depends on whether it's a moving mesh or not - // BUG IN GIMPACT - //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); - int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); - btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); - return (matVal); + // return the pointer to the place with the friction for the triangle + // TODO: This depends on whether it's a moving mesh or not + // BUG IN GIMPACT + //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); + int *matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); + btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); + return (matVal); } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h index 5ebaede4a..d1d42f8e0 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -22,82 +22,80 @@ subject to the following restrictions: #include "btMaterial.h" ///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution. -ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape +ATTRIBUTE_ALIGNED16(class) +btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape { - btAlignedObjectArray m_materialList; + btAlignedObjectArray m_materialList; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true): - btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) - { - m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + btMultimaterialTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true) : btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) + { + m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - const unsigned char *vertexbase; - int numverts; - PHY_ScalarType type; - int stride; - const unsigned char *indexbase; - int indexstride; - int numfaces; - PHY_ScalarType indicestype; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; - //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); + //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); - for(int i = 0; i < meshInterface->getNumSubParts(); i++) - { - m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase, - numverts, - type, - stride, - &indexbase, - indexstride, - numfaces, - indicestype, - i); - //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16)); - } - } + for (int i = 0; i < meshInterface->getNumSubParts(); i++) + { + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + i); + //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16)); + } + } ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb - btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true): - btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh) - { - m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + btMultimaterialTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, const btVector3 &bvhAabbMin, const btVector3 &bvhAabbMax, bool buildBvh = true) : btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh) + { + m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - const unsigned char *vertexbase; - int numverts; - PHY_ScalarType type; - int stride; - const unsigned char *indexbase; - int indexstride; - int numfaces; - PHY_ScalarType indicestype; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; - //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); + //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); - for(int i = 0; i < meshInterface->getNumSubParts(); i++) - { - m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase, - numverts, - type, - stride, - &indexbase, - indexstride, - numfaces, - indicestype, - i); - //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16)); - } - } - - virtual ~btMultimaterialTriangleMeshShape() - { -/* + for (int i = 0; i < meshInterface->getNumSubParts(); i++) + { + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + i); + //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16)); + } + } + + virtual ~btMultimaterialTriangleMeshShape() + { + /* for(int i = 0; i < m_meshInterface->getNumSubParts(); i++) { btAlignedFree(m_materialValues[i]); @@ -106,14 +104,12 @@ public: btAlignedFree(m_materialValues); m_materialLookup = NULL; */ - } + } //debugging - virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";} + virtual const char *getName() const { return "MULTIMATERIALTRIANGLEMESH"; } - ///Obtains the material for a specific triangle - const btMaterial * getMaterialProperties(int partID, int triIndex); + ///Obtains the material for a specific triangle + const btMaterial *getMaterialProperties(int partID, int triIndex); +}; -} -; - -#endif //BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H +#endif //BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 6f36775f7..687399e0a 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -13,51 +13,46 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btOptimizedBvh.h" #include "btStridingMeshInterface.h" #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btIDebugDraw.h" - btOptimizedBvh::btOptimizedBvh() -{ +{ } btOptimizedBvh::~btOptimizedBvh() { } - void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax) { m_useQuantization = useQuantizedAabbCompression; - // NodeArray triangleNodes; - struct NodeTriangleCallback : public btInternalTriangleIndexCallback + struct NodeTriangleCallback : public btInternalTriangleIndexCallback { - - NodeArray& m_triangleNodes; + NodeArray& m_triangleNodes; NodeTriangleCallback& operator=(NodeTriangleCallback& other) { m_triangleNodes.copyFromArray(other.m_triangleNodes); return *this; } - - NodeTriangleCallback(NodeArray& triangleNodes) - :m_triangleNodes(triangleNodes) + + NodeTriangleCallback(NodeArray& triangleNodes) + : m_triangleNodes(triangleNodes) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { btOptimizedBvhNode node; - btVector3 aabbMin,aabbMax; - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + btVector3 aabbMin, aabbMax; + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangle[0]); aabbMax.setMax(triangle[0]); aabbMin.setMin(triangle[1]); @@ -70,17 +65,17 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized node.m_aabbMaxOrg = aabbMax; node.m_escapeIndex = -1; - + //for child nodes node.m_subPart = partId; node.m_triangleIndex = triangleIndex; m_triangleNodes.push_back(node); } }; - struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback + struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback { - QuantizedNodeArray& m_triangleNodes; - const btQuantizedBvh* m_optimizedTree; // for quantization + QuantizedNodeArray& m_triangleNodes; + const btQuantizedBvh* m_optimizedTree; // for quantization QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other) { @@ -89,23 +84,23 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized return *this; } - QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btQuantizedBvh* tree) - :m_triangleNodes(triangleNodes),m_optimizedTree(tree) + QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes, const btQuantizedBvh* tree) + : m_triangleNodes(triangleNodes), m_optimizedTree(tree) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { // The partId and triangle index must fit in the same (positive) integer - btAssert(partId < (1<=0); + btAssert(triangleIndex >= 0); btQuantizedBvhNode node; - btVector3 aabbMin,aabbMax; - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + btVector3 aabbMin, aabbMax; + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangle[0]); aabbMax.setMax(triangle[0]); aabbMin.setMin(triangle[1]); @@ -132,59 +127,52 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION); } - m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); - m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); + 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; + 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); + setQuantizationValues(bvhAabbMin, bvhAabbMax); - QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this); + QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes, this); - - triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax); + 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 + m_quantizedContiguousNodes.resize(2 * numLeafNodes); + } + else { - NodeTriangleCallback callback(m_leafNodes); + NodeTriangleCallback callback(m_leafNodes); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); - triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); + triangles->InternalProcessAllTriangles(&callback, aabbMin, aabbMax); //now we have an array of leafnodes in m_leafNodes numLeafNodes = m_leafNodes.size(); - m_contiguousNodes.resize(2*numLeafNodes); + m_contiguousNodes.resize(2 * numLeafNodes); } m_curNodeIndex = 0; - buildTree(0,numLeafNodes); + 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()) + if (m_useQuantization && !m_SubtreeHeaders.size()) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); @@ -200,37 +188,29 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized m_leafNodes.clear(); } - - - -void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) +void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface, const btVector3& aabbMin, const btVector3& aabbMax) { if (m_useQuantization) { + setQuantizationValues(aabbMin, aabbMax); - setQuantizationValues(aabbMin,aabbMax); - - updateBvhNodes(meshInterface,0,m_curNodeIndex,0); + updateBvhNodes(meshInterface, 0, m_curNodeIndex, 0); ///now update all subtree headers int i; - for (i=0;im_SubtreeHeaders.size();i++) + for (i = 0; i < this->m_SubtreeHeaders.size(); i++) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; //PCK: unsigned instead of bool - unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax); if (overlap != 0) { - updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i); + updateBvhNodes(meshInterface, subtree.m_rootNodeIndex, subtree.m_rootNodeIndex + subtree.m_subtreeSize, i); subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); } } - } -void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index) +void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int firstNode, int endNode, int index) { (void)index; btAssert(m_useQuantization); - int curNodeSubPart=-1; + 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; + const unsigned char* vertexbase = 0; + int numverts = 0; + PHY_ScalarType type = PHY_INTEGER; + int stride = 0; + const unsigned char* indexbase = 0; + int indexstride = 0; + int numfaces = 0; + PHY_ScalarType indicestype = PHY_INTEGER; - btVector3 triangleVerts[3]; - btVector3 aabbMin,aabbMax; - const btVector3& meshScaling = meshInterface->getScaling(); - - int i; - for (i=endNode-1;i>=firstNode;i--) + btVector3 triangleVerts[3]; + btVector3 aabbMin, aabbMax; + const btVector3& meshScaling = meshInterface->getScaling(); + + int i; + for (i = endNode - 1; i >= firstNode; i--) + { + btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; + if (curNode.isLeafNode()) { - - - btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; - if (curNode.isLeafNode()) + //recalc aabb from triangle data + int nodeSubPart = curNode.getPartId(); + int nodeTriangleIndex = curNode.getTriangleIndex(); + if (nodeSubPart != curNodeSubPart) { - //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); + if (curNodeSubPart >= 0) + meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart); - curNodeSubPart = nodeSubPart; - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); - } - //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, + curNodeSubPart = nodeSubPart; + btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); + } + //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - - - for (int j=2;j>=0;j--) - { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - if (type == PHY_FLOAT) - { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); - } - else - { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); - } - } + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); - - - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMin.setMin(triangleVerts[0]); - aabbMax.setMax(triangleVerts[0]); - aabbMin.setMin(triangleVerts[1]); - aabbMax.setMax(triangleVerts[1]); - aabbMin.setMin(triangleVerts[2]); - aabbMax.setMax(triangleVerts[2]); - - quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0); - quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1); - - } else + for (int j = 2; j >= 0; j--) { - //combine aabb from both children - - btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1]; - - btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] : - &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()]; - - + int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + if (type == PHY_FLOAT) { - 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]; - } + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); + triangleVerts[j] = btVector3( + graphicsbase[0] * meshScaling.getX(), + graphicsbase[1] * meshScaling.getY(), + graphicsbase[2] * meshScaling.getZ()); + } + else + { + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ())); } } + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMin.setMin(triangleVerts[0]); + aabbMax.setMax(triangleVerts[0]); + aabbMin.setMin(triangleVerts[1]); + aabbMax.setMax(triangleVerts[1]); + aabbMin.setMin(triangleVerts[2]); + aabbMax.setMax(triangleVerts[2]); + + quantize(&curNode.m_quantizedAabbMin[0], aabbMin, 0); + quantize(&curNode.m_quantizedAabbMax[0], aabbMax, 1); } + else + { + //combine aabb from both children - if (curNodeSubPart >= 0) - meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); + btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i + 1]; - + btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i + 2] : &m_quantizedContiguousNodes[i + 1 + leftChildNode->getEscapeIndex()]; + + { + for (int i = 0; i < 3; i++) + { + curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i]; + if (curNode.m_quantizedAabbMin[i] > rightChildNode->m_quantizedAabbMin[i]) + curNode.m_quantizedAabbMin[i] = rightChildNode->m_quantizedAabbMin[i]; + + curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i]; + if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i]) + curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i]; + } + } + } + } + + if (curNodeSubPart >= 0) + meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); } ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' -btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) +btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) { - btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian); - + btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer, i_dataBufferSize, i_swapEndian); + //we don't add additional data so just do a static upcast return static_cast(bvh); } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index 715961f55..22f131c8b 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -22,44 +22,35 @@ subject to the following restrictions: class btStridingMeshInterface; - ///The btOptimizedBvh extends the btQuantizedBvh to create AABB tree for triangle meshes, through the btStridingMeshInterface. -ATTRIBUTE_ALIGNED16(class) btOptimizedBvh : public btQuantizedBvh +ATTRIBUTE_ALIGNED16(class) +btOptimizedBvh : public btQuantizedBvh { - public: BT_DECLARE_ALIGNED_ALLOCATOR(); protected: - public: - btOptimizedBvh(); virtual ~btOptimizedBvh(); - void build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax); + void build(btStridingMeshInterface * triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax); - void refit(btStridingMeshInterface* triangles,const btVector3& aabbMin,const btVector3& aabbMax); + void refit(btStridingMeshInterface * triangles, const btVector3& aabbMin, const btVector3& aabbMax); - void refitPartial(btStridingMeshInterface* triangles,const btVector3& aabbMin, const btVector3& aabbMax); + void refitPartial(btStridingMeshInterface * triangles, const btVector3& aabbMin, const btVector3& aabbMax); - void updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index); + void updateBvhNodes(btStridingMeshInterface * 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 + virtual bool serializeInPlace(void* o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const { - return btQuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian); - + return btQuantizedBvh::serialize(o_alignedDataBuffer, i_dataBufferSize, i_swapEndian); } ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' - static btOptimizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); - - + static btOptimizedBvh* deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); }; - -#endif //BT_OPTIMIZED_BVH_H - - +#endif //BT_OPTIMIZED_BVH_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 4854f370f..521ecfc76 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -12,7 +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. */ -#if defined (_WIN32) || defined (__i386__) +#if defined(_WIN32) || defined(__i386__) #define BT_USE_SSE_IN_API #endif @@ -23,11 +23,9 @@ subject to the following restrictions: #include "LinearMath/btGeometryUtil.h" #include "LinearMath/btGrahamScan2dConvexHull.h" - -btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(), -m_polyhedron(0) +btPolyhedralConvexShape::btPolyhedralConvexShape() : btConvexInternalShape(), + m_polyhedron(0) { - } btPolyhedralConvexShape::~btPolyhedralConvexShape() @@ -39,79 +37,148 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape() } } - -bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin) +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) +{ if (m_polyhedron) { m_polyhedron->~btConvexPolyhedron(); btAlignedFree(m_polyhedron); } - - void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16); + + void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16); m_polyhedron = new (mem) btConvexPolyhedron; btAlignedObjectArray orgVertices; - for (int i=0;i planeEquations; - btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); + btGeometryUtil::getPlaneEquationsFromVertices(orgVertices, planeEquations); btAlignedObjectArray shiftedPlaneEquations; - for (int p=0;p tmpVertices; - btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); - - conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); - } else + btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations, tmpVertices); + + conv.compute(&tmpVertices[0].getX(), sizeof(btVector3), tmpVertices.size(), 0.f, 0.f); + } + else { - - conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); + 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; p < numVertices; p++) + { + m_polyhedron->m_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; v < combinedFace.m_indices.size(); v++) + { + btScalar eq = m_polyhedron->m_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); btConvexHullComputer* convexUtil = &conv; - - btAlignedObjectArray tmpFaces; + btAlignedObjectArray tmpFaces; tmpFaces.resize(numFaces); int numVertices = convexUtil->vertices.size(); m_polyhedron->m_vertices.resize(numVertices); - for (int p=0;pm_vertices[p] = convexUtil->vertices[p]; } - - for (int i=0;ifaces[i]; //printf("face=%d\n",face); - const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; - const btConvexHullComputer::Edge* edge = firstEdge; + const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; + const btConvexHullComputer::Edge* edge = firstEdge; btVector3 edges[3]; int numEdges = 0; @@ -119,25 +186,23 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa do { - int src = edge->getSourceVertex(); tmpFaces[i].m_indices.push_back(src); int targ = edge->getTargetVertex(); btVector3 wa = convexUtil->vertices[src]; btVector3 wb = convexUtil->vertices[targ]; - btVector3 newEdge = wb-wa; + btVector3 newEdge = wb - wa; newEdge.normalize(); - if (numEdges<2) + if (numEdges < 2) edges[numEdges++] = newEdge; edge = edge->getNextEdgeOfFace(); - } while (edge!=firstEdge); + } while (edge != firstEdge); btScalar planeEq = 1e30f; - - if (numEdges==2) + if (numEdges == 2) { faceNormals[i] = edges[0].cross(edges[1]); faceNormals[i].normalize(); @@ -145,20 +210,19 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa tmpFaces[i].m_plane[1] = faceNormals[i].getY(); tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); tmpFaces[i].m_plane[3] = planeEq; - } else { - btAssert(0);//degenerate? + btAssert(0); //degenerate? faceNormals[i].setZero(); } - for (int v=0;vm_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]); - if (planeEq>eq) + if (planeEq > eq) { - planeEq=eq; + planeEq = eq; } } tmpFaces[i].m_plane[3] = -planeEq; @@ -166,89 +230,86 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa //merge coplanar faces and copy them to m_polyhedron - btScalar faceWeldThreshold= 0.999f; + btScalar faceWeldThreshold = 0.999f; btAlignedObjectArray todoFaces; - for (int i=0;i coplanarFaceGroup; - int refFace = todoFaces[todoFaces.size()-1]; + int refFace = todoFaces[todoFaces.size() - 1]; coplanarFaceGroup.push_back(refFace); btFace& faceA = tmpFaces[refFace]; todoFaces.pop_back(); - btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); - for (int j=todoFaces.size()-1;j>=0;j--) + btVector3 faceNormalA(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]; btFace& faceB = tmpFaces[i]; - btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); - if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) + btVector3 faceNormalB(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) + if (coplanarFaceGroup.size() > 1) { //do the merge: use Graham Scan 2d convex hull btAlignedObjectArray orgpoints; - btVector3 averageFaceNormal(0,0,0); + btVector3 averageFaceNormal(0, 0, 0); - for (int i=0;im_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + // m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); btFace& face = tmpFaces[coplanarFaceGroup[i]]; - btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]); - averageFaceNormal+=faceNormal; - for (int f=0;fm_vertices[orgIndex]; - + bool found = false; - for (int i=0;i hull; averageFaceNormal.normalize(); - GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal); + GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal); - for (int i=0;im_faces.push_back(combinedFace); } } - if(!did_merge) + if (!did_merge) { - for (int i=0;im_faces.push_back(face); } - - } - - - + } } - + +#endif //BT_RECONSTRUCT_FACES + m_polyhedron->initialize(); return true; } #ifndef MIN - #define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b)) +#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b)) #endif -btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - - - btVector3 supVec(0,0,0); + btVector3 supVec(0, 0, 0); #ifndef __SPU__ int i; btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); @@ -334,37 +394,36 @@ btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const b btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } btVector3 vtx; btScalar newDot; - for( int k = 0; k < getNumVertices(); k += 128 ) - { - btVector3 temp[128]; - int inner_count = MIN(getNumVertices() - k, 128); - for( i = 0; i < inner_count; i++ ) - getVertex(i,temp[i]); - i = (int) vec.maxDot( temp, inner_count, newDot); + for (int k = 0; k < getNumVertices(); k += 128) + { + btVector3 temp[128]; + int inner_count = MIN(getNumVertices() - k, 128); + for (i = 0; i < inner_count; i++) + getVertex(i, temp[i]); + i = (int)vec.maxDot(temp, inner_count, newDot); if (newDot > maxDot) { maxDot = newDot; supVec = temp[i]; - } - } - -#endif //__SPU__ + } + } + +#endif //__SPU__ return supVec; } - - -void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { #ifndef __SPU__ int i; @@ -372,36 +431,34 @@ void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin( btVector3 vtx; btScalar newDot; - for (i=0;i supportVerticesOut[j][3]) - { + const btVector3& vec = vectors[j]; + + for (int k = 0; k < getNumVertices(); k += 128) + { + btVector3 temp[128]; + int inner_count = MIN(getNumVertices() - k, 128); + for (i = 0; i < inner_count; i++) + getVertex(i, temp[i]); + i = (int)vec.maxDot(temp, inner_count, newDot); + if (newDot > supportVerticesOut[j][3]) + { supportVerticesOut[j] = temp[i]; supportVerticesOut[j][3] = newDot; - } - } - } + } + } + } -#endif //__SPU__ +#endif //__SPU__ } - - -void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { #ifndef __SPU__ //not yet, return box inertia @@ -410,81 +467,77 @@ void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& ine btTransform ident; ident.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(ident,aabbMin,aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); + btVector3 aabbMin, aabbMax; + getAabb(ident, aabbMin, aabbMax); + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); - btScalar lx=btScalar(2.)*(halfExtents.x()+margin); - btScalar ly=btScalar(2.)*(halfExtents.y()+margin); - btScalar lz=btScalar(2.)*(halfExtents.z()+margin); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = btScalar(2.) * (halfExtents.x() + margin); + btScalar ly = btScalar(2.) * (halfExtents.y() + margin); + btScalar lz = btScalar(2.) * (halfExtents.z() + margin); + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(0.08333333); - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); -#endif //__SPU__ + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); +#endif //__SPU__ } - - -void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling) +void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling) { btConvexInternalShape::setLocalScaling(scaling); recalcLocalAabb(); } btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape() -:btPolyhedralConvexShape(), -m_localAabbMin(1,1,1), -m_localAabbMax(-1,-1,-1), -m_isLocalAabbValid(false) + : btPolyhedralConvexShape(), + m_localAabbMin(1, 1, 1), + m_localAabbMax(-1, -1, -1), + m_isLocalAabbValid(false) { } -void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); + getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin()); } -void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() +void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() { m_isLocalAabbValid = true; - - #if 1 + +#if 1 static const btVector3 _directions[] = - { - btVector3( 1., 0., 0.), - btVector3( 0., 1., 0.), - btVector3( 0., 0., 1.), - btVector3( -1., 0., 0.), - btVector3( 0., -1., 0.), - btVector3( 0., 0., -1.) - }; - + { + btVector3(1., 0., 0.), + btVector3(0., 1., 0.), + btVector3(0., 0., 1.), + btVector3(-1., 0., 0.), + btVector3(0., -1., 0.), + btVector3(0., 0., -1.)}; + btVector3 _supporting[] = - { - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.) - }; - + { + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.)}; + batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); - - for ( int i = 0; i < 3; ++i ) + + for (int i = 0; i < 3; ++i) { m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin; m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin; } - - #else - for (int i=0;i<3;i++) +#else + + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); m_localAabbMax[i] = tmp[i]; @@ -492,9 +545,5 @@ void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() tmp = localGetSupportingVertex(vec); m_localAabbMin[i] = tmp[i]; } - #endif +#endif } - - - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 7bf8e01c1..b3ffab7a2 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -20,20 +20,15 @@ subject to the following restrictions: #include "btConvexInternalShape.h" class btConvexPolyhedron; - ///The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes. -ATTRIBUTE_ALIGNED16(class) btPolyhedralConvexShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btPolyhedralConvexShape : public btConvexInternalShape { - - protected: - btConvexPolyhedron* m_polyhedron; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btPolyhedralConvexShape(); @@ -41,52 +36,49 @@ public: ///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges) ///experimental/work-in-progress - virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0); + virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin = 0); - const btConvexPolyhedron* getConvexPolyhedron() const + virtual void setPolyhedralFeatures(btConvexPolyhedron & polyhedron); + + const btConvexPolyhedron* getConvexPolyhedron() const { return m_polyhedron; } //brute force implementations - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - - virtual int getNumVertices() const = 0 ; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; + + virtual int getNumVertices() const = 0; virtual int getNumEdges() const = 0; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const = 0; - virtual void getVertex(int i,btVector3& vtx) const = 0; - virtual int getNumPlanes() const = 0; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0; -// virtual int getIndex(int i) const = 0 ; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const = 0; + virtual void getVertex(int i, btVector3& vtx) const = 0; + virtual int getNumPlanes() const = 0; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const = 0; + // virtual int getIndex(int i) const = 0 ; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const = 0; - + virtual bool isInside(const btVector3& pt, btScalar tolerance) const = 0; }; - ///The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape class btPolyhedralConvexAabbCachingShape : public btPolyhedralConvexShape { + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - bool m_isLocalAabbValid; - protected: - - void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax) + void setCachedLocalAabb(const btVector3& aabbMin, const btVector3& aabbMax) { m_isLocalAabbValid = true; m_localAabbMin = aabbMin; m_localAabbMax = aabbMax; } - inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const + inline void getCachedLocalAabb(btVector3& aabbMin, btVector3& aabbMax) const { btAssert(m_isLocalAabbValid); aabbMin = m_localAabbMin; @@ -94,25 +86,21 @@ protected: } protected: - btPolyhedralConvexAabbCachingShape(); - -public: - - inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const - { +public: + inline void getNonvirtualAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax, btScalar margin) const + { //lazy evaluation of local aabb btAssert(m_isLocalAabbValid); - btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax); + btTransformAabb(m_localAabbMin, m_localAabbMax, margin, trans, aabbMin, aabbMax); } - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - void recalcLocalAabb(); + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + void recalcLocalAabb(); }; -#endif //BT_POLYHEDRAL_CONVEX_SHAPE_H +#endif //BT_POLYHEDRAL_CONVEX_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp index 6a337c786..f42731997 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -13,11 +13,10 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btScaledBvhTriangleMeshShape.h" -btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling) -:m_localScaling(localScaling),m_bvhTriMeshShape(childShape) +btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape, const btVector3& localScaling) + : m_localScaling(localScaling), m_bvhTriMeshShape(childShape) { m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; } @@ -26,55 +25,51 @@ btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() { } - class btScaledTriangleCallback : public btTriangleCallback { btTriangleCallback* m_originalCallback; - btVector3 m_localScaling; + btVector3 m_localScaling; public: - - btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling) - :m_originalCallback(originalCallback), - m_localScaling(localScaling) + btScaledTriangleCallback(btTriangleCallback* originalCallback, const btVector3& localScaling) + : m_originalCallback(originalCallback), + m_localScaling(localScaling) { } virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { btVector3 newTriangle[3]; - newTriangle[0] = triangle[0]*m_localScaling; - newTriangle[1] = triangle[1]*m_localScaling; - newTriangle[2] = triangle[2]*m_localScaling; - m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex); + newTriangle[0] = triangle[0] * m_localScaling; + newTriangle[1] = triangle[1] * m_localScaling; + newTriangle[2] = triangle[2] * m_localScaling; + m_originalCallback->processTriangle(&newTriangle[0], partId, triangleIndex); } }; -void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - btScaledTriangleCallback scaledCallback(callback,m_localScaling); - - btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ()); - btVector3 scaledAabbMin,scaledAabbMax; + btScaledTriangleCallback scaledCallback(callback, m_localScaling); + + btVector3 invLocalScaling(1.f / m_localScaling.getX(), 1.f / m_localScaling.getY(), 1.f / m_localScaling.getZ()); + btVector3 scaledAabbMin, scaledAabbMax; ///support negative scaling scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; scaledAabbMin[3] = 0.f; - + scaledAabbMax[0] = m_localScaling.getX() <= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; scaledAabbMax[1] = m_localScaling.getY() <= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; scaledAabbMax[3] = 0.f; - - - m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax); + + m_bvhTriMeshShape->processAllTriangles(&scaledCallback, scaledAabbMin, scaledAabbMax); } - -void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin(); btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax(); @@ -89,22 +84,21 @@ void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& a localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; - btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); + btVector3 localHalfExtents = btScalar(0.5) * (localAabbMax - localAabbMin); btScalar margin = m_bvhTriMeshShape->getMargin(); - localHalfExtents += btVector3(margin,margin,margin); - btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); - - btMatrix3x3 abs_b = trans.getBasis().absolute(); + localHalfExtents += btVector3(margin, margin, margin); + btVector3 localCenter = btScalar(0.5) * (localAabbMax + localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); aabbMin = center - extent; aabbMax = center + extent; - } -void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } @@ -114,8 +108,8 @@ const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const return m_localScaling; } -void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { ///don't make this a movable object! -// btAssert(0); + // btAssert(0); } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h index 39049eaf0..4d6feb61a 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -18,78 +18,69 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" - ///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape. ///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling' -ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btScaledBvhTriangleMeshShape : public btConcaveShape { - - - btVector3 m_localScaling; + btVector3 m_localScaling; - btBvhTriangleMeshShape* m_bvhTriMeshShape; + btBvhTriangleMeshShape* m_bvhTriMeshShape; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling); + btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape * childShape, const btVector3& localScaling); virtual ~btScaledBvhTriangleMeshShape(); - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling); + 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 void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - btBvhTriangleMeshShape* getChildShape() + btBvhTriangleMeshShape* getChildShape() { return m_bvhTriMeshShape; } - const btBvhTriangleMeshShape* getChildShape() const + const btBvhTriangleMeshShape* getChildShape() const { return m_bvhTriMeshShape; } //debugging - virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} + virtual const char* getName() const { return "SCALEDBVHTRIANGLEMESH"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btScaledTriangleMeshShapeData +struct btScaledTriangleMeshShapeData { - btTriangleMeshShapeData m_trimeshShapeData; + btTriangleMeshShapeData m_trimeshShapeData; - btVector3FloatData m_localScaling; + btVector3FloatData m_localScaling; }; - -SIMD_FORCE_INLINE int btScaledBvhTriangleMeshShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btScaledBvhTriangleMeshShape::calculateSerializeBufferSize() const { return sizeof(btScaledTriangleMeshShapeData); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btScaledBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btScaledBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btScaledTriangleMeshShapeData* scaledMeshData = (btScaledTriangleMeshShapeData*) dataBuffer; - m_bvhTriMeshShape->serialize(&scaledMeshData->m_trimeshShapeData,serializer); + btScaledTriangleMeshShapeData* scaledMeshData = (btScaledTriangleMeshShapeData*)dataBuffer; + m_bvhTriMeshShape->serialize(&scaledMeshData->m_trimeshShapeData, serializer); scaledMeshData->m_trimeshShapeData.m_collisionShapeData.m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; m_localScaling.serializeFloat(scaledMeshData->m_localScaling); return "btScaledTriangleMeshShapeData"; } - -#endif //BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H +#endif //BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp new file mode 100644 index 000000000..4a95dbea4 --- /dev/null +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp @@ -0,0 +1,92 @@ +#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/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h new file mode 100644 index 000000000..3989d6245 --- /dev/null +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h @@ -0,0 +1,29 @@ +#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/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btShapeHull.cpp index 3beaf8658..a2c490faf 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btShapeHull.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -15,51 +15,48 @@ subject to the following restrictions: //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) +btShapeHull::btShapeHull(const btConvexShape* shape) { m_shape = shape; - m_vertices.clear (); + m_vertices.clear(); m_indices.clear(); m_numIndices = 0; } -btShapeHull::~btShapeHull () +btShapeHull::~btShapeHull() { - m_indices.clear(); - m_vertices.clear (); + m_indices.clear(); + m_vertices.clear(); } -bool -btShapeHull::buildHull (btScalar /*margin*/) +bool btShapeHull::buildHull(btScalar /*margin*/, int highres) { - 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 numSampleDirections = highres ? NUM_UNITSPHERE_POINTS_HIGHRES : NUM_UNITSPHERE_POINTS; + 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()[i]); + supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]); } + int numPDA = m_shape->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int s = 0; s < numPDA; s++) + { + btVector3 norm; + m_shape->getPreferredPenetrationDirection(s, norm); + supportPoints[i++] = m_shape->localGetSupportingVertex(norm); + numSampleDirections++; + } + } HullDesc hd; hd.mFlags = QF_TRIANGLES; hd.mVcount = static_cast(numSampleDirections); @@ -69,18 +66,17 @@ btShapeHull::buildHull (btScalar /*margin*/) hd.mVertexStride = sizeof(btVector3); #else hd.mVertices = &supportPoints[0]; - hd.mVertexStride = sizeof (btVector3); + hd.mVertexStride = sizeof(btVector3); #endif HullLibrary hl; HullResult hr; - if (hl.CreateConvexHull (hd, hr) == QE_FAIL) + if (hl.CreateConvexHull(hd, hr) == QE_FAIL) { return false; } - m_vertices.resize (static_cast(hr.mNumOutputVertices)); - + m_vertices.resize(static_cast(hr.mNumOutputVertices)); for (i = 0; i < static_cast(hr.mNumOutputVertices); i++) { @@ -94,77 +90,332 @@ btShapeHull::buildHull (btScalar /*margin*/) } // free temporary hull result that we just copied - hl.ReleaseResult (hr); + hl.ReleaseResult(hr); return true; } -int -btShapeHull::numTriangles () const +int btShapeHull::numTriangles() const { return static_cast(m_numIndices / 3); } -int -btShapeHull::numVertices () const +int btShapeHull::numVertices() const { - return m_vertices.size (); + return m_vertices.size(); } -int -btShapeHull::numIndices () const +int btShapeHull::numIndices() const { return static_cast(m_numIndices); } - -btVector3* btShapeHull::getUnitSpherePoints() +btVector3* btShapeHull::getUnitSpherePoints(int highres) { - 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)) - }; + 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/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btShapeHull.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btShapeHull.h index e959f198b..54439f9ca 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btShapeHull.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btShapeHull.h @@ -21,32 +21,31 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" - ///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices. ///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object. ///It approximates the convex hull using the supporting vertex of 42 directions. -ATTRIBUTE_ALIGNED16(class) btShapeHull +ATTRIBUTE_ALIGNED16(class) +btShapeHull { protected: - btAlignedObjectArray m_vertices; btAlignedObjectArray m_indices; unsigned int m_numIndices; const btConvexShape* m_shape; - static btVector3* getUnitSpherePoints(); + static btVector3* getUnitSpherePoints(int highres = 0); public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btShapeHull (const btConvexShape* shape); - ~btShapeHull (); - bool buildHull (btScalar margin); + btShapeHull(const btConvexShape* shape); + ~btShapeHull(); - int numTriangles () const; - int numVertices () const; - int numIndices () const; + bool buildHull(btScalar margin, int highres = 0); + + int numTriangles() const; + int numVertices() const; + int numIndices() const; const btVector3* getVertexPointer() const { @@ -58,4 +57,4 @@ public: } }; -#endif //BT_SHAPE_HULL_H +#endif //BT_SHAPE_HULL_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSphereShape.cpp index b9a736c0f..027db2e10 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSphereShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSphereShape.cpp @@ -18,54 +18,48 @@ subject to the following restrictions: #include "LinearMath/btQuaternion.h" -btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { (void)vec; - return btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); + return btVector3(btScalar(0.), btScalar(0.), btScalar(0.)); } -void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { (void)vectors; - for (int i=0;iprocessTriangle(triangle,0,0); + callback->processTriangle(triangle, 0, 0); - triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius; - triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius; - triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius; - - callback->processTriangle(triangle,0,1); + triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; + triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius; + triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; + callback->processTriangle(triangle, 0, 1); } -void btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btStaticPlaneShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { (void)mass; //moving concave objects not supported - - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } -void btStaticPlaneShape::setLocalScaling(const btVector3& scaling) +void btStaticPlaneShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index 5e9eccc77..1cda8bbc7 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -18,78 +18,74 @@ subject to the following restrictions: #include "btConcaveShape.h" - ///The btStaticPlaneShape simulates an infinite non-moving (static) collision plane. -ATTRIBUTE_ALIGNED16(class) btStaticPlaneShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btStaticPlaneShape : public btConcaveShape { protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - - btVector3 m_planeNormal; - btScalar m_planeConstant; - btVector3 m_localScaling; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + + btVector3 m_planeNormal; + btScalar m_planeConstant; + btVector3 m_localScaling; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant); + btStaticPlaneShape(const btVector3& planeNormal, btScalar planeConstant); virtual ~btStaticPlaneShape(); + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; - - const btVector3& getPlaneNormal() const + + const btVector3& getPlaneNormal() const { - return m_planeNormal; + return m_planeNormal; } - const btScalar& getPlaneConstant() const + const btScalar& getPlaneConstant() const { - return m_planeConstant; + return m_planeConstant; } //debugging - virtual const char* getName()const {return "STATICPLANE";} + virtual const char* getName() const { return "STATICPLANE"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btStaticPlaneShapeData +struct btStaticPlaneShapeData { - btCollisionShapeData m_collisionShapeData; + btCollisionShapeData m_collisionShapeData; - btVector3FloatData m_localScaling; - btVector3FloatData m_planeNormal; - float m_planeConstant; - char m_pad[4]; + btVector3FloatData m_localScaling; + btVector3FloatData m_planeNormal; + float m_planeConstant; + char m_pad[4]; }; - -SIMD_FORCE_INLINE int btStaticPlaneShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btStaticPlaneShape::calculateSerializeBufferSize() const { return sizeof(btStaticPlaneShapeData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*) dataBuffer; - btCollisionShape::serialize(&planeData->m_collisionShapeData,serializer); + btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)dataBuffer; + btCollisionShape::serialize(&planeData->m_collisionShapeData, serializer); m_localScaling.serializeFloat(planeData->m_localScaling); m_planeNormal.serializeFloat(planeData->m_planeNormal); @@ -104,8 +100,4 @@ SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, bt return "btStaticPlaneShapeData"; } - -#endif //BT_STATIC_PLANE_SHAPE_H - - - +#endif //BT_STATIC_PLANE_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp index 78ddeb370..eb288e99c 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp @@ -18,32 +18,30 @@ subject to the following restrictions: btStridingMeshInterface::~btStridingMeshInterface() { - } - -void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { (void)aabbMin; (void)aabbMax; int numtotalphysicsverts = 0; - int part,graphicssubparts = getNumSubParts(); - const unsigned char * vertexbase; - const unsigned char * indexbase; + 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 stride, numverts, numtriangles; int gfxindex; btVector3 triangle[3]; btVector3 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; gfxindex < numtriangles; gfxindex++) + { + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + case PHY_UCHAR: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + default: + btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + } + break; + } - 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: - btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); - } - break; - } - - case PHY_DOUBLE: + case PHY_DOUBLE: { double* graphicsbase; switch (gfxindextype) { - case PHY_INTEGER: + case PHY_INTEGER: { - for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); + unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } - case PHY_SHORT: + case PHY_SHORT: { - for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } - case PHY_UCHAR: + case PHY_UCHAR: { - for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } - default: - btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + default: + btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); } break; } - default: - btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); + default: + btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); } unLockReadOnlyVertexBase(part); } } -void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax) +void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin, btVector3& aabbMax) { - - struct AabbCalculationCallback : public btInternalTriangleIndexCallback + struct AabbCalculationCallback : public btInternalTriangleIndexCallback { - btVector3 m_aabbMin; - btVector3 m_aabbMax; + btVector3 m_aabbMin; + btVector3 m_aabbMax; AabbCalculationCallback() { - m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { (void)partId; (void)triangleIndex; @@ -202,21 +198,19 @@ void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVecto }; //first calculate the total aabb for all triangles - AabbCalculationCallback aabbCallback; - aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax); + AabbCalculationCallback aabbCallback; + aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + InternalProcessAllTriangles(&aabbCallback, aabbMin, aabbMax); aabbMin = aabbCallback.m_aabbMin; aabbMax = aabbCallback.m_aabbMax; } - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const { - btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*) dataBuffer; + btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*)dataBuffer; trimeshData->m_numMeshParts = getNumSubParts(); @@ -226,29 +220,28 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s if (trimeshData->m_numMeshParts) { - btChunk* chunk = serializer->allocate(sizeof(btMeshPartData),trimeshData->m_numMeshParts); + btChunk* chunk = serializer->allocate(sizeof(btMeshPartData), trimeshData->m_numMeshParts); btMeshPartData* memPtr = (btMeshPartData*)chunk->m_oldPtr; - trimeshData->m_meshPartsPtr = (btMeshPartData *)serializer->getUniquePointer(memPtr); + trimeshData->m_meshPartsPtr = (btMeshPartData*)serializer->getUniquePointer(memPtr); - - // int numtotalphysicsverts = 0; - int part,graphicssubparts = getNumSubParts(); - const unsigned char * vertexbase; - const unsigned char * indexbase; + // 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 stride, numverts, numtriangles; int gfxindex; - // btVector3 triangle[3]; + // btVector3 triangle[3]; - // btVector3 meshScaling = getScaling(); + // btVector3 meshScaling = getScaling(); ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype - for (part=0;partm_numTriangles = numtriangles;//indices = 3*numtriangles + getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part); + memPtr->m_numTriangles = numtriangles; //indices = 3*numtriangles memPtr->m_numVertices = numverts; memPtr->m_indices16 = 0; memPtr->m_indices32 = 0; @@ -257,39 +250,38 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s memPtr->m_vertices3f = 0; memPtr->m_vertices3d = 0; - switch (gfxindextype) { - case PHY_INTEGER: + case PHY_INTEGER: { - int numindices = numtriangles*3; - + int numindices = numtriangles * 3; + if (numindices) { - btChunk* chunk = serializer->allocate(sizeof(btIntIndexData),numindices); + btChunk* chunk = serializer->allocate(sizeof(btIntIndexData), numindices); btIntIndexData* tmpIndices = (btIntIndexData*)chunk->m_oldPtr; memPtr->m_indices32 = (btIntIndexData*)serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindexfinalizeChunk(chunk,"btIntIndexData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btIntIndexData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } - case PHY_SHORT: + case PHY_SHORT: { if (numtriangles) { - btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData),numtriangles); + btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData), numtriangles); btShortIntIndexTripletData* tmpIndices = (btShortIntIndexTripletData*)chunk->m_oldPtr; - memPtr->m_3indices16 = (btShortIntIndexTripletData*) serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindexm_3indices16 = (btShortIntIndexTripletData*)serializer->getUniquePointer(tmpIndices); + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); tmpIndices[gfxindex].m_values[0] = tri_indices[0]; tmpIndices[gfxindex].m_values[1] = tri_indices[1]; tmpIndices[gfxindex].m_values[2] = tri_indices[2]; @@ -297,7 +289,7 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s tmpIndices[gfxindex].m_pad[0] = 0; tmpIndices[gfxindex].m_pad[1] = 0; } - serializer->finalizeChunk(chunk,"btShortIntIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btShortIntIndexTripletData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } @@ -305,23 +297,23 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s { if (numtriangles) { - btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData),numtriangles); + btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData), numtriangles); btCharIndexTripletData* tmpIndices = (btCharIndexTripletData*)chunk->m_oldPtr; - memPtr->m_3indices8 = (btCharIndexTripletData*) serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindexm_3indices8 = (btCharIndexTripletData*)serializer->getUniquePointer(tmpIndices); + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); 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); + serializer->finalizeChunk(chunk, "btCharIndexTripletData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } - default: + default: { btAssert(0); //unknown index type @@ -330,54 +322,54 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s switch (type) { - case PHY_FLOAT: - { - float* graphicsbase; - - if (numverts) - { - btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData),numverts); - btVector3FloatData* tmpVertices = (btVector3FloatData*) chunk->m_oldPtr; - memPtr->m_vertices3f = (btVector3FloatData *)serializer->getUniquePointer(tmpVertices); - for (int i=0;ifinalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); - } - break; - } - - case PHY_DOUBLE: + case PHY_FLOAT: { + float* graphicsbase; + if (numverts) { - btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData),numverts); - btVector3DoubleData* tmpVertices = (btVector3DoubleData*) chunk->m_oldPtr; - memPtr->m_vertices3d = (btVector3DoubleData *) serializer->getUniquePointer(tmpVertices); - for (int i=0;ifinalizeChunk(chunk,"btVector3DoubleData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData), numverts); + btVector3FloatData* tmpVertices = (btVector3FloatData*)chunk->m_oldPtr; + memPtr->m_vertices3f = (btVector3FloatData*)serializer->getUniquePointer(tmpVertices); + for (int i = 0; i < numverts; i++) + { + graphicsbase = (float*)(vertexbase + i * stride); + tmpVertices[i].m_floats[0] = graphicsbase[0]; + tmpVertices[i].m_floats[1] = graphicsbase[1]; + tmpVertices[i].m_floats[2] = graphicsbase[2]; + } + serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } - default: - btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); + case PHY_DOUBLE: + { + if (numverts) + { + btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData), numverts); + btVector3DoubleData* tmpVertices = (btVector3DoubleData*)chunk->m_oldPtr; + memPtr->m_vertices3d = (btVector3DoubleData*)serializer->getUniquePointer(tmpVertices); + for (int i = 0; i < numverts; i++) + { + double* graphicsbase = (double*)(vertexbase + i * stride); //for now convert to float, might leave it at double + tmpVertices[i].m_floats[0] = graphicsbase[0]; + tmpVertices[i].m_floats[1] = graphicsbase[1]; + tmpVertices[i].m_floats[2] = graphicsbase[2]; + } + serializer->finalizeChunk(chunk, "btVector3DoubleData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); + } + break; + } + + default: + btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); } unLockReadOnlyVertexBase(part); } - serializer->finalizeChunk(chunk,"btMeshPartData",BT_ARRAY_CODE,chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btMeshPartData", BT_ARRAY_CODE, chunk->m_oldPtr); } // Fill padding with zeros to appease msan. diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h index 9fbe13976..7d729ee0d 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h @@ -20,110 +20,102 @@ subject to the following restrictions: #include "btTriangleCallback.h" #include "btConcaveShape.h" - - - - /// The btStridingMeshInterface is the interface class for high performance generic access to triangle meshes, used in combination with btBvhTriangleMeshShape 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. -ATTRIBUTE_ALIGNED16(class ) btStridingMeshInterface +ATTRIBUTE_ALIGNED16(class) +btStridingMeshInterface { - protected: - - btVector3 m_scaling; +protected: + btVector3 m_scaling; - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btStridingMeshInterface() :m_scaling(btScalar(1.),btScalar(1.),btScalar(1.)) - { +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); - } + btStridingMeshInterface() : m_scaling(btScalar(1.), btScalar(1.), btScalar(1.)) + { + } - virtual ~btStridingMeshInterface(); + virtual ~btStridingMeshInterface(); + virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; + ///brute force method to calculate aabb + void calculateAabbBruteForce(btVector3 & aabbMin, btVector3 & aabbMax); - virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + /// 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; - ///brute force method to calculate aabb - void calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax); + 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; - /// 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; + /// 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; + 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; - /// 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 void preallocateVertices(int numverts)=0; - virtual void preallocateIndices(int numindices)=0; + virtual bool hasPremadeAabb() const { return false; } + virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax) const + { + (void)aabbMin; + (void)aabbMax; + } + virtual void getPremadeAabb(btVector3 * aabbMin, btVector3 * aabbMax) const + { + (void)aabbMin; + (void)aabbMax; + } - virtual bool hasPremadeAabb() const { return false; } - virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const - { - (void) aabbMin; - (void) aabbMax; - } - virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const - { - (void) aabbMin; - (void) aabbMax; - } - - const btVector3& getScaling() const { - return m_scaling; - } - void setScaling(const btVector3& 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, btSerializer* serializer) const; + const btVector3& getScaling() const + { + return m_scaling; + } + void setScaling(const btVector3& 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, btSerializer* serializer) const; }; -struct btIntIndexData +struct btIntIndexData { - int m_value; + int m_value; }; -struct btShortIntIndexData +struct btShortIntIndexData { short m_value; char m_pad[2]; }; -struct btShortIntIndexTripletData +struct btShortIntIndexTripletData { - short m_values[3]; - char m_pad[2]; + short m_values[3]; + char m_pad[2]; }; -struct btCharIndexTripletData +struct btCharIndexTripletData { unsigned char m_values[3]; - char m_pad; + char m_pad; }; +// clang-format off ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btMeshPartData @@ -151,14 +143,11 @@ struct btStridingMeshInterfaceData char m_padding[4]; }; +// clang-format on - - -SIMD_FORCE_INLINE int btStridingMeshInterface::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btStridingMeshInterface::calculateSerializeBufferSize() const { return sizeof(btStridingMeshInterfaceData); } - - -#endif //BT_STRIDING_MESHINTERFACE_H +#endif //BT_STRIDING_MESHINTERFACE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp index 52f346bf7..c4d33c429 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp @@ -16,29 +16,29 @@ subject to the following restrictions: #include "btTetrahedronShape.h" #include "LinearMath/btMatrix3x3.h" -btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); addVertex(pt1); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); @@ -46,8 +46,8 @@ m_numVertices(0) addVertex(pt2); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2, const btVector3& pt3) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); @@ -56,17 +56,16 @@ m_numVertices(0) addVertex(pt3); } - -void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btBU_Simplex1to4::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { #if 1 - btPolyhedralConvexAabbCachingShape::getAabb(t,aabbMin,aabbMax); + btPolyhedralConvexAabbCachingShape::getAabb(t, aabbMin, aabbMax); #else - aabbMin.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); - aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); + aabbMin.setValue(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); + aabbMax.setValue(-BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT); //just transform the vertices in worldspace, and take their AABB - for (int i=0;i m_triangleBuffer; - btAlignedObjectArray m_triangleBuffer; - public: - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - - int getNumTriangles() const + + int getNumTriangles() const { return int(m_triangleBuffer.size()); } - - const btTriangle& getTriangle(int index) const + + const btTriangle& getTriangle(int index) const { return m_triangleBuffer[index]; } - void clearBuffer() + void clearBuffer() { m_triangleBuffer.clear(); } - }; - -#endif //BT_TRIANGLE_BUFFER_H - +#endif //BT_TRIANGLE_BUFFER_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp index f558bf6d2..5bd2c595f 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp @@ -17,12 +17,8 @@ subject to the following restrictions: btTriangleCallback::~btTriangleCallback() { - } - btInternalTriangleIndexCallback::~btInternalTriangleIndexCallback() { - } - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.h index 461c57f87..d3644891e 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.h @@ -18,13 +18,11 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" - ///The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTriangles. ///This callback is called by processAllTriangles for all btConcaveShape derived class, such as btBvhTriangleMeshShape, btStaticPlaneShape and btHeightfieldTerrainShape. class btTriangleCallback { public: - virtual ~btTriangleCallback(); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) = 0; }; @@ -32,11 +30,8 @@ public: class btInternalTriangleIndexCallback { public: - virtual ~btInternalTriangleIndexCallback(); - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) = 0; + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) = 0; }; - - -#endif //BT_TRIANGLE_CALLBACK_H +#endif //BT_TRIANGLE_CALLBACK_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index a665024cb..dae425519 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -15,81 +15,76 @@ subject to the following restrictions: #include "btTriangleIndexVertexArray.h" -btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride) -: m_hasAabb(0) +btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, int numVertices, btScalar* vertexBase, int vertexStride) + : m_hasAabb(0) { btIndexedMesh mesh; mesh.m_numTriangles = numTriangles; - mesh.m_triangleIndexBase = (const unsigned char *)triangleIndexBase; + mesh.m_triangleIndexBase = (const unsigned char*)triangleIndexBase; mesh.m_triangleIndexStride = triangleIndexStride; mesh.m_numVertices = numVertices; - mesh.m_vertexBase = (const unsigned char *)vertexBase; + mesh.m_vertexBase = (const unsigned char*)vertexBase; mesh.m_vertexStride = vertexStride; addIndexedMesh(mesh); - } btTriangleIndexVertexArray::~btTriangleIndexVertexArray() { - } -void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) +void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char** vertexbase, int& numverts, PHY_ScalarType& type, int& vertexStride, unsigned char** indexbase, int& indexstride, int& numfaces, PHY_ScalarType& indicestype, int subpart) { - btAssert(subpart< getNumSubParts() ); + btAssert(subpart < getNumSubParts()); btIndexedMesh& mesh = m_indexedMeshes[subpart]; numverts = mesh.m_numVertices; - (*vertexbase) = (unsigned char *) mesh.m_vertexBase; + (*vertexbase) = (unsigned char*)mesh.m_vertexBase; - type = mesh.m_vertexType; + type = mesh.m_vertexType; vertexStride = mesh.m_vertexStride; numfaces = mesh.m_numTriangles; - (*indexbase) = (unsigned char *)mesh.m_triangleIndexBase; + (*indexbase) = (unsigned char*)mesh.m_triangleIndexBase; indexstride = mesh.m_triangleIndexStride; indicestype = mesh.m_indexType; } -void btTriangleIndexVertexArray::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 +void btTriangleIndexVertexArray::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 btIndexedMesh& mesh = m_indexedMeshes[subpart]; numverts = mesh.m_numVertices; - (*vertexbase) = (const unsigned char *)mesh.m_vertexBase; + (*vertexbase) = (const unsigned char*)mesh.m_vertexBase; + + type = mesh.m_vertexType; - type = mesh.m_vertexType; - vertexStride = mesh.m_vertexStride; numfaces = mesh.m_numTriangles; - (*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase; + (*indexbase) = (const unsigned char*)mesh.m_triangleIndexBase; indexstride = mesh.m_triangleIndexStride; indicestype = mesh.m_indexType; } -bool btTriangleIndexVertexArray::hasPremadeAabb() const +bool btTriangleIndexVertexArray::hasPremadeAabb() const { return (m_hasAabb == 1); } - -void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const +void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax) const { m_aabbMin = aabbMin; m_aabbMax = aabbMax; - m_hasAabb = 1; // this is intentionally an int see notes in header + m_hasAabb = 1; // this is intentionally an int see notes in header } -void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const +void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax) const { *aabbMin = m_aabbMin; *aabbMax = m_aabbMax; } - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index 9e1544e87..8ebb22baa 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -20,62 +20,59 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btScalar.h" - ///The btIndexedMesh indexes a single vertex and index array. Multiple btIndexedMesh objects can be passed into a btTriangleIndexVertexArray using addIndexedMesh. ///Instead of the number of indices, we pass the number of triangles. -ATTRIBUTE_ALIGNED16( struct) btIndexedMesh +ATTRIBUTE_ALIGNED16(struct) +btIndexedMesh { BT_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; + 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 - // btTriangleIndexVertexArray, do not set it manually - PHY_ScalarType m_indexType; + // The index type is set when adding an indexed mesh to the + // btTriangleIndexVertexArray, 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; + // 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; - - btIndexedMesh() - :m_indexType(PHY_INTEGER), + btIndexedMesh() + : m_indexType(PHY_INTEGER), #ifdef BT_USE_DOUBLE_PRECISION - m_vertexType(PHY_DOUBLE) -#else // BT_USE_DOUBLE_PRECISION - m_vertexType(PHY_FLOAT) -#endif // BT_USE_DOUBLE_PRECISION - { - } -} -; + m_vertexType(PHY_DOUBLE) +#else // BT_USE_DOUBLE_PRECISION + m_vertexType(PHY_FLOAT) +#endif // BT_USE_DOUBLE_PRECISION + { + } +}; - -typedef btAlignedObjectArray IndexedMeshArray; +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 +ATTRIBUTE_ALIGNED16(class) +btTriangleIndexVertexArray : public btStridingMeshInterface { protected: - IndexedMeshArray m_indexedMeshes; + IndexedMeshArray m_indexedMeshes; int m_pad[2]; - mutable int m_hasAabb; // using int instead of bool to maintain alignment + mutable int m_hasAabb; // using int instead of bool to maintain alignment mutable btVector3 m_aabbMin; mutable btVector3 m_aabbMax; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); btTriangleIndexVertexArray() : m_hasAabb(0) @@ -85,49 +82,47 @@ public: virtual ~btTriangleIndexVertexArray(); //just to be backwards compatible - btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride); - - void addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER) + btTriangleIndexVertexArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, int numVertices, btScalar* vertexBase, int vertexStride); + + void addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER) { m_indexedMeshes.push_back(mesh); - m_indexedMeshes[m_indexedMeshes.size()-1].m_indexType = indexType; + 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; + 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 unLockVertexBase(int subpart) { (void)subpart; } - virtual void unLockReadOnlyVertexBase(int subpart) const {(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 { + virtual int getNumSubParts() const + { return (int)m_indexedMeshes.size(); } - IndexedMeshArray& getIndexedMeshArray() + IndexedMeshArray& getIndexedMeshArray() { return m_indexedMeshes; } - const IndexedMeshArray& getIndexedMeshArray() const + const IndexedMeshArray& getIndexedMeshArray() const { return m_indexedMeshes; } - virtual void preallocateVertices(int numverts){(void) numverts;} - virtual void preallocateIndices(int numindices){(void) numindices;} + virtual void preallocateVertices(int numverts) { (void)numverts; } + virtual void preallocateIndices(int numindices) { (void)numindices; } - virtual bool hasPremadeAabb() const; - virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const; - virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const; + virtual bool hasPremadeAabb() const; + virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax) const; + virtual void getPremadeAabb(btVector3 * aabbMin, btVector3 * aabbMax) const; +}; -} -; - -#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H +#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp index dc562941a..4bf133d7a 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp @@ -17,70 +17,68 @@ subject to the following restrictions: #include "btTriangleIndexVertexMaterialArray.h" -btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, - int numVertices,btScalar* vertexBase,int vertexStride, - int numMaterials, unsigned char* materialBase, int materialStride, - int* triangleMaterialsBase, int materialIndexStride) : -btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride) +btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, + int numVertices, btScalar* vertexBase, int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride) : btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride) { - btMaterialProperties mat; + btMaterialProperties mat; - mat.m_numMaterials = numMaterials; - mat.m_materialBase = materialBase; - mat.m_materialStride = materialStride; + mat.m_numMaterials = numMaterials; + mat.m_materialBase = materialBase; + mat.m_materialStride = materialStride; #ifdef BT_USE_DOUBLE_PRECISION - mat.m_materialType = PHY_DOUBLE; + mat.m_materialType = PHY_DOUBLE; #else - mat.m_materialType = PHY_FLOAT; + mat.m_materialType = PHY_FLOAT; #endif - mat.m_numTriangles = numTriangles; - mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase; - mat.m_triangleMaterialStride = materialIndexStride; - mat.m_triangleType = PHY_INTEGER; + mat.m_numTriangles = numTriangles; + mat.m_triangleMaterialsBase = (unsigned char*)triangleMaterialsBase; + mat.m_triangleMaterialStride = materialIndexStride; + mat.m_triangleType = PHY_INTEGER; - addMaterialProperties(mat); + addMaterialProperties(mat); } - -void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) +void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) { - btAssert(subpart< getNumSubParts() ); + btAssert(subpart < getNumSubParts()); - btMaterialProperties& mats = m_materials[subpart]; + btMaterialProperties& mats = m_materials[subpart]; - numMaterials = mats.m_numMaterials; - (*materialBase) = (unsigned char *) mats.m_materialBase; + numMaterials = mats.m_numMaterials; + (*materialBase) = (unsigned char*)mats.m_materialBase; #ifdef BT_USE_DOUBLE_PRECISION - materialType = PHY_DOUBLE; + materialType = PHY_DOUBLE; #else - materialType = PHY_FLOAT; + materialType = PHY_FLOAT; #endif - materialStride = mats.m_materialStride; + materialStride = mats.m_materialStride; - numTriangles = mats.m_numTriangles; - (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase; - triangleMaterialStride = mats.m_triangleMaterialStride; - triangleType = mats.m_triangleType; + numTriangles = mats.m_numTriangles; + (*triangleMaterialBase) = (unsigned char*)mats.m_triangleMaterialsBase; + triangleMaterialStride = mats.m_triangleMaterialStride; + triangleType = mats.m_triangleType; } -void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) +void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + const unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) { - btMaterialProperties& mats = m_materials[subpart]; + btMaterialProperties& mats = m_materials[subpart]; - numMaterials = mats.m_numMaterials; - (*materialBase) = (const unsigned char *) mats.m_materialBase; + numMaterials = mats.m_numMaterials; + (*materialBase) = (const unsigned char*)mats.m_materialBase; #ifdef BT_USE_DOUBLE_PRECISION - materialType = PHY_DOUBLE; + materialType = PHY_DOUBLE; #else - materialType = PHY_FLOAT; + materialType = PHY_FLOAT; #endif - materialStride = mats.m_materialStride; + materialStride = mats.m_materialStride; - numTriangles = mats.m_numTriangles; - (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase; - triangleMaterialStride = mats.m_triangleMaterialStride; - triangleType = mats.m_triangleType; + numTriangles = mats.m_numTriangles; + (*triangleMaterialBase) = (const unsigned char*)mats.m_triangleMaterialsBase; + triangleMaterialStride = mats.m_triangleMaterialStride; + triangleType = mats.m_triangleType; } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h index ba4f7b460..315b1e21f 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h @@ -20,26 +20,26 @@ subject to the following restrictions: #include "btTriangleIndexVertexArray.h" - -ATTRIBUTE_ALIGNED16( struct) btMaterialProperties +ATTRIBUTE_ALIGNED16(struct) +btMaterialProperties { - ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution - int m_numMaterials; - const unsigned char * m_materialBase; - int m_materialStride; - PHY_ScalarType m_materialType; - ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're - /// padding the structure, it can be reproduced at no real cost - ///m_triangleMaterials =====> 1 integer value makes up one entry - /// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5 - int m_numTriangles; - const unsigned char * m_triangleMaterialsBase; - int m_triangleMaterialStride; - ///m_triangleType <========== Automatically set in addMaterialProperties - PHY_ScalarType m_triangleType; + ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution + int m_numMaterials; + const unsigned char* m_materialBase; + int m_materialStride; + PHY_ScalarType m_materialType; + ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're + /// padding the structure, it can be reproduced at no real cost + ///m_triangleMaterials =====> 1 integer value makes up one entry + /// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5 + int m_numTriangles; + const unsigned char* m_triangleMaterialsBase; + int m_triangleMaterialStride; + ///m_triangleType <========== Automatically set in addMaterialProperties + PHY_ScalarType m_triangleType; }; -typedef btAlignedObjectArray MaterialArray; +typedef btAlignedObjectArray MaterialArray; ///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray ///The addition of a material array allows for the utilization of the partID and @@ -47,38 +47,37 @@ typedef btAlignedObjectArray MaterialArray; ///TriangleIndexVertexArray, no duplicate is made of the material data, so it ///is the users responsibility to maintain the array during the lifetime of the ///TriangleIndexVertexMaterialArray. -ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray +ATTRIBUTE_ALIGNED16(class) +btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray { protected: - MaterialArray m_materials; - + MaterialArray m_materials; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btTriangleIndexVertexMaterialArray() + btTriangleIndexVertexMaterialArray() { } - btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, - int numVertices,btScalar* vertexBase,int vertexStride, - int numMaterials, unsigned char* materialBase, int materialStride, - int* triangleMaterialsBase, int materialIndexStride); + btTriangleIndexVertexMaterialArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, + int numVertices, btScalar* vertexBase, int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride); - virtual ~btTriangleIndexVertexMaterialArray() {} + virtual ~btTriangleIndexVertexMaterialArray() {} - void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER) - { - m_materials.push_back(mat); - m_materials[m_materials.size()-1].m_triangleType = triangleType; - } + void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER) + { + m_materials.push_back(mat); + m_materials[m_materials.size() - 1].m_triangleType = triangleType; + } - virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType ,int subpart = 0); + virtual void getLockedMaterialBase(unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0); - virtual void getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0); + virtual void getLockedReadOnlyMaterialBase(const unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + const unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0); +}; -} -; - -#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H +#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h index 642758959..8ee35ef5f 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h @@ -16,11 +16,9 @@ subject to the following restrictions: #ifndef _BT_TRIANGLE_INFO_MAP_H #define _BT_TRIANGLE_INFO_MAP_H - #include "LinearMath/btHashMap.h" #include "LinearMath/btSerializer.h" - ///for btTriangleInfo m_flags #define TRI_INFO_V0V1_CONVEX 1 #define TRI_INFO_V1V2_CONVEX 2 @@ -30,61 +28,58 @@ subject to the following restrictions: #define TRI_INFO_V1V2_SWAP_NORMALB 16 #define TRI_INFO_V2V0_SWAP_NORMALB 32 - ///The btTriangleInfo structure stores information to adjust collision normals to avoid collisions against internal edges -///it can be generated using -struct btTriangleInfo +///it can be generated using +struct btTriangleInfo { btTriangleInfo() { m_edgeV0V1Angle = SIMD_2_PI; m_edgeV1V2Angle = SIMD_2_PI; m_edgeV2V0Angle = SIMD_2_PI; - m_flags=0; + m_flags = 0; } - int m_flags; - - btScalar m_edgeV0V1Angle; - btScalar m_edgeV1V2Angle; - btScalar m_edgeV2V0Angle; + int m_flags; + btScalar m_edgeV0V1Angle; + btScalar m_edgeV1V2Angle; + btScalar m_edgeV2V0Angle; }; -typedef btHashMap btInternalTriangleInfoMap; - +typedef btHashMap btInternalTriangleInfoMap; ///The btTriangleInfoMap stores edge angle information for some triangles. You can compute this information yourself or using btGenerateInternalEdgeInfo. -struct btTriangleInfoMap : public btInternalTriangleInfoMap +struct btTriangleInfoMap : public btInternalTriangleInfoMap { - btScalar m_convexEpsilon;///used to determine if an edge or contact normal is convex, using the dot product - btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle - btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared' - btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge" - btScalar m_maxEdgeAngleThreshold; //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold - btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold) - - + btScalar m_convexEpsilon; ///used to determine if an edge or contact normal is convex, using the dot product + btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle + btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared' + btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge" + btScalar m_maxEdgeAngleThreshold; //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold + btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold) + btTriangleInfoMap() { m_convexEpsilon = 0.00f; m_planarEpsilon = 0.0001f; - m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001); + m_equalVertexThreshold = btScalar(0.0001) * btScalar(0.0001); m_edgeDistanceThreshold = btScalar(0.1); - m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001); + m_zeroAreaThreshold = btScalar(0.0001) * btScalar(0.0001); m_maxEdgeAngleThreshold = SIMD_2_PI; } virtual ~btTriangleInfoMap() {} - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - void deSerialize(struct btTriangleInfoMapData& data); + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + void deSerialize(struct btTriangleInfoMapData& data); }; +// clang-format off + ///those fields have to be float and not btScalar for the serialization to work properly struct btTriangleInfoData { @@ -114,86 +109,86 @@ struct btTriangleInfoMapData char m_padding[4]; }; -SIMD_FORCE_INLINE int btTriangleInfoMap::calculateSerializeBufferSize() const +// clang-format on + +SIMD_FORCE_INLINE int btTriangleInfoMap::calculateSerializeBufferSize() const { return sizeof(btTriangleInfoMapData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btTriangleInfoMap::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btTriangleInfoMap::serialize(void* dataBuffer, btSerializer* serializer) const { - btTriangleInfoMapData* tmapData = (btTriangleInfoMapData*) dataBuffer; + btTriangleInfoMapData* tmapData = (btTriangleInfoMapData*)dataBuffer; tmapData->m_convexEpsilon = (float)m_convexEpsilon; tmapData->m_planarEpsilon = (float)m_planarEpsilon; - tmapData->m_equalVertexThreshold =(float) m_equalVertexThreshold; + tmapData->m_equalVertexThreshold = (float)m_equalVertexThreshold; tmapData->m_edgeDistanceThreshold = (float)m_edgeDistanceThreshold; tmapData->m_zeroAreaThreshold = (float)m_zeroAreaThreshold; - + tmapData->m_hashTableSize = m_hashTable.size(); tmapData->m_hashTablePtr = tmapData->m_hashTableSize ? (int*)serializer->getUniquePointer((void*)&m_hashTable[0]) : 0; if (tmapData->m_hashTablePtr) - { + { //serialize an int buffer int sz = sizeof(int); int numElem = tmapData->m_hashTableSize; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;ifinalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_hashTable[0]); - + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_hashTable[0]); } tmapData->m_nextSize = m_next.size(); - tmapData->m_nextPtr = tmapData->m_nextSize? (int*)serializer->getUniquePointer((void*)&m_next[0]): 0; + tmapData->m_nextPtr = tmapData->m_nextSize ? (int*)serializer->getUniquePointer((void*)&m_next[0]) : 0; if (tmapData->m_nextPtr) { int sz = sizeof(int); int numElem = tmapData->m_nextSize; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;ifinalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_next[0]); + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_next[0]); } - + tmapData->m_numValues = m_valueArray.size(); - tmapData->m_valueArrayPtr = tmapData->m_numValues ? (btTriangleInfoData*)serializer->getUniquePointer((void*)&m_valueArray[0]): 0; + tmapData->m_valueArrayPtr = tmapData->m_numValues ? (btTriangleInfoData*)serializer->getUniquePointer((void*)&m_valueArray[0]) : 0; if (tmapData->m_valueArrayPtr) { int sz = sizeof(btTriangleInfoData); int numElem = tmapData->m_numValues; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btTriangleInfoData* memPtr = (btTriangleInfoData*)chunk->m_oldPtr; - for (int i=0;im_edgeV0V1Angle = (float)m_valueArray[i].m_edgeV0V1Angle; memPtr->m_edgeV1V2Angle = (float)m_valueArray[i].m_edgeV1V2Angle; memPtr->m_edgeV2V0Angle = (float)m_valueArray[i].m_edgeV2V0Angle; memPtr->m_flags = m_valueArray[i].m_flags; } - serializer->finalizeChunk(chunk,"btTriangleInfoData",BT_ARRAY_CODE,(void*) &m_valueArray[0]); + serializer->finalizeChunk(chunk, "btTriangleInfoData", BT_ARRAY_CODE, (void*)&m_valueArray[0]); } - + tmapData->m_numKeys = m_keyArray.size(); tmapData->m_keyArrayPtr = tmapData->m_numKeys ? (int*)serializer->getUniquePointer((void*)&m_keyArray[0]) : 0; if (tmapData->m_keyArrayPtr) { int sz = sizeof(int); int numElem = tmapData->m_numValues; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;ifinalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*) &m_keyArray[0]); - + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_keyArray[0]); } // Fill padding with zeros to appease msan. @@ -205,44 +200,39 @@ SIMD_FORCE_INLINE const char* btTriangleInfoMap::serialize(void* dataBuffer, btS return "btTriangleInfoMapData"; } - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE void btTriangleInfoMap::deSerialize(btTriangleInfoMapData& tmapData ) +SIMD_FORCE_INLINE void btTriangleInfoMap::deSerialize(btTriangleInfoMapData& tmapData) { - - m_convexEpsilon = tmapData.m_convexEpsilon; m_planarEpsilon = tmapData.m_planarEpsilon; m_equalVertexThreshold = tmapData.m_equalVertexThreshold; m_edgeDistanceThreshold = tmapData.m_edgeDistanceThreshold; m_zeroAreaThreshold = tmapData.m_zeroAreaThreshold; m_hashTable.resize(tmapData.m_hashTableSize); - int i =0; - for (i=0;i m_4componentVertices; - btAlignedObjectArray m_3componentVertices; + btAlignedObjectArray m_4componentVertices; + btAlignedObjectArray m_3componentVertices; - btAlignedObjectArray m_32bitIndices; - btAlignedObjectArray m_16bitIndices; - bool m_use32bitIndices; - bool m_use4componentVertices; - + btAlignedObjectArray m_32bitIndices; + btAlignedObjectArray m_16bitIndices; + bool m_use32bitIndices; + bool m_use4componentVertices; - public: - btScalar m_weldingThreshold; +public: + btScalar m_weldingThreshold; - btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true); + btTriangleMesh(bool use32bitIndices = true, bool use4componentVertices = true); - bool getUse32bitIndices() const - { - return m_use32bitIndices; - } + bool getUse32bitIndices() const + { + return m_use32bitIndices; + } - bool getUse4componentVertices() const - { - return m_use4componentVertices; - } - ///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes. - ///In general it is better to directly use btTriangleIndexVertexArray instead. - void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2, bool removeDuplicateVertices=false); + bool getUse4componentVertices() const + { + return m_use4componentVertices; + } + ///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes. + ///In general it is better to directly use btTriangleIndexVertexArray instead. + void addTriangle(const btVector3& vertex0, const btVector3& vertex1, const btVector3& vertex2, bool removeDuplicateVertices = false); - ///Add a triangle using its indices. Make sure the indices are pointing within the vertices array, so add the vertices first (and to be sure, avoid removal of duplicate vertices) - void addTriangleIndices(int index1, int index2, int index3 ); - - int getNumTriangles() const; + ///Add a triangle using its indices. Make sure the indices are pointing within the vertices array, so add the vertices first (and to be sure, avoid removal of duplicate vertices) + void addTriangleIndices(int index1, int index2, int index3); - virtual void preallocateVertices(int numverts); - virtual void preallocateIndices(int numindices); + int getNumTriangles() const; - ///findOrAddVertex is an internal method, use addTriangle instead - int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices); - ///addIndex is an internal method, use addTriangle instead - void addIndex(int index); - + virtual void preallocateVertices(int numverts); + virtual void preallocateIndices(int numindices); + + ///findOrAddVertex is an internal method, use addTriangle instead + int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices); + ///addIndex is an internal method, use addTriangle instead + void addIndex(int index); }; -#endif //BT_TRIANGLE_MESH_H - +#endif //BT_TRIANGLE_MESH_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp index 0e1795140..aec239063 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -20,12 +20,11 @@ subject to the following restrictions: #include "LinearMath/btAabbUtil2.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" - btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) -: btConcaveShape (), m_meshInterface(meshInterface) + : btConcaveShape(), m_meshInterface(meshInterface) { m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; - if(meshInterface->hasPremadeAabb()) + if (meshInterface->hasPremadeAabb()) { meshInterface->getPremadeAabb(&m_localAabbMin, &m_localAabbMax); } @@ -35,69 +34,60 @@ btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) } } - btTriangleMeshShape::~btTriangleMeshShape() { - } - - - -void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btTriangleMeshShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { + btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin); + localHalfExtents += btVector3(getMargin(), getMargin(), getMargin()); + btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin); - btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); - localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); - btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); - - btMatrix3x3 abs_b = trans.getBasis().absolute(); + btMatrix3x3 abs_b = trans.getBasis().absolute(); btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); aabbMin = center - extent; aabbMax = center + extent; } -void btTriangleMeshShape::recalcLocalAabb() +void btTriangleMeshShape::recalcLocalAabb() { - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]+m_collisionMargin; + m_localAabbMax[i] = tmp[i] + m_collisionMargin; vec[i] = btScalar(-1.); tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]-m_collisionMargin; + m_localAabbMin[i] = tmp[i] - m_collisionMargin; } } - - class SupportVertexCallback : public btTriangleCallback { - btVector3 m_supportVertexLocal; -public: - btTransform m_worldTrans; +public: + btTransform m_worldTrans; btScalar m_maxDot; btVector3 m_supportVecLocal; - SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans) - : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-BT_LARGE_FLOAT)) - + SupportVertexCallback(const btVector3& supportVecWorld, const btTransform& trans) + : m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)), m_worldTrans(trans), m_maxDot(btScalar(-BT_LARGE_FLOAT)) + { m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis(); } - virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex) + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { (void)partId; (void)triangleIndex; - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { btScalar dot = m_supportVecLocal.dot(triangle[i]); if (dot > m_maxDot) @@ -113,14 +103,12 @@ public: return m_worldTrans(m_supportVertexLocal); } - btVector3 GetSupportVertexLocal() + btVector3 GetSupportVertexLocal() { return m_supportVertexLocal; } - }; - void btTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_meshInterface->setScaling(scaling); @@ -132,60 +120,46 @@ const btVector3& btTriangleMeshShape::getLocalScaling() const return m_meshInterface->getScaling(); } - - - - - //#define DEBUG_TRIANGLE_MESH - - -void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - struct FilteredCallback : public btInternalTriangleIndexCallback + struct FilteredCallback : public btInternalTriangleIndexCallback { btTriangleCallback* m_callback; btVector3 m_aabbMin; btVector3 m_aabbMax; - FilteredCallback(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) - :m_callback(callback), - m_aabbMin(aabbMin), - m_aabbMax(aabbMax) + FilteredCallback(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) + : m_callback(callback), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { - if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax)) + if (TestTriangleAgainstAabb2(&triangle[0], m_aabbMin, m_aabbMax)) { //check aabb in triangle-space, before doing this - m_callback->processTriangle(triangle,partId,triangleIndex); + m_callback->processTriangle(triangle, partId, triangleIndex); } - } - }; - FilteredCallback filterCallback(callback,aabbMin,aabbMax); + FilteredCallback filterCallback(callback, aabbMin, aabbMax); - m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax); + m_meshInterface->InternalProcessAllTriangles(&filterCallback, aabbMin, aabbMax); } - - - - -void btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btTriangleMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { (void)mass; //moving concave objects not supported btAssert(0); - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } - btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supportVertex; @@ -193,15 +167,13 @@ btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) co btTransform ident; ident.setIdentity(); - SupportVertexCallback supportCallback(vec,ident); + SupportVertexCallback supportCallback(vec, ident); + + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + + processAllTriangles(&supportCallback, -aabbMax, aabbMax); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - - processAllTriangles(&supportCallback,-aabbMax,aabbMax); - supportVertex = supportCallback.GetSupportVertexLocal(); return supportVertex; } - - diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index 453e58005..4a70e283f 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -19,18 +19,18 @@ subject to the following restrictions: #include "btConcaveShape.h" #include "btStridingMeshInterface.h" - ///The btTriangleMeshShape is an internal concave triangle mesh interface. Don't use this class directly, use btBvhTriangleMeshShape instead. -ATTRIBUTE_ALIGNED16(class) btTriangleMeshShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btTriangleMeshShape : public btConcaveShape { protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; btStridingMeshInterface* m_meshInterface; ///btTriangleMeshShape constructor has been disabled/protected, so that users will not mistakenly use this class. ///Don't use btTriangleMeshShape but use btBvhTriangleMeshShape instead! - btTriangleMeshShape(btStridingMeshInterface* meshInterface); + btTriangleMeshShape(btStridingMeshInterface * meshInterface); public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -39,23 +39,23 @@ public: virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { btAssert(0); return localGetSupportingVertex(vec); } - void recalcLocalAabb(); + void recalcLocalAabb(); - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; - + btStridingMeshInterface* getMeshInterface() { return m_meshInterface; @@ -75,16 +75,8 @@ public: return m_localAabbMax; } - - //debugging - virtual const char* getName()const {return "TRIANGLEMESH";} - - - + virtual const char* getName() const { return "TRIANGLEMESH"; } }; - - - -#endif //BT_TRIANGLE_MESH_SHAPE_H +#endif //BT_TRIANGLE_MESH_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleShape.h index a8a80f82f..190cbdae6 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -19,15 +19,13 @@ subject to the following restrictions: #include "btConvexShape.h" #include "btBoxShape.h" -ATTRIBUTE_ALIGNED16(class) btTriangleShape : public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) +btTriangleShape : public btPolyhedralConvexShape { - - public: + BT_DECLARE_ALIGNED_ALLOCATOR(); -BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 m_vertices1[3]; + btVector3 m_vertices1[3]; virtual int getNumVertices() const { @@ -43,7 +41,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { return m_vertices1[index]; } - virtual void getVertex(int index,btVector3& vert) const + virtual void getVertex(int index, btVector3& vert) const { vert = m_vertices1[index]; } @@ -52,83 +50,79 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { return 3; } - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const + + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const { - getVertex(i,pa); - getVertex((i+1)%3,pb); + getVertex(i, pa); + getVertex((i + 1) % 3, pb); } - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { -// btAssert(0); - getAabbSlow(t,aabbMin,aabbMax); + // btAssert(0); + getAabbSlow(t, aabbMin, aabbMax); } - btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir)const + btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir) const { - btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]); - return m_vertices1[dots.maxAxis()]; - + btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]); + return m_vertices1[dots.maxAxis()]; } - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;ilocalGetSupportingVertexWithoutMargin(vec); - return tmpVertex*m_uniformScalingFactor; + return tmpVertex * m_uniformScalingFactor; } -void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); + m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors, supportVerticesOut, numVectors); int i; - for (i=0;ilocalGetSupportingVertex(vec); - return tmpVertex*m_uniformScalingFactor; + return tmpVertex * m_uniformScalingFactor; } - -void btUniformScalingShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btUniformScalingShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { - ///this linear upscaling is not realistic, but we don't deal with large mass ratios... btVector3 tmpInertia; - m_childConvexShape->calculateLocalInertia(mass,tmpInertia); + m_childConvexShape->calculateLocalInertia(mass, tmpInertia); inertia = tmpInertia * m_uniformScalingFactor; } - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btUniformScalingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btUniformScalingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - getAabbSlow(trans,aabbMin,aabbMax); - + getAabbSlow(trans, aabbMin, aabbMax); } -void btUniformScalingShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btUniformScalingShape::getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { #if 1 btVector3 _directions[] = - { - btVector3( 1., 0., 0.), - btVector3( 0., 1., 0.), - btVector3( 0., 0., 1.), - btVector3( -1., 0., 0.), - btVector3( 0., -1., 0.), - btVector3( 0., 0., -1.) - }; - + { + btVector3(1., 0., 0.), + btVector3(0., 1., 0.), + btVector3(0., 0., 1.), + btVector3(-1., 0., 0.), + btVector3(0., -1., 0.), + btVector3(0., 0., -1.)}; + btVector3 _supporting[] = - { - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.) - }; + { + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.)}; - for (int i=0;i<6;i++) + for (int i = 0; i < 6; i++) { - _directions[i] = _directions[i]*t.getBasis(); + _directions[i] = _directions[i] * t.getBasis(); } - - batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); - - btVector3 aabbMin1(0,0,0),aabbMax1(0,0,0); - for ( int i = 0; i < 3; ++i ) + batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); + + btVector3 aabbMin1(0, 0, 0), aabbMax1(0, 0, 0); + + for (int i = 0; i < 3; ++i) { aabbMax1[i] = t(_supporting[i])[i]; aabbMin1[i] = t(_supporting[i + 3])[i]; } - btVector3 marginVec(getMargin(),getMargin(),getMargin()); - aabbMin = aabbMin1-marginVec; - aabbMax = aabbMax1+marginVec; - + btVector3 marginVec(getMargin(), getMargin(), getMargin()); + aabbMin = aabbMin1 - marginVec; + aabbMax = aabbMax1 + marginVec; + #else btScalar margin = getMargin(); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); - btVector3 sv = localGetSupportingVertex(vec*t.getBasis()); + btVector3 sv = localGetSupportingVertex(vec * t.getBasis()); btVector3 tmp = t(sv); - aabbMax[i] = tmp[i]+margin; + aabbMax[i] = tmp[i] + margin; vec[i] = btScalar(-1.); - sv = localGetSupportingVertex(vec*t.getBasis()); + sv = localGetSupportingVertex(vec * t.getBasis()); tmp = t(sv); - aabbMin[i] = tmp[i]-margin; + aabbMin[i] = tmp[i] - margin; } #endif } -void btUniformScalingShape::setLocalScaling(const btVector3& scaling) +void btUniformScalingShape::setLocalScaling(const btVector3& scaling) { m_childConvexShape->setLocalScaling(scaling); } @@ -140,21 +130,21 @@ const btVector3& btUniformScalingShape::getLocalScaling() const return m_childConvexShape->getLocalScaling(); } -void btUniformScalingShape::setMargin(btScalar margin) +void btUniformScalingShape::setMargin(btScalar margin) { m_childConvexShape->setMargin(margin); } -btScalar btUniformScalingShape::getMargin() const +btScalar btUniformScalingShape::getMargin() const { return m_childConvexShape->getMargin() * m_uniformScalingFactor; } -int btUniformScalingShape::getNumPreferredPenetrationDirections() const +int btUniformScalingShape::getNumPreferredPenetrationDirections() const { return m_childConvexShape->getNumPreferredPenetrationDirections(); } - -void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + +void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { - m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); + m_childConvexShape->getPreferredPenetrationDirection(index, penetrationVector); } diff --git a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btUniformScalingShape.h b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btUniformScalingShape.h index a10f58d24..4dfe34efb 100644 --- a/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btUniformScalingShape.h +++ b/Engine/lib/bullet/src/BulletCollision/CollisionShapes/btUniformScalingShape.h @@ -17,73 +17,68 @@ subject to the following restrictions: #define BT_UNIFORM_SCALING_SHAPE_H #include "btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///The btUniformScalingShape allows to re-use uniform scaled instances of btConvexShape in a memory efficient way. ///Istead of using btUniformScalingShape, it is better to use the non-uniform setLocalScaling method on convex shapes that implement it. -ATTRIBUTE_ALIGNED16(class) btUniformScalingShape : public btConvexShape +ATTRIBUTE_ALIGNED16(class) +btUniformScalingShape : public btConvexShape { - btConvexShape* m_childConvexShape; + btConvexShape* m_childConvexShape; - btScalar m_uniformScalingFactor; - - public: - + btScalar m_uniformScalingFactor; + +public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btUniformScalingShape( btConvexShape* convexChildShape, btScalar uniformScalingFactor); - + + btUniformScalingShape(btConvexShape * convexChildShape, btScalar uniformScalingFactor); + virtual ~btUniformScalingShape(); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - btScalar getUniformScalingFactor() const + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; + + btScalar getUniformScalingFactor() const { return m_uniformScalingFactor; } - btConvexShape* getChildShape() + btConvexShape* getChildShape() { return m_childConvexShape; } - const btConvexShape* getChildShape() const + const btConvexShape* getChildShape() const { return m_childConvexShape; } - virtual const char* getName()const + virtual const char* getName() const { return "UniformScalingShape"; } - - /////////////////////////// - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling) ; - virtual const btVector3& getLocalScaling() const ; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; - virtual void setMargin(btScalar margin); - virtual btScalar getMargin() const; - - virtual int getNumPreferredPenetrationDirections() const; - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + virtual int getNumPreferredPenetrationDirections() const; + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; }; -#endif //BT_UNIFORM_SCALING_SHAPE_H +#endif //BT_UNIFORM_SCALING_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btBoxCollision.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btBoxCollision.h index 0a0357e5a..182835c3b 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btBoxCollision.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btBoxCollision.h @@ -26,27 +26,21 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" - ///Swap numbers -#define BT_SWAP_NUMBERS(a,b){ \ - a = a+b; \ - b = a-b; \ - a = a-b; \ -}\ - - -#define BT_MAX(a,b) (ab?b:a) - -#define BT_GREATER(x, y) btFabs(x) > (y) - -#define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c)) -#define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c)) - - +#define BT_SWAP_NUMBERS(a, b) \ + { \ + a = a + b; \ + b = a - b; \ + a = a - b; \ + } +#define BT_MAX(a, b) (a < b ? b : a) +#define BT_MIN(a, b) (a > b ? b : a) +#define BT_GREATER(x, y) btFabs(x) > (y) +#define BT_MAX3(a, b, c) BT_MAX(a, BT_MAX(b, c)) +#define BT_MIN3(a, b, c) BT_MIN(a, BT_MIN(b, c)) enum eBT_PLANE_INTERSECTION_TYPE { @@ -115,152 +109,144 @@ enum eBT_PLANE_INTERSECTION_TYPE // return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1); //} +#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]; \ + const btScalar dir1 = edge[i_dir_1]; \ + btScalar pmin = pointa[i_comp_0] * dir0 + pointa[i_comp_1] * dir1; \ + btScalar pmax = pointb[i_comp_0] * dir0 + pointb[i_comp_1] * dir1; \ + if (pmin > pmax) \ + { \ + BT_SWAP_NUMBERS(pmin, pmax); \ + } \ + const btScalar abs_dir0 = absolute_edge[i_dir_0]; \ + const btScalar abs_dir1 = absolute_edge[i_dir_1]; \ + const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1; \ + if (pmin > rad || -rad > pmax) return false; \ + } -#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];\ - const btScalar dir1 = edge[i_dir_1];\ - btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\ - btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\ - if(pmin>pmax)\ - {\ - BT_SWAP_NUMBERS(pmin,pmax); \ - }\ - const btScalar abs_dir0 = absolute_edge[i_dir_0];\ - const btScalar abs_dir1 = absolute_edge[i_dir_1];\ - const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\ - if(pmin>rad || -rad>pmax) return false;\ -}\ +#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \ + { \ + TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 2, 1, 1, 2); \ + } +#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \ + { \ + TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 0, 2, 2, 0); \ + } -#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\ -}\ - -#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\ -}\ - -#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\ -}\ - +#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \ + { \ + TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 1, 0, 0, 1); \ + } //! Returns the dot product between a vec3f and the col of a matrix SIMD_FORCE_INLINE btScalar bt_mat3_dot_col( -const btMatrix3x3 & mat, const btVector3 & vec3, int colindex) + const btMatrix3x3 &mat, const btVector3 &vec3, int colindex) { - return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex]; + return vec3[0] * mat[0][colindex] + vec3[1] * mat[1][colindex] + vec3[2] * mat[2][colindex]; } - //! Class for transforming a model1 to the space of model0 -ATTRIBUTE_ALIGNED16 (class) BT_BOX_BOX_TRANSFORM_CACHE +ATTRIBUTE_ALIGNED16(class) +BT_BOX_BOX_TRANSFORM_CACHE { public: - btVector3 m_T1to0;//!< Transforms translation of model1 to model 0 - btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal to R0' * R1 - btMatrix3x3 m_AR;//!< Absolute value of m_R1to0 + btVector3 m_T1to0; //!< Transforms translation of model1 to model 0 + btMatrix3x3 m_R1to0; //!< Transforms Rotation of model1 to model 0, equal to R0' * R1 + btMatrix3x3 m_AR; //!< Absolute value of m_R1to0 SIMD_FORCE_INLINE void calc_absolute_matrix() { -// static const btVector3 vepsi(1e-6f,1e-6f,1e-6f); -// m_AR[0] = vepsi + m_R1to0[0].absolute(); -// m_AR[1] = vepsi + m_R1to0[1].absolute(); -// m_AR[2] = vepsi + m_R1to0[2].absolute(); + // static const btVector3 vepsi(1e-6f,1e-6f,1e-6f); + // m_AR[0] = vepsi + m_R1to0[0].absolute(); + // m_AR[1] = vepsi + m_R1to0[1].absolute(); + // m_AR[2] = vepsi + m_R1to0[2].absolute(); - int i,j; - - for(i=0;i<3;i++) - { - for(j=0;j<3;j++ ) - { - m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]); - } - } + int i, j; + for (i = 0; i < 3; i++) + { + for (j = 0; j < 3; j++) + { + m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]); + } + } } BT_BOX_BOX_TRANSFORM_CACHE() { } - - //! Calc the transformation relative 1 to 0. Inverts matrics by transposing - SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1) + SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform &trans0, const btTransform &trans1) { - btTransform temp_trans = trans0.inverse(); temp_trans = temp_trans * trans1; m_T1to0 = temp_trans.getOrigin(); m_R1to0 = temp_trans.getBasis(); - calc_absolute_matrix(); } //! Calcs the full invertion of the matrices. Useful for scaling matrices - SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1) + SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform &trans0, const btTransform &trans1) { m_R1to0 = trans0.getBasis().inverse(); m_T1to0 = m_R1to0 * (-trans0.getOrigin()); - m_T1to0 += m_R1to0*trans1.getOrigin(); + m_T1to0 += m_R1to0 * trans1.getOrigin(); m_R1to0 *= trans1.getBasis(); calc_absolute_matrix(); } - SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const + SIMD_FORCE_INLINE btVector3 transform(const btVector3 &point) const { - return point.dot3( m_R1to0[0], m_R1to0[1], m_R1to0[2] ) + m_T1to0; + return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0; } }; - #define BOX_PLANE_EPSILON 0.000001f //! Axis aligned box -ATTRIBUTE_ALIGNED16 (class) btAABB +ATTRIBUTE_ALIGNED16(class) +btAABB { public: btVector3 m_min; btVector3 m_max; btAABB() - {} - - - btAABB(const btVector3 & V1, - const btVector3 & V2, - const btVector3 & V3) { - m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); - - m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); } - btAABB(const btVector3 & V1, - const btVector3 & V2, - const btVector3 & V3, - btScalar margin) + btAABB(const btVector3 &V1, + const btVector3 &V2, + const btVector3 &V3) { - m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]); + } + + btAABB(const btVector3 &V1, + const btVector3 &V2, + const btVector3 &V3, + btScalar margin) + { + m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]); + + m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]); m_min[0] -= margin; m_min[1] -= margin; @@ -270,13 +256,11 @@ public: m_max[2] += margin; } - btAABB(const btAABB &other): - m_min(other.m_min),m_max(other.m_max) + btAABB(const btAABB &other) : m_min(other.m_min), m_max(other.m_max) { } - btAABB(const btAABB &other,btScalar margin ): - m_min(other.m_min),m_max(other.m_max) + btAABB(const btAABB &other, btScalar margin) : m_min(other.m_min), m_max(other.m_max) { m_min[0] -= margin; m_min[1] -= margin; @@ -317,34 +301,34 @@ public: m_max[2] = other.m_max[2] + margin; } - template + template SIMD_FORCE_INLINE void calc_from_triangle( - const CLASS_POINT & V1, - const CLASS_POINT & V2, - const CLASS_POINT & V3) + const CLASS_POINT &V1, + const CLASS_POINT &V2, + const CLASS_POINT &V3) { - m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]); } - template + template SIMD_FORCE_INLINE void calc_from_triangle_margin( - const CLASS_POINT & V1, - const CLASS_POINT & V2, - const CLASS_POINT & V3, btScalar margin) + const CLASS_POINT &V1, + const CLASS_POINT &V2, + const CLASS_POINT &V3, btScalar margin) { - m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]); m_min[0] -= margin; m_min[1] -= margin; @@ -355,91 +339,89 @@ public: } //! Apply a transform to an AABB - SIMD_FORCE_INLINE void appy_transform(const btTransform & trans) + SIMD_FORCE_INLINE void appy_transform(const btTransform &trans) { - btVector3 center = (m_max+m_min)*0.5f; + btVector3 center = (m_max + m_min) * 0.5f; btVector3 extends = m_max - center; // Compute new center center = trans(center); - btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), - trans.getBasis().getRow(1).absolute(), - trans.getBasis().getRow(2).absolute()); + btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), + trans.getBasis().getRow(1).absolute(), + trans.getBasis().getRow(2).absolute()); m_min = center - textends; m_max = center + textends; } - //! Apply a transform to an AABB - SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans) + SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE &trans) { - btVector3 center = (m_max+m_min)*0.5f; + btVector3 center = (m_max + m_min) * 0.5f; btVector3 extends = m_max - center; // Compute new center center = trans.transform(center); - btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(), - trans.m_R1to0.getRow(1).absolute(), - trans.m_R1to0.getRow(2).absolute()); - + btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(), + trans.m_R1to0.getRow(1).absolute(), + trans.m_R1to0.getRow(2).absolute()); + m_min = center - textends; m_max = center + textends; } //! Merges a Box - SIMD_FORCE_INLINE void merge(const btAABB & box) + SIMD_FORCE_INLINE void merge(const btAABB &box) { - m_min[0] = BT_MIN(m_min[0],box.m_min[0]); - m_min[1] = BT_MIN(m_min[1],box.m_min[1]); - m_min[2] = BT_MIN(m_min[2],box.m_min[2]); + m_min[0] = BT_MIN(m_min[0], box.m_min[0]); + m_min[1] = BT_MIN(m_min[1], box.m_min[1]); + m_min[2] = BT_MIN(m_min[2], box.m_min[2]); - m_max[0] = BT_MAX(m_max[0],box.m_max[0]); - m_max[1] = BT_MAX(m_max[1],box.m_max[1]); - m_max[2] = BT_MAX(m_max[2],box.m_max[2]); + m_max[0] = BT_MAX(m_max[0], box.m_max[0]); + m_max[1] = BT_MAX(m_max[1], box.m_max[1]); + m_max[2] = BT_MAX(m_max[2], box.m_max[2]); } //! Merges a point - template - SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point) + template + SIMD_FORCE_INLINE void merge_point(const CLASS_POINT &point) { - m_min[0] = BT_MIN(m_min[0],point[0]); - m_min[1] = BT_MIN(m_min[1],point[1]); - m_min[2] = BT_MIN(m_min[2],point[2]); + m_min[0] = BT_MIN(m_min[0], point[0]); + m_min[1] = BT_MIN(m_min[1], point[1]); + m_min[2] = BT_MIN(m_min[2], point[2]); - m_max[0] = BT_MAX(m_max[0],point[0]); - m_max[1] = BT_MAX(m_max[1],point[1]); - m_max[2] = BT_MAX(m_max[2],point[2]); + m_max[0] = BT_MAX(m_max[0], point[0]); + m_max[1] = BT_MAX(m_max[1], point[1]); + m_max[2] = BT_MAX(m_max[2], point[2]); } //! Gets the extend and center - SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const + SIMD_FORCE_INLINE void get_center_extend(btVector3 & center, btVector3 & extend) const { - center = (m_max+m_min)*0.5f; + center = (m_max + m_min) * 0.5f; extend = m_max - center; } //! Finds the intersecting box between this box and the other. - SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection) const + SIMD_FORCE_INLINE void find_intersection(const btAABB &other, btAABB &intersection) const { - intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]); - intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]); - intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]); + intersection.m_min[0] = BT_MAX(other.m_min[0], m_min[0]); + intersection.m_min[1] = BT_MAX(other.m_min[1], m_min[1]); + intersection.m_min[2] = BT_MAX(other.m_min[2], m_min[2]); - intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]); - intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]); - intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]); + intersection.m_max[0] = BT_MIN(other.m_max[0], m_max[0]); + intersection.m_max[1] = BT_MIN(other.m_max[1], m_max[1]); + intersection.m_max[2] = BT_MIN(other.m_max[2], m_max[2]); } - - SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const + SIMD_FORCE_INLINE bool has_collision(const btAABB &other) const { - if(m_min[0] > other.m_max[0] || - m_max[0] < other.m_min[0] || - m_min[1] > other.m_max[1] || - m_max[1] < other.m_min[1] || - m_min[2] > other.m_max[2] || - m_max[2] < other.m_min[2]) + if (m_min[0] > other.m_max[0] || + m_max[0] < other.m_min[0] || + m_min[1] > other.m_max[1] || + m_max[1] < other.m_min[1] || + m_min[2] > other.m_max[2] || + m_max[2] < other.m_min[2]) { return false; } @@ -451,35 +433,34 @@ public: \param vorigin A vec3f with the origin of the ray \param vdir A vec3f with the direction of the ray */ - SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir) const + SIMD_FORCE_INLINE bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) const { - btVector3 extents,center; - this->get_center_extend(center,extents);; + btVector3 extents, center; + this->get_center_extend(center, extents); + ; btScalar Dx = vorigin[0] - center[0]; - if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false; + if (BT_GREATER(Dx, extents[0]) && Dx * vdir[0] >= 0.0f) return false; btScalar Dy = vorigin[1] - center[1]; - if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false; + if (BT_GREATER(Dy, extents[1]) && Dy * vdir[1] >= 0.0f) return false; btScalar Dz = vorigin[2] - center[2]; - if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false; - + if (BT_GREATER(Dz, extents[2]) && Dz * vdir[2] >= 0.0f) return false; btScalar f = vdir[1] * Dz - vdir[2] * Dy; - if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false; + if (btFabs(f) > extents[1] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[1])) return false; f = vdir[2] * Dx - vdir[0] * Dz; - if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false; + if (btFabs(f) > extents[0] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[0])) return false; f = vdir[0] * Dy - vdir[1] * Dx; - if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false; + if (btFabs(f) > extents[0] * btFabs(vdir[1]) + extents[1] * btFabs(vdir[0])) return false; return true; } - - SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const + SIMD_FORCE_INLINE void projection_interval(const btVector3 &direction, btScalar &vmin, btScalar &vmax) const { - btVector3 center = (m_max+m_min)*0.5f; - btVector3 extend = m_max-center; + btVector3 center = (m_max + m_min) * 0.5f; + btVector3 extend = m_max - center; - btScalar _fOrigin = direction.dot(center); + btScalar _fOrigin = direction.dot(center); btScalar _fMaximumExtent = extend.dot(direction.absolute()); vmin = _fOrigin - _fMaximumExtent; vmax = _fOrigin + _fMaximumExtent; @@ -487,30 +468,30 @@ public: SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const { - btScalar _fmin,_fmax; - this->projection_interval(plane,_fmin,_fmax); + btScalar _fmin, _fmax; + this->projection_interval(plane, _fmin, _fmax); - if(plane[3] > _fmax + BOX_PLANE_EPSILON) + if (plane[3] > _fmax + BOX_PLANE_EPSILON) { - return BT_CONST_BACK_PLANE; // 0 + return BT_CONST_BACK_PLANE; // 0 } - if(plane[3]+BOX_PLANE_EPSILON >=_fmin) + if (plane[3] + BOX_PLANE_EPSILON >= _fmin) { - return BT_CONST_COLLIDE_PLANE; //1 + return BT_CONST_COLLIDE_PLANE; //1 } - return BT_CONST_FRONT_PLANE;//2 + return BT_CONST_FRONT_PLANE; //2 } - SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const + SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB &box, btTransform &trans1_to_0) const { btAABB tbox = box; tbox.appy_transform(trans1_to_0); return has_collision(tbox); } - SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box, - const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const + SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB &box, + const BT_BOX_BOX_TRANSFORM_CACHE &trans1_to_0) const { btAABB tbox = box; tbox.appy_transform_trans_cache(trans1_to_0); @@ -519,52 +500,50 @@ public: //! transcache is the transformation cache from box to this AABB SIMD_FORCE_INLINE bool overlapping_trans_cache( - const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const + const btAABB &box, const BT_BOX_BOX_TRANSFORM_CACHE &transcache, bool fulltest) const { - //Taken from OPCODE - btVector3 ea,eb;//extends - btVector3 ca,cb;//extends - get_center_extend(ca,ea); - box.get_center_extend(cb,eb); - + btVector3 ea, eb; //extends + btVector3 ca, cb; //extends + get_center_extend(ca, ea); + box.get_center_extend(cb, eb); btVector3 T; - btScalar t,t2; + btScalar t, t2; int i; // Class I : A's basis vectors - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i]; + T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i]; t = transcache.m_AR[i].dot(eb) + ea[i]; - if(BT_GREATER(T[i], t)) return false; + if (BT_GREATER(T[i], t)) return false; } // Class II : B's basis vectors - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - t = bt_mat3_dot_col(transcache.m_R1to0,T,i); - t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i]; - if(BT_GREATER(t,t2)) return false; + t = bt_mat3_dot_col(transcache.m_R1to0, T, i); + t2 = bt_mat3_dot_col(transcache.m_AR, ea, i) + eb[i]; + if (BT_GREATER(t, t2)) return false; } // Class III : 9 cross products - if(fulltest) + if (fulltest) { - int j,m,n,o,p,q,r; - for(i=0;i<3;i++) + int j, m, n, o, p, q, r; + for (i = 0; i < 3; i++) { - m = (i+1)%3; - n = (i+2)%3; - o = i==0?1:0; - p = i==2?1:2; - for(j=0;j<3;j++) + m = (i + 1) % 3; + n = (i + 2) % 3; + o = i == 0 ? 1 : 0; + p = i == 2 ? 1 : 2; + for (j = 0; j < 3; j++) { - q = j==2?1:2; - r = j==0?1:0; - t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j]; - t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] + - eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r]; - if(BT_GREATER(t,t2)) return false; + q = j == 2 ? 1 : 2; + r = j == 0 ? 1 : 0; + t = T[n] * transcache.m_R1to0[m][j] - T[m] * transcache.m_R1to0[n][j]; + t2 = ea[o] * transcache.m_AR[p][j] + ea[p] * transcache.m_AR[o][j] + + eb[r] * transcache.m_AR[i][q] + eb[q] * transcache.m_AR[i][r]; + if (BT_GREATER(t, t2)) return false; } } } @@ -573,7 +552,7 @@ public: //! Simple test for planes. SIMD_FORCE_INLINE bool collide_plane( - const btVector4 & plane) const + const btVector4 &plane) const { eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane); return (classify == BT_CONST_COLLIDE_PLANE); @@ -581,15 +560,15 @@ public: //! test for a triangle, with edges SIMD_FORCE_INLINE bool collide_triangle_exact( - const btVector3 & p1, - const btVector3 & p2, - const btVector3 & p3, - const btVector4 & triangle_plane) const + const btVector3 &p1, + const btVector3 &p2, + const btVector3 &p3, + const btVector4 &triangle_plane) const { - if(!collide_plane(triangle_plane)) return false; + if (!collide_plane(triangle_plane)) return false; - btVector3 center,extends; - this->get_center_extend(center,extends); + btVector3 center, extends; + this->get_center_extend(center, extends); const btVector3 v1(p1 - center); const btVector3 v2(p2 - center); @@ -599,47 +578,43 @@ public: btVector3 diff(v2 - v1); btVector3 abs_diff = diff.absolute(); //Test With X axis - TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends); + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v1, v3, extends); //Test With Y axis - TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends); + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v1, v3, extends); //Test With Z axis - TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends); - + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v1, v3, extends); diff = v3 - v2; abs_diff = diff.absolute(); //Test With X axis - TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends); + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v2, v1, extends); //Test With Y axis - TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends); + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v2, v1, extends); //Test With Z axis - TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends); + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v2, v1, extends); diff = v1 - v3; abs_diff = diff.absolute(); //Test With X axis - TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends); + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v3, v2, extends); //Test With Y axis - TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends); + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v3, v2, extends); //Test With Z axis - TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends); + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v3, v2, extends); return true; } }; - //! Compairison of transformation objects -SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) +SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform &t1, const btTransform &t2) { - if(!(t1.getOrigin() == t2.getOrigin()) ) return false; + if (!(t1.getOrigin() == t2.getOrigin())) return false; - if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false; - if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false; - if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false; + if (!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0))) return false; + if (!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1))) return false; + if (!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2))) return false; return true; } - - -#endif // GIM_BOX_COLLISION_H_INCLUDED +#endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btClipPolygon.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btClipPolygon.h index de0a5231b..38c23e222 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btClipPolygon.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btClipPolygon.h @@ -27,77 +27,74 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btGeometryUtil.h" - -SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point) +SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 &plane, const btVector3 &point) { return point.dot(plane) - plane[3]; } /*! Vector blending Takes two vectors a, b, blends them together*/ -SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va,const btVector3 &vb, btScalar blend_factor) +SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va, const btVector3 &vb, btScalar blend_factor) { - vr = (1-blend_factor)*va + blend_factor*vb; + vr = (1 - blend_factor) * va + blend_factor * vb; } //! This function calcs the distance from a 3D plane SIMD_FORCE_INLINE void bt_plane_clip_polygon_collect( - const btVector3 & point0, - const btVector3 & point1, - btScalar dist0, - btScalar dist1, - btVector3 * clipped, - int & clipped_count) + const btVector3 &point0, + const btVector3 &point1, + btScalar dist0, + btScalar dist1, + btVector3 *clipped, + int &clipped_count) { - bool _prevclassif = (dist0>SIMD_EPSILON); - bool _classif = (dist1>SIMD_EPSILON); - if(_classif!=_prevclassif) + bool _prevclassif = (dist0 > SIMD_EPSILON); + bool _classif = (dist1 > SIMD_EPSILON); + if (_classif != _prevclassif) { - btScalar blendfactor = -dist0/(dist1-dist0); - bt_vec_blend(clipped[clipped_count],point0,point1,blendfactor); + btScalar blendfactor = -dist0 / (dist1 - dist0); + bt_vec_blend(clipped[clipped_count], point0, point1, blendfactor); clipped_count++; } - if(!_classif) + if (!_classif) { clipped[clipped_count] = point1; clipped_count++; } } - //! Clips a polygon by a plane /*! *\return The count of the clipped counts */ SIMD_FORCE_INLINE int bt_plane_clip_polygon( - const btVector4 & plane, - const btVector3 * polygon_points, - int polygon_point_count, - btVector3 * clipped) + const btVector4 &plane, + const btVector3 *polygon_points, + int polygon_point_count, + btVector3 *clipped) { - int clipped_count = 0; + int clipped_count = 0; - - //clip first point - btScalar firstdist = bt_distance_point_plane(plane,polygon_points[0]);; - if(!(firstdist>SIMD_EPSILON)) + //clip first point + btScalar firstdist = bt_distance_point_plane(plane, polygon_points[0]); + ; + if (!(firstdist > SIMD_EPSILON)) { clipped[clipped_count] = polygon_points[0]; clipped_count++; } btScalar olddist = firstdist; - for(int i=1;iSIMD_EPSILON)) + //clip first point0 + btScalar firstdist = bt_distance_point_plane(plane, point0); + ; + if (!(firstdist > SIMD_EPSILON)) { clipped[clipped_count] = point0; clipped_count++; @@ -139,44 +137,37 @@ SIMD_FORCE_INLINE int bt_plane_clip_triangle( // point 1 btScalar olddist = firstdist; - btScalar dist = bt_distance_point_plane(plane,point1); + btScalar dist = bt_distance_point_plane(plane, point1); bt_plane_clip_polygon_collect( - point0,point1, - olddist, - dist, - clipped, - clipped_count); + point0, point1, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - // point 2 - dist = bt_distance_point_plane(plane,point2); + dist = bt_distance_point_plane(plane, point2); bt_plane_clip_polygon_collect( - point1,point2, - olddist, - dist, - clipped, - clipped_count); + point1, point2, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - - //RETURN TO FIRST point0 bt_plane_clip_polygon_collect( - point2,point0, - olddist, - firstdist, - clipped, - clipped_count); + point2, point0, + olddist, + firstdist, + clipped, + clipped_count); return clipped_count; } - - - - -#endif // GIM_TRI_COLLISION_H_INCLUDED +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btCompoundFromGimpact.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btCompoundFromGimpact.h index 19f7ecddd..ede59e8a5 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btCompoundFromGimpact.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btCompoundFromGimpact.h @@ -5,7 +5,8 @@ #include "btGImpactShape.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" -ATTRIBUTE_ALIGNED16(class) btCompoundFromGimpactShape : public btCompoundShape +ATTRIBUTE_ALIGNED16(class) +btCompoundFromGimpactShape : public btCompoundShape { public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -18,92 +19,87 @@ public: delete m_children[i].m_childShape; } } - }; struct MyCallback : public btTriangleRaycastCallback +{ + int m_ignorePart; + int m_ignoreTriangleIndex; + + MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex) + : btTriangleRaycastCallback(from, to), + m_ignorePart(ignorePart), + m_ignoreTriangleIndex(ignoreTriangleIndex) + { + } + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) + { + if (partId != m_ignorePart || triangleIndex != m_ignoreTriangleIndex) { - int m_ignorePart; - int m_ignoreTriangleIndex; - + if (hitFraction < m_hitFraction) + return hitFraction; + } - MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex) - :btTriangleRaycastCallback(from,to), - m_ignorePart(ignorePart), - m_ignoreTriangleIndex(ignoreTriangleIndex) - { - - } - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) - { - if (partId!=m_ignorePart || triangleIndex!=m_ignoreTriangleIndex) - { - if (hitFraction < m_hitFraction) - return hitFraction; - } + return m_hitFraction; + } +}; +struct MyInternalTriangleIndexCallback : public btInternalTriangleIndexCallback +{ + const btGImpactMeshShape* m_gimpactShape; + btCompoundShape* m_colShape; + btScalar m_depth; - return m_hitFraction; - } - }; - struct MyInternalTriangleIndexCallback :public btInternalTriangleIndexCallback + MyInternalTriangleIndexCallback(btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth) + : m_colShape(colShape), + m_gimpactShape(meshShape), + m_depth(depth) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + btVector3 scale = m_gimpactShape->getLocalScaling(); + btVector3 v0 = triangle[0] * scale; + btVector3 v1 = triangle[1] * scale; + btVector3 v2 = triangle[2] * scale; + + btVector3 centroid = (v0 + v1 + v2) / 3; + btVector3 normal = (v1 - v0).cross(v2 - v0); + normal.normalize(); + btVector3 rayFrom = centroid; + btVector3 rayTo = centroid - normal * m_depth; + + MyCallback cb(rayFrom, rayTo, partId, triangleIndex); + + m_gimpactShape->processAllTrianglesRay(&cb, rayFrom, rayTo); + if (cb.m_hitFraction < 1) { - const btGImpactMeshShape* m_gimpactShape; - btCompoundShape* m_colShape; - btScalar m_depth; + rayTo.setInterpolate3(cb.m_from, cb.m_to, cb.m_hitFraction); + //rayTo = cb.m_from; + //rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction); + //gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0)); + } - MyInternalTriangleIndexCallback (btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth) - :m_colShape(colShape), - m_gimpactShape(meshShape), - m_depth(depth) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - btVector3 scale = m_gimpactShape->getLocalScaling(); - btVector3 v0=triangle[0]*scale; - btVector3 v1=triangle[1]*scale; - btVector3 v2=triangle[2]*scale; - - btVector3 centroid = (v0+v1+v2)/3; - btVector3 normal = (v1-v0).cross(v2-v0); - normal.normalize(); - btVector3 rayFrom = centroid; - btVector3 rayTo = centroid-normal*m_depth; - - MyCallback cb(rayFrom,rayTo,partId,triangleIndex); - - m_gimpactShape->processAllTrianglesRay(&cb,rayFrom, rayTo); - if (cb.m_hitFraction<1) - { - rayTo.setInterpolate3(cb.m_from,cb.m_to,cb.m_hitFraction); - //rayTo = cb.m_from; - //rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction); - //gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0)); - } - + btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0, v1, v2, rayTo); + btTransform ident; + ident.setIdentity(); + m_colShape->addChildShape(ident, tet); + } +}; - - btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0,v1,v2,rayTo); - btTransform ident; - ident.setIdentity(); - m_colShape->addChildShape(ident,tet); - } - }; - -btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth) +btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth) { btCompoundShape* colShape = new btCompoundFromGimpactShape(); - - btTransform tr; - tr.setIdentity(); - - MyInternalTriangleIndexCallback cb(colShape,gimpactMesh, depth); - btVector3 aabbMin,aabbMax; - gimpactMesh->getAabb(tr,aabbMin,aabbMax); - gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb,aabbMin,aabbMax); - return colShape; -} + btTransform tr; + tr.setIdentity(); -#endif //BT_COMPOUND_FROM_GIMPACT + MyInternalTriangleIndexCallback cb(colShape, gimpactMesh, depth); + btVector3 aabbMin, aabbMax; + gimpactMesh->getAabb(tr, aabbMin, aabbMax); + gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb, aabbMin, aabbMax); + + return colShape; +} + +#endif //BT_COMPOUND_FROM_GIMPACT diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btContactProcessing.cpp b/Engine/lib/bullet/src/BulletCollision/Gimpact/btContactProcessing.cpp index eed31d839..f2e3e18d6 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btContactProcessing.cpp +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btContactProcessing.cpp @@ -27,54 +27,50 @@ struct CONTACT_KEY_TOKEN unsigned int m_key; int m_value; CONTACT_KEY_TOKEN() - { - } + { + } - CONTACT_KEY_TOKEN(unsigned int key,int token) - { - m_key = key; - m_value = token; - } + CONTACT_KEY_TOKEN(unsigned int key, int token) + { + m_key = key; + m_value = token; + } - CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken) - { - m_key = rtoken.m_key; - m_value = rtoken.m_value; - } + CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken) + { + m_key = rtoken.m_key; + m_value = rtoken.m_value; + } - inline bool operator <(const CONTACT_KEY_TOKEN& other) const + inline bool operator<(const CONTACT_KEY_TOKEN& other) const { return (m_key < other.m_key); } - inline bool operator >(const CONTACT_KEY_TOKEN& other) const + inline bool operator>(const CONTACT_KEY_TOKEN& other) const { return (m_key > other.m_key); } - }; class CONTACT_KEY_TOKEN_COMP { - public: - - bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) const - { - return ( a < b ); - } +public: + bool operator()(const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b) const + { + return (a < b); + } }; - void btContactArray::merge_contacts( - const btContactArray & contacts, bool normal_contact_average) + const btContactArray& contacts, bool normal_contact_average) { clear(); int i; - if(contacts.size()==0) return; + if (contacts.size() == 0) return; - - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts[0]); return; @@ -86,16 +82,16 @@ void btContactArray::merge_contacts( //fill key contacts - for ( i = 0;im_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) + if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth) //) { *pcontact = *scontact; - coincident_count = 0; + coincident_count = 0; } - else if(normal_contact_average) + else if (normal_contact_average) { - if(btFabs(pcontact->m_depth - scontact->m_depth)m_normal; - coincident_count++; - } - } + if (btFabs(pcontact->m_depth - scontact->m_depth) < CONTACT_DIFF_EPSILON) + { + if (coincident_count < MAX_COINCIDENT) + { + coincident_normals[coincident_count] = scontact->m_normal; + coincident_count++; + } + } } } else - {//add new contact + { //add new contact - if(normal_contact_average && coincident_count>0) - { - pcontact->interpolate_normals(coincident_normals,coincident_count); - coincident_count = 0; - } + if (normal_contact_average && coincident_count > 0) + { + pcontact->interpolate_normals(coincident_normals, coincident_count); + coincident_count = 0; + } - push_back(*scontact); - pcontact = &(*this)[this->size()-1]; - } + push_back(*scontact); + pcontact = &(*this)[this->size() - 1]; + } last_key = key; } } -void btContactArray::merge_contacts_unique(const btContactArray & contacts) +void btContactArray::merge_contacts_unique(const btContactArray& contacts) { clear(); - if(contacts.size()==0) return; + if (contacts.size() == 0) return; - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts[0]); return; @@ -160,14 +156,14 @@ void btContactArray::merge_contacts_unique(const btContactArray & contacts) GIM_CONTACT average_contact = contacts[0]; - for (int i=1;i +class btContactArray : public btAlignedObjectArray { public: btContactArray() @@ -38,28 +38,28 @@ public: } SIMD_FORCE_INLINE void push_contact( - const btVector3 &point,const btVector3 & normal, + const btVector3 &point, const btVector3 &normal, btScalar depth, int feature1, int feature2) { - push_back( GIM_CONTACT(point,normal,depth,feature1,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) + const GIM_TRIANGLE_CONTACT &tricontact, + int feature1, int feature2) { - for(int i = 0;i splitValue) { //swap - primitive_boxes.swap(i,splitIndex); + primitive_boxes.swap(i, splitIndex); //swapLeafNodes(i,splitIndex); splitIndex++; } @@ -142,32 +137,30 @@ int btBvhTree::_sort_and_calc_splitting_index( //bool unbalanced2 = true; //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + int rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); + btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); return splitIndex; - } - -void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex) { int curIndex = m_num_nodes; m_num_nodes++; - btAssert((endIndex-startIndex)>0); + btAssert((endIndex - startIndex) > 0); - if ((endIndex-startIndex)==1) + if ((endIndex - startIndex) == 1) { - //We have a leaf node - setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + //We have a leaf node + setNodeBound(curIndex, primitive_boxes[startIndex].m_bound); m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); return; @@ -175,47 +168,42 @@ void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startI //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. //split axis - int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); + int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex); splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex, - splitIndex//split axis - ); - + primitive_boxes, startIndex, endIndex, + splitIndex //split axis + ); //calc this node bounding box btAABB node_bound; node_bound.invalidate(); - for (int i=startIndex;iget_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); + m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox); + setNodeBound(nodecount, leafbox); } else { @@ -243,20 +231,20 @@ void btGImpactBvh::refit() btAABB temp_box; int child_node = getLeftNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } child_node = getRightNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } - setNodeBound(nodecount,bound); + setNodeBound(nodecount, bound); } } } @@ -268,17 +256,17 @@ void btGImpactBvh::buildSet() GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); - for (int i = 0;iget_primitive_box(i,primitive_boxes[i].m_bound); - primitive_boxes[i].m_data = i; + m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound); + primitive_boxes[i].m_data = i; } m_box_tree.build_tree(primitive_boxes); } //! returns the indices of the primitives in the m_primitive_manager -bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray & collided_results) const +bool btGImpactBvh::boxQuery(const btAABB& box, btAlignedObjectArray& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -286,7 +274,7 @@ bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray & coll while (curIndex < numNodes) { btAABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data @@ -306,19 +294,17 @@ bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray & coll else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } - - //! returns the indices of the primitives in the m_primitive_manager bool btGImpactBvh::rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray & collided_results) const + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -326,16 +312,16 @@ bool btGImpactBvh::rayQuery( while (curIndex < numNodes) { btAABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) { - collided_results.push_back(getNodeData( curIndex)); + collided_results.push_back(getNodeData(curIndex)); } if (aabbOverlap || isleafnode) @@ -346,153 +332,133 @@ bool btGImpactBvh::rayQuery( else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } - SIMD_FORCE_INLINE bool _node_collision( - btGImpactBvh * boxset0, btGImpactBvh * boxset1, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, - int node0 ,int node1, bool complete_primitive_tests) + btGImpactBvh* boxset0, btGImpactBvh* boxset1, + const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, + int node0, int node1, bool complete_primitive_tests) { btAABB box0; - boxset0->getNodeBound(node0,box0); + boxset0->getNodeBound(node0, box0); btAABB box1; - boxset1->getNodeBound(node1,box1); - - return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests ); -// box1.appy_transform_trans_cache(trans_cache_1to0); -// return box0.has_collision(box1); + boxset1->getNodeBound(node1, box1); + return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests); + // box1.appy_transform_trans_cache(trans_cache_1to0); + // return box0.has_collision(box1); } - //stackless recursive collision routine static void _find_collision_pairs_recursive( - btGImpactBvh * boxset0, btGImpactBvh * boxset1, - btPairSet * collision_pairs, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, + btGImpactBvh* boxset0, btGImpactBvh* boxset1, + btPairSet* collision_pairs, + const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, int node0, int node1, bool complete_primitive_tests) { + if (_node_collision( + boxset0, boxset1, trans_cache_1to0, + node0, node1, complete_primitive_tests) == false) return; //avoid colliding internal nodes - - - if( _node_collision( - boxset0,boxset1,trans_cache_1to0, - node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes - - if(boxset0->isLeafNode(node0)) + if (boxset0->isLeafNode(node0)) { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { // collision result collision_pairs->push_pair( - boxset0->getNodeData(node0),boxset1->getNodeData(node1)); + boxset0->getNodeData(node0), boxset1->getNodeData(node1)); return; } else { - //collide left recursive _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getLeftNode(node1), false); //collide right recursive _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getRightNode(node1),false); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getRightNode(node1), false); } } else { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { - //collide left recursive _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),node1,false); - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), node1, false); //collide right recursive _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),node1,false); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), node1, false); } else { //collide left0 left1 - - _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false); //collide left0 right1 _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false); - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false); //collide right0 left1 _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false); //collide right0 right1 _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getRightNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), boxset1->getRightNode(node1), false); - }// else if node1 is not a leaf - }// else if node0 is not a leaf + } // else if node1 is not a leaf + } // else if node0 is not a leaf } - -void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0, - btGImpactBvh * boxset1, const btTransform & trans1, - btPairSet & collision_pairs) +void btGImpactBvh::find_collision(btGImpactBvh* boxset0, const btTransform& trans0, + btGImpactBvh* boxset1, const btTransform& trans1, + btPairSet& collision_pairs) { - - if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; + if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return; BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - trans_cache_1to0.calc_from_homogenic(trans0,trans1); + trans_cache_1to0.calc_from_homogenic(trans0, trans1); #ifdef TRI_COLLISION_PROFILING bt_begin_gim02_tree_time(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING _find_collision_pairs_recursive( - boxset0,boxset1, - &collision_pairs,trans_cache_1to0,0,0,true); + boxset0, boxset1, + &collision_pairs, trans_cache_1to0, 0, 0, true); #ifdef TRI_COLLISION_PROFILING bt_end_gim02_tree_time(); -#endif //TRI_COLLISION_PROFILING - +#endif //TRI_COLLISION_PROFILING } - diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactBvh.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactBvh.h index e20e03cc1..3cd8fa24e 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactBvh.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactBvh.h @@ -1,5 +1,5 @@ -#ifndef GIM_BOX_SET_H_INCLUDED -#define GIM_BOX_SET_H_INCLUDED +#ifndef BT_GIMPACT_BVH_H_INCLUDED +#define BT_GIMPACT_BVH_H_INCLUDED /*! \file gim_box_set.h \author Francisco Leon Najera @@ -24,7 +24,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "LinearMath/btAlignedObjectArray.h" #include "btBoxCollision.h" @@ -32,50 +31,48 @@ subject to the following restrictions: #include "btGImpactBvhStructs.h" //! A pairset array -class btPairSet: public btAlignedObjectArray +class btPairSet : public btAlignedObjectArray { public: btPairSet() { reserve(32); } - inline void push_pair(int index1,int index2) + inline void push_pair(int index1, int index2) { - push_back(GIM_PAIR(index1,index2)); + push_back(GIM_PAIR(index1, index2)); } - inline void push_pair_inv(int index1,int index2) + inline void push_pair_inv(int index1, int index2) { - push_back(GIM_PAIR(index2,index1)); + push_back(GIM_PAIR(index2, index1)); } }; -class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray +class GIM_BVH_DATA_ARRAY : public btAlignedObjectArray { }; - -class GIM_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray +class GIM_BVH_TREE_NODE_ARRAY : public btAlignedObjectArray { }; - - - //! Basic Box tree structure class btBvhTree { protected: int m_num_nodes; GIM_BVH_TREE_NODE_ARRAY m_node_array; + protected: int _sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, - int startIndex, int endIndex, int splitAxis); + GIM_BVH_DATA_ARRAY& primitive_boxes, + int startIndex, int endIndex, int splitAxis); - int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + int _calc_splitting_axis(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); + + void _build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); - void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); public: btBvhTree() { @@ -84,7 +81,7 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); + void build_tree(GIM_BVH_DATA_ARRAY& primitive_boxes); SIMD_FORCE_INLINE void clearNodes() { @@ -109,25 +106,25 @@ public: return m_node_array[nodeindex].getDataIndex(); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const { bound = m_node_array[nodeindex].m_bound; } - SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound) { m_node_array[nodeindex].m_bound = bound; } SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const { - return nodeindex+1; + return nodeindex + 1; } SIMD_FORCE_INLINE int getRightNode(int nodeindex) const { - if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2; - return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex(); + if (m_node_array[nodeindex + 1].isLeafNode()) return nodeindex + 2; + return nodeindex + 1 + m_node_array[nodeindex + 1].getEscapeIndex(); } SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const @@ -135,7 +132,7 @@ public: return m_node_array[nodeindex].getEscapeIndex(); } - SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE* get_node_pointer(int index = 0) const { return &m_node_array[index]; } @@ -143,7 +140,6 @@ public: //!@} }; - //! Prototype Base class for primitive classification /*! This class is a wrapper for primitive collections. @@ -153,18 +149,16 @@ This class can manage Compound shapes and trimeshes, and if it is managing trime class btPrimitiveManagerBase { public: - virtual ~btPrimitiveManagerBase() {} //! determines if this manager consist on only triangles, which special case will be optimized virtual bool is_trimesh() const = 0; virtual int get_primitive_count() const = 0; - virtual void get_primitive_box(int prim_index ,btAABB & primbox) const = 0; + virtual void get_primitive_box(int prim_index, btAABB& primbox) const = 0; //! retrieves only the points of the triangle, and the collision margin - virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const= 0; + virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const = 0; }; - //! Structure for containing Boxes /*! This class offers an structure for managing a box tree of primitives. @@ -174,13 +168,13 @@ class btGImpactBvh { protected: btBvhTree m_box_tree; - btPrimitiveManagerBase * m_primitive_manager; + btPrimitiveManagerBase* m_primitive_manager; protected: //stackless refit void refit(); -public: +public: //! this constructor doesn't build the tree. you must call buildSet btGImpactBvh() { @@ -188,31 +182,30 @@ public: } //! this constructor doesn't build the tree. you must call buildSet - btGImpactBvh(btPrimitiveManagerBase * primitive_manager) + btGImpactBvh(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btAABB getGlobalBox() const + SIMD_FORCE_INLINE btAABB getGlobalBox() const { btAABB totalbox; getNodeBound(0, totalbox); return totalbox; } - SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) + SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const + SIMD_FORCE_INLINE btPrimitiveManagerBase* getPrimitiveManager() const { return m_primitive_manager; } - -//! node manager prototype functions -///@{ + //! node manager prototype functions + ///@{ //! this attemps to refit the box set. SIMD_FORCE_INLINE void update() @@ -224,21 +217,21 @@ public: void buildSet(); //! returns the indices of the primitives in the m_primitive_manager - bool boxQuery(const btAABB & box, btAlignedObjectArray & collided_results) const; + bool boxQuery(const btAABB& box, btAlignedObjectArray& collided_results) const; //! returns the indices of the primitives in the m_primitive_manager - SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box, - const btTransform & transform, btAlignedObjectArray & collided_results) const + SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB& box, + const btTransform& transform, btAlignedObjectArray& collided_results) const { - btAABB transbox=box; + btAABB transbox = box; transbox.appy_transform(transform); - return boxQuery(transbox,collided_results); + return boxQuery(transbox, collided_results); } //! returns the indices of the primitives in the m_primitive_manager bool rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray & collided_results) const; + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray& collided_results) const; //! tells if this set has hierarcht SIMD_FORCE_INLINE bool hasHierarchy() const @@ -247,7 +240,7 @@ public: } //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const + SIMD_FORCE_INLINE bool isTrimesh() const { return m_primitive_manager->is_trimesh(); } @@ -269,17 +262,16 @@ public: return m_box_tree.getNodeData(nodeindex); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const { m_box_tree.getNodeBound(nodeindex, bound); } - SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound) { m_box_tree.setNodeBound(nodeindex, bound); } - SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const { return m_box_tree.getLeftNode(nodeindex); @@ -295,24 +287,23 @@ public: return m_box_tree.getEscapeNodeIndex(nodeindex); } - SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const + SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex, btPrimitiveTriangle& triangle) const { - m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); + m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex), triangle); } - - SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE* get_node_pointer(int index = 0) const { return m_box_tree.get_node_pointer(index); } #ifdef TRI_COLLISION_PROFILING static float getAverageTreeCollisionTime(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING - static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1, - btGImpactBvh * boxset2, const btTransform & trans2, - btPairSet & collision_pairs); + static void find_collision(btGImpactBvh* boxset1, const btTransform& trans1, + btGImpactBvh* boxset2, const btTransform& trans2, + btPairSet& collision_pairs); }; -#endif // GIM_BOXPRUNING_H_INCLUDED +#endif // BT_GIMPACT_BVH_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactBvhStructs.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactBvhStructs.h index 9342a572d..8f78c234b 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactBvhStructs.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactBvhStructs.h @@ -24,32 +24,11 @@ subject to the following restrictions: 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; - } -}; +#include "gim_pair.h" //for GIM_PAIR ///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box struct GIM_BVH_DATA @@ -63,8 +42,10 @@ class GIM_BVH_TREE_NODE { public: btAABB m_bound; + protected: - int m_escapeIndexOrDataIndex; + int m_escapeIndexOrDataIndex; + public: GIM_BVH_TREE_NODE() { @@ -74,7 +55,7 @@ public: SIMD_FORCE_INLINE bool isLeafNode() const { //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrDataIndex>=0); + return (m_escapeIndexOrDataIndex >= 0); } SIMD_FORCE_INLINE int getEscapeIndex() const @@ -99,7 +80,6 @@ public: { m_escapeIndexOrDataIndex = index; } - }; -#endif // GIM_BOXPRUNING_H_INCLUDED +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp index 2e87475e3..73e3db101 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp @@ -18,7 +18,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ /* -Author: Francisco Len Nßjera +Author: Francisco Leon Najera Concave-Concave Collision */ @@ -31,18 +31,16 @@ Concave-Concave Collision #include "btContactProcessing.h" #include "LinearMath/btQuickprof.h" - //! Class for accessing the plane equation class btPlaneShape : public btStaticPlaneShape { public: - btPlaneShape(const btVector3& v, float f) - :btStaticPlaneShape(v,f) + : btStaticPlaneShape(v, f) { } - void get_plane_equation(btVector4 &equation) + void get_plane_equation(btVector4& equation) { equation[0] = m_planeNormal[0]; equation[1] = m_planeNormal[1]; @@ -50,18 +48,16 @@ public: equation[3] = m_planeConstant; } - - void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) const + void get_plane_equation_transformed(const btTransform& trans, btVector4& equation) const { - equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal); - equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal); - equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal); - equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant; + const btVector3 normal = trans.getBasis() * m_planeNormal; + equation[0] = normal[0]; + equation[1] = normal[1]; + equation[2] = normal[2]; + equation[3] = normal.dot(trans * (m_planeConstant * m_planeNormal)); } }; - - ////////////////////////////////////////////////////////////////////////////////////////////// #ifdef TRI_COLLISION_PROFILING @@ -80,7 +76,7 @@ void bt_end_gim02_tri_time() g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds(); g_count_triangle_collision++; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING //! Retrieving shapes shapes /*! Declared here due of insuficent space on Pool allocators @@ -89,7 +85,7 @@ Declared here due of insuficent space on Pool allocators class GIM_ShapeRetriever { public: - const btGImpactShapeInterface * m_gim_shape; + const btGImpactShapeInterface* m_gim_shape; btTriangleShapeEx m_trishape; btTetrahedronShapeEx m_tetrashape; @@ -97,51 +93,50 @@ public: class ChildShapeRetriever { public: - GIM_ShapeRetriever * m_parent; - virtual const btCollisionShape * getChildShape(int index) + GIM_ShapeRetriever* m_parent; + virtual const btCollisionShape* getChildShape(int index) { return m_parent->m_gim_shape->getChildShape(index); } virtual ~ChildShapeRetriever() {} }; - class TriangleShapeRetriever:public ChildShapeRetriever + class TriangleShapeRetriever : public ChildShapeRetriever { public: - - virtual btCollisionShape * getChildShape(int index) + virtual btCollisionShape* getChildShape(int index) { - m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape); + m_parent->m_gim_shape->getBulletTriangle(index, m_parent->m_trishape); return &m_parent->m_trishape; } virtual ~TriangleShapeRetriever() {} }; - class TetraShapeRetriever:public ChildShapeRetriever + class TetraShapeRetriever : public ChildShapeRetriever { public: - - virtual btCollisionShape * getChildShape(int index) + virtual btCollisionShape* getChildShape(int index) { - m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape); + m_parent->m_gim_shape->getBulletTetrahedron(index, m_parent->m_tetrashape); return &m_parent->m_tetrashape; } }; + public: ChildShapeRetriever m_child_retriever; TriangleShapeRetriever m_tri_retriever; - TetraShapeRetriever m_tetra_retriever; - ChildShapeRetriever * m_current_retriever; + TetraShapeRetriever m_tetra_retriever; + ChildShapeRetriever* m_current_retriever; - GIM_ShapeRetriever(const btGImpactShapeInterface * gim_shape) + GIM_ShapeRetriever(const btGImpactShapeInterface* gim_shape) { m_gim_shape = gim_shape; //select retriever - if(m_gim_shape->needsRetrieveTriangles()) + if (m_gim_shape->needsRetrieveTriangles()) { m_current_retriever = &m_tri_retriever; } - else if(m_gim_shape->needsRetrieveTetrahedrons()) + else if (m_gim_shape->needsRetrieveTetrahedrons()) { m_current_retriever = &m_tetra_retriever; } @@ -153,32 +148,26 @@ public: m_current_retriever->m_parent = this; } - const btCollisionShape * getChildShape(int index) + const btCollisionShape* getChildShape(int index) { return m_current_retriever->getChildShape(index); } - - }; - - //!@} - #ifdef TRI_COLLISION_PROFILING //! Gets the average time in miliseconds of tree collisions float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime() { return btGImpactBoxSet::getAverageTreeCollisionTime(); - } //! Gets the average time in miliseconds of triangle collisions float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() { - if(g_count_triangle_collision == 0) return 0; + if (g_count_triangle_collision == 0) return 0; float avgtime = g_accum_triangle_collision_time; avgtime /= (float)g_count_triangle_collision; @@ -189,12 +178,10 @@ float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() return avgtime; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING - - -btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap) +btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap) { m_manifoldPtr = NULL; m_convex_algorithm = NULL; @@ -205,77 +192,62 @@ btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm() clearCache(); } - - - - -void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btVector3 & point, - const btVector3 & normal, - btScalar distance) +void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btVector3& point, + const btVector3& normal, + btScalar distance) { - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - checkManifold(body0Wrap,body1Wrap); - m_resultOut->addContactPoint(normal,point,distance); + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); + checkManifold(body0Wrap, body1Wrap); + m_resultOut->addContactPoint(normal, point, distance); } - void btGImpactCollisionAlgorithm::shape_vs_shape_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape * shape0, - const btCollisionShape * shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1) { - - { - - btCollisionAlgorithm* algor = newAlgorithm(body0Wrap,body1Wrap); + btCollisionAlgorithm* algor = newAlgorithm(body0Wrap, body1Wrap); // post : checkManifold is called - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - - algor->processCollision(body0Wrap,body1Wrap,*m_dispatchInfo,m_resultOut); - + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); + + algor->processCollision(body0Wrap, body1Wrap, *m_dispatchInfo, m_resultOut); + algor->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algor); } - } void btGImpactCollisionAlgorithm::convex_vs_convex_collision( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape* shape0, - const btCollisionShape* shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1) { + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - - btCollisionObjectWrapper ob0(body0Wrap,shape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0); - btCollisionObjectWrapper ob1(body1Wrap,shape1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),m_part1,m_triface1); - checkConvexAlgorithm(&ob0,&ob1); - m_convex_algorithm->processCollision(&ob0,&ob1,*m_dispatchInfo,m_resultOut); - - + btCollisionObjectWrapper ob0(body0Wrap, shape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0); + btCollisionObjectWrapper ob1(body1Wrap, shape1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), m_part1, m_triface1); + checkConvexAlgorithm(&ob0, &ob1); + m_convex_algorithm->processCollision(&ob0, &ob1, *m_dispatchInfo, m_resultOut); } - - - void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1,btPairSet & pairset) + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1, btPairSet& pairset) { - if(shape0->hasBoxSet() && shape1->hasBoxSet()) + if (shape0->hasBoxSet() && shape1->hasBoxSet()) { - btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset); + btGImpactBoxSet::find_collision(shape0->getBoxSet(), trans0, shape1->getBoxSet(), trans1, pairset); } else { @@ -283,74 +255,66 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( btAABB boxshape1; int i = shape0->getNumChildShapes(); - while(i--) + while (i--) { - shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + shape0->getChildAabb(i, trans0, boxshape0.m_min, boxshape0.m_max); int j = shape1->getNumChildShapes(); - while(j--) + while (j--) { - shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max); + shape1->getChildAabb(i, trans1, boxshape1.m_min, boxshape1.m_max); - if(boxshape1.has_collision(boxshape0)) + if (boxshape1.has_collision(boxshape0)) { - pairset.push_pair(i,j); + pairset.push_pair(i, j); } } } } - - } - void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1, - btAlignedObjectArray & collided_primitives) + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, + btAlignedObjectArray& collided_primitives) { - btAABB boxshape; - - if(shape0->hasBoxSet()) + if (shape0->hasBoxSet()) { btTransform trans1to0 = trans0.inverse(); trans1to0 *= trans1; - shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max); + shape1->getAabb(trans1to0, boxshape.m_min, boxshape.m_max); shape0->getBoxSet()->boxQuery(boxshape, collided_primitives); } else { - shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max); + shape1->getAabb(trans1, boxshape.m_min, boxshape.m_max); btAABB boxshape0; int i = shape0->getNumChildShapes(); - while(i--) + while (i--) { - shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + shape0->getChildAabb(i, trans0, boxshape0.m_min, boxshape0.m_max); - if(boxshape.has_collision(boxshape0)) + if (boxshape.has_collision(boxshape0)) { collided_primitives.push_back(i); } } - } - } - -void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count) +void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count) { btTriangleShapeEx tri0; btTriangleShapeEx tri1; @@ -358,27 +322,22 @@ void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectW shape0->lockChildShapes(); shape1->lockChildShapes(); - const int * pair_pointer = pairs; + const int* pair_pointer = pairs; - while(pair_count--) + while (pair_count--) { - m_triface0 = *(pair_pointer); - m_triface1 = *(pair_pointer+1); - pair_pointer+=2; - - - - shape0->getBulletTriangle(m_triface0,tri0); - shape1->getBulletTriangle(m_triface1,tri1); + m_triface1 = *(pair_pointer + 1); + pair_pointer += 2; + shape0->getBulletTriangle(m_triface0, tri0); + shape1->getBulletTriangle(m_triface1, tri1); //collide two convex shapes - if(tri0.overlap_test_conservative(tri1)) + if (tri0.overlap_test_conservative(tri1)) { - convex_vs_convex_collision(body0Wrap,body1Wrap,&tri0,&tri1); + convex_vs_convex_collision(body0Wrap, body1Wrap, &tri0, &tri1); } - } shape0->unlockChildShapes(); @@ -386,10 +345,10 @@ void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectW } void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count) { btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); @@ -401,119 +360,105 @@ void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectW shape0->lockChildShapes(); shape1->lockChildShapes(); - const int * pair_pointer = pairs; + const int* pair_pointer = pairs; - while(pair_count--) + while (pair_count--) { - m_triface0 = *(pair_pointer); - m_triface1 = *(pair_pointer+1); - pair_pointer+=2; + m_triface1 = *(pair_pointer + 1); + pair_pointer += 2; + shape0->getPrimitiveTriangle(m_triface0, ptri0); + shape1->getPrimitiveTriangle(m_triface1, ptri1); - shape0->getPrimitiveTriangle(m_triface0,ptri0); - shape1->getPrimitiveTriangle(m_triface1,ptri1); - - #ifdef TRI_COLLISION_PROFILING +#ifdef TRI_COLLISION_PROFILING bt_begin_gim02_tri_time(); - #endif +#endif ptri0.applyTransform(orgtrans0); ptri1.applyTransform(orgtrans1); - //build planes ptri0.buildTriPlane(); ptri1.buildTriPlane(); // test conservative - - - if(ptri0.overlap_test_conservative(ptri1)) + if (ptri0.overlap_test_conservative(ptri1)) { - if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data)) + if (ptri0.find_triangle_collision_clip_method(ptri1, contact_data)) { - int j = contact_data.m_point_count; - while(j--) + while (j--) { - addContactPoint(body0Wrap, body1Wrap, - contact_data.m_points[j], - contact_data.m_separating_normal, - -contact_data.m_penetration_depth); + contact_data.m_points[j], + contact_data.m_separating_normal, + -contact_data.m_penetration_depth); } } } - #ifdef TRI_COLLISION_PROFILING +#ifdef TRI_COLLISION_PROFILING bt_end_gim02_tri_time(); - #endif - +#endif } shape0->unlockChildShapes(); shape1->unlockChildShapes(); - } - void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1) { - - if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape0 = static_cast(shape0); + const btGImpactMeshShape* meshshape0 = static_cast(shape0); m_part0 = meshshape0->getMeshPartCount(); - while(m_part0--) + while (m_part0--) { - gimpact_vs_gimpact(body0Wrap,body1Wrap,meshshape0->getMeshPart(m_part0),shape1); + gimpact_vs_gimpact(body0Wrap, body1Wrap, meshshape0->getMeshPart(m_part0), shape1); } return; } - if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape1 = static_cast(shape1); + const btGImpactMeshShape* meshshape1 = static_cast(shape1); m_part1 = meshshape1->getMeshPartCount(); - while(m_part1--) + while (m_part1--) { - - gimpact_vs_gimpact(body0Wrap,body1Wrap,shape0,meshshape1->getMeshPart(m_part1)); - + gimpact_vs_gimpact(body0Wrap, body1Wrap, shape0, meshshape1->getMeshPart(m_part1)); } return; } - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); btPairSet pairset; - gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset); + gimpact_vs_gimpact_find_pairs(orgtrans0, orgtrans1, shape0, shape1, pairset); - if(pairset.size()== 0) return; + if (pairset.size() == 0) return; - if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART) { - const btGImpactMeshShapePart * shapepart0 = static_cast(shape0); - const btGImpactMeshShapePart * shapepart1 = static_cast(shape1); - //specialized function - #ifdef BULLET_TRIANGLE_COLLISION - collide_gjk_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); - #else - collide_sat_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); - #endif + const btGImpactMeshShapePart* shapepart0 = static_cast(shape0); + const btGImpactMeshShapePart* shapepart1 = static_cast(shape1); +//specialized function +#ifdef BULLET_TRIANGLE_COLLISION + collide_gjk_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size()); +#else + collide_sat_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size()); +#endif return; } @@ -530,32 +475,32 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( bool child_has_transform1 = shape1->childrenHasTransform(); int i = pairset.size(); - while(i--) + while (i--) { - GIM_PAIR * pair = &pairset[i]; + GIM_PAIR* pair = &pairset[i]; m_triface0 = pair->m_index1; m_triface1 = pair->m_index2; - const btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0); - const btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1); + const btCollisionShape* colshape0 = retriever0.getChildShape(m_triface0); + const btCollisionShape* colshape1 = retriever1.getChildShape(m_triface1); btTransform tr0 = body0Wrap->getWorldTransform(); btTransform tr1 = body1Wrap->getWorldTransform(); - if(child_has_transform0) + if (child_has_transform0) { - tr0 = orgtrans0*shape0->getChildTransform(m_triface0); + tr0 = orgtrans0 * shape0->getChildTransform(m_triface0); } - if(child_has_transform1) + if (child_has_transform1) { - tr1 = orgtrans1*shape1->getChildTransform(m_triface1); + tr1 = orgtrans1 * shape1->getChildTransform(m_triface1); } - btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),tr0,m_part0,m_triface0); - btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),tr1,m_part1,m_triface1); + btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), tr0, m_part0, m_triface0); + btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), tr1, m_part1, m_triface1); //collide two convex shapes - convex_vs_convex_collision(&ob0,&ob1,colshape0,colshape1); + convex_vs_convex_collision(&ob0, &ob1, colshape0, colshape1); } shape0->unlockChildShapes(); @@ -563,159 +508,159 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( } void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1,bool swapped) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, bool swapped) { - if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape0 = static_cast(shape0); + const btGImpactMeshShape* meshshape0 = static_cast(shape0); int& part = swapped ? m_part1 : m_part0; part = meshshape0->getMeshPartCount(); - while(part--) + while (part--) { - gimpact_vs_shape(body0Wrap, - body1Wrap, - meshshape0->getMeshPart(part), - shape1,swapped); - + body1Wrap, + meshshape0->getMeshPart(part), + shape1, swapped); } return; } - #ifdef GIMPACT_VS_PLANE_COLLISION - if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && +#ifdef GIMPACT_VS_PLANE_COLLISION + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && shape1->getShapeType() == STATIC_PLANE_PROXYTYPE) { - const btGImpactMeshShapePart * shapepart = static_cast(shape0); - const btStaticPlaneShape * planeshape = static_cast(shape1); - gimpacttrimeshpart_vs_plane_collision(body0Wrap,body1Wrap,shapepart,planeshape,swapped); + const btGImpactMeshShapePart* shapepart = static_cast(shape0); + const btStaticPlaneShape* planeshape = static_cast(shape1); + gimpacttrimeshpart_vs_plane_collision(body0Wrap, body1Wrap, shapepart, planeshape, swapped); return; } - #endif +#endif - - - if(shape1->isCompound()) + if (shape1->isCompound()) { - const btCompoundShape * compoundshape = static_cast(shape1); - gimpact_vs_compoundshape(body0Wrap,body1Wrap,shape0,compoundshape,swapped); + const btCompoundShape* compoundshape = static_cast(shape1); + gimpact_vs_compoundshape(body0Wrap, body1Wrap, shape0, compoundshape, swapped); return; } - else if(shape1->isConcave()) + else if (shape1->isConcave()) { - const btConcaveShape * concaveshape = static_cast(shape1); - gimpact_vs_concave(body0Wrap,body1Wrap,shape0,concaveshape,swapped); + const btConcaveShape* concaveshape = static_cast(shape1); + gimpact_vs_concave(body0Wrap, body1Wrap, shape0, concaveshape, swapped); return; } - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); btAlignedObjectArray collided_results; - gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results); - - if(collided_results.size() == 0) return; + gimpact_vs_shape_find_pairs(orgtrans0, orgtrans1, shape0, shape1, collided_results); + if (collided_results.size() == 0) return; shape0->lockChildShapes(); GIM_ShapeRetriever retriever0(shape0); - bool child_has_transform0 = shape0->childrenHasTransform(); - int i = collided_results.size(); - while(i--) + while (i--) { int child_index = collided_results[i]; - if(swapped) - m_triface1 = child_index; - else - m_triface0 = child_index; + if (swapped) + m_triface1 = child_index; + else + m_triface0 = child_index; - const btCollisionShape * colshape0 = retriever0.getChildShape(child_index); + const btCollisionShape* colshape0 = retriever0.getChildShape(child_index); btTransform tr0 = body0Wrap->getWorldTransform(); - if(child_has_transform0) + if (child_has_transform0) { - tr0 = orgtrans0*shape0->getChildTransform(child_index); + tr0 = orgtrans0 * shape0->getChildTransform(child_index); } - btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0); - const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap(); - - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob0.getCollisionObject()) + btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0); + const btCollisionObjectWrapper* prevObj; + + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject()) { + prevObj = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&ob0); - } else + } + else { + prevObj = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&ob0); } //collide two shapes - if(swapped) + if (swapped) { - - shape_vs_shape_collision(body1Wrap,&ob0,shape1,colshape0); + shape_vs_shape_collision(body1Wrap, &ob0, shape1, colshape0); } else { - - shape_vs_shape_collision(&ob0,body1Wrap,colshape0,shape1); + shape_vs_shape_collision(&ob0, body1Wrap, colshape0, shape1); } - m_resultOut->setBody0Wrap(prevObj0); + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject()) + { + m_resultOut->setBody0Wrap(prevObj); + } + else + { + m_resultOut->setBody1Wrap(prevObj); + } } shape0->unlockChildShapes(); - } void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactShapeInterface * shape0, - const btCompoundShape * shape1,bool swapped) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCompoundShape* shape1, bool swapped) { btTransform orgtrans1 = body1Wrap->getWorldTransform(); int i = shape1->getNumChildShapes(); - while(i--) + while (i--) { + const btCollisionShape* colshape1 = shape1->getChildShape(i); + btTransform childtrans1 = orgtrans1 * shape1->getChildTransform(i); - const btCollisionShape * colshape1 = shape1->getChildShape(i); - btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i); + btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), childtrans1, -1, i); - btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),childtrans1,-1,i); - const btCollisionObjectWrapper* tmp = 0; - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject()) { tmp = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&ob1); - } else + } + else { tmp = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&ob1); } //collide child shape gimpact_vs_shape(body0Wrap, &ob1, - shape0,colshape1,swapped); + shape0, colshape1, swapped); - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject()) { m_resultOut->setBody0Wrap(tmp); - } else + } + else { m_resultOut->setBody1Wrap(tmp); } @@ -723,27 +668,25 @@ void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObje } void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btStaticPlaneShape * shape1,bool swapped) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btStaticPlaneShape* shape1, bool swapped) { - - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); - const btPlaneShape * planeshape = static_cast(shape1); + const btPlaneShape* planeshape = static_cast(shape1); btVector4 plane; - planeshape->get_plane_equation_transformed(orgtrans1,plane); + planeshape->get_plane_equation_transformed(orgtrans1, plane); //test box against plane btAABB tribox; - shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max); + shape0->getAabb(orgtrans0, tribox.m_min, tribox.m_max); tribox.increment_margin(planeshape->getMargin()); - if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return; + if (tribox.plane_classify(plane) != BT_CONST_COLLIDE_PLANE) return; shape0->lockChildShapes(); @@ -751,28 +694,28 @@ void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( btVector3 vertex; int vi = shape0->getVertexCount(); - while(vi--) + while (vi--) { - shape0->getVertex(vi,vertex); + shape0->getVertex(vi, vertex); vertex = orgtrans0(vertex); btScalar distance = vertex.dot(plane) - plane[3] - margin; - if(distance<0.0)//add contact + if (distance < 0.0) //add contact { - if(swapped) + if (swapped) { addContactPoint(body1Wrap, body0Wrap, - vertex, - -plane, - distance); + vertex, + -plane, + distance); } else { addContactPoint(body0Wrap, body1Wrap, - vertex, - plane, - distance); + vertex, + plane, + distance); } } } @@ -780,69 +723,64 @@ void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( shape0->unlockChildShapes(); } - - - -class btGImpactTriangleCallback: public btTriangleCallback +class btGImpactTriangleCallback : public btTriangleCallback { public: - btGImpactCollisionAlgorithm * algorithm; - const btCollisionObjectWrapper * body0Wrap; - const btCollisionObjectWrapper * body1Wrap; - const btGImpactShapeInterface * gimpactshape0; + btGImpactCollisionAlgorithm* algorithm; + const btCollisionObjectWrapper* body0Wrap; + const btCollisionObjectWrapper* body1Wrap; + const btGImpactShapeInterface* gimpactshape0; bool swapped; btScalar margin; virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { - btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]); + btTriangleShapeEx tri1(triangle[0], triangle[1], triangle[2]); tri1.setMargin(margin); - if(swapped) - { - algorithm->setPart0(partId); - algorithm->setFace0(triangleIndex); - } - else - { - algorithm->setPart1(partId); - algorithm->setFace1(triangleIndex); - } + if (swapped) + { + algorithm->setPart0(partId); + algorithm->setFace0(triangleIndex); + } + else + { + algorithm->setPart1(partId); + algorithm->setFace1(triangleIndex); + } - btCollisionObjectWrapper ob1Wrap(body1Wrap,&tri1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),partId,triangleIndex); - const btCollisionObjectWrapper * tmp = 0; + btCollisionObjectWrapper ob1Wrap(body1Wrap, &tri1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), partId, triangleIndex); + const btCollisionObjectWrapper* tmp = 0; - if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) + if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject()) { tmp = algorithm->internalGetResultOut()->getBody0Wrap(); algorithm->internalGetResultOut()->setBody0Wrap(&ob1Wrap); - } else + } + else { tmp = algorithm->internalGetResultOut()->getBody1Wrap(); algorithm->internalGetResultOut()->setBody1Wrap(&ob1Wrap); } - - algorithm->gimpact_vs_shape( - body0Wrap,&ob1Wrap,gimpactshape0,&tri1,swapped); - if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) + algorithm->gimpact_vs_shape( + body0Wrap, &ob1Wrap, gimpactshape0, &tri1, swapped); + + if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject()) { algorithm->internalGetResultOut()->setBody0Wrap(tmp); - } else + } + else { algorithm->internalGetResultOut()->setBody1Wrap(tmp); } - } }; - - - void btGImpactCollisionAlgorithm::gimpact_vs_concave( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btConcaveShape * shape1,bool swapped) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btConcaveShape* shape1, bool swapped) { //create the callback btGImpactTriangleCallback tricallback; @@ -858,75 +796,71 @@ void btGImpactCollisionAlgorithm::gimpact_vs_concave( gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform(); - btVector3 minAABB,maxAABB; - shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB); - - shape1->processAllTriangles(&tricallback,minAABB,maxAABB); + btVector3 minAABB, maxAABB; + shape0->getAabb(gimpactInConcaveSpace, minAABB, maxAABB); + shape1->processAllTriangles(&tricallback, minAABB, maxAABB); } - - -void btGImpactCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btGImpactCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - clearCache(); + clearCache(); - m_resultOut = resultOut; + m_resultOut = resultOut; m_dispatchInfo = &dispatchInfo; - const btGImpactShapeInterface * gimpactshape0; - const btGImpactShapeInterface * gimpactshape1; + const btGImpactShapeInterface* gimpactshape0; + const btGImpactShapeInterface* gimpactshape1; - if (body0Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE) + if (body0Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape0 = static_cast(body0Wrap->getCollisionShape()); + gimpactshape0 = static_cast(body0Wrap->getCollisionShape()); - if( body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape1 = static_cast(body1Wrap->getCollisionShape()); + gimpactshape1 = static_cast(body1Wrap->getCollisionShape()); - gimpact_vs_gimpact(body0Wrap,body1Wrap,gimpactshape0,gimpactshape1); + gimpact_vs_gimpact(body0Wrap, body1Wrap, gimpactshape0, gimpactshape1); } else { - gimpact_vs_shape(body0Wrap,body1Wrap,gimpactshape0,body1Wrap->getCollisionShape(),false); + gimpact_vs_shape(body0Wrap, body1Wrap, gimpactshape0, body1Wrap->getCollisionShape(), false); } - } - else if (body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + else if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape1 = static_cast(body1Wrap->getCollisionShape()); + gimpactshape1 = static_cast(body1Wrap->getCollisionShape()); - gimpact_vs_shape(body1Wrap,body0Wrap,gimpactshape1,body0Wrap->getCollisionShape(),true); + gimpact_vs_shape(body1Wrap, body0Wrap, gimpactshape1, body0Wrap->getCollisionShape(), true); + } + + // Ensure that gContactProcessedCallback is called for concave shapes. + if (getLastManifold()) + { + m_resultOut->refreshContactPoints(); } } - -btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { return 1.f; - } ///////////////////////////////////// REGISTERING ALGORITHM ////////////////////////////////////////////// - - //! Use this function for register the algorithm externally -void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) +void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher* dispatcher) { - static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf; int i; - for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { - dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf); + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE, i, &s_gimpact_cf); } - for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { - dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf); + dispatcher->registerCollisionCreateFunc(i, GIMPACT_SHAPE_PROXYTYPE, &s_gimpact_cf); } - } diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h index 3e5675f72..a368c8a0c 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -42,7 +42,6 @@ class btDispatcher; #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - //! Collision Algorithm for GImpact Shapes /*! For register this algorithm in Bullet, proceed as following: @@ -54,36 +53,35 @@ btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher); class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm { protected: - btCollisionAlgorithm * m_convex_algorithm; - btPersistentManifold * m_manifoldPtr; + btCollisionAlgorithm* m_convex_algorithm; + btPersistentManifold* m_manifoldPtr; btManifoldResult* m_resultOut; - const btDispatcherInfo * m_dispatchInfo; + const btDispatcherInfo* m_dispatchInfo; int m_triface0; int m_part0; int m_triface1; int m_part1; - //! Creates a new contact point - SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(const btCollisionObject* body0,const btCollisionObject* body1) + SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(const btCollisionObject* body0, const btCollisionObject* body1) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_manifoldPtr = m_dispatcher->getNewManifold(body0, body1); return m_manifoldPtr; } SIMD_FORCE_INLINE void destroyConvexAlgorithm() { - if(m_convex_algorithm) + if (m_convex_algorithm) { m_convex_algorithm->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm); + m_dispatcher->freeCollisionAlgorithm(m_convex_algorithm); m_convex_algorithm = NULL; } } SIMD_FORCE_INLINE void destroyContactManifolds() { - if(m_manifoldPtr == NULL) return; + if (m_manifoldPtr == NULL) return; m_dispatcher->releaseManifold(m_manifoldPtr); m_manifoldPtr = NULL; } @@ -104,207 +102,187 @@ protected: return m_manifoldPtr; } - // Call before process collision - SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - if(getLastManifold() == 0) + if (getLastManifold() == 0) { - newContactManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + newContactManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); } m_resultOut->setPersistentManifold(getLastManifold()); } // Call before process collision - SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + SIMD_FORCE_INLINE btCollisionAlgorithm* newAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - checkManifold(body0Wrap,body1Wrap); + checkManifold(body0Wrap, body1Wrap); - btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm( - body0Wrap,body1Wrap,getLastManifold(), BT_CONTACT_POINT_ALGORITHMS); - return convex_algorithm ; + btCollisionAlgorithm* convex_algorithm = m_dispatcher->findAlgorithm( + body0Wrap, body1Wrap, getLastManifold(), BT_CONTACT_POINT_ALGORITHMS); + return convex_algorithm; } // Call before process collision - SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - if(m_convex_algorithm) return; - m_convex_algorithm = newAlgorithm(body0Wrap,body1Wrap); + if (m_convex_algorithm) return; + m_convex_algorithm = newAlgorithm(body0Wrap, body1Wrap); } + void addContactPoint(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btVector3& point, + const btVector3& normal, + btScalar distance); - - - void addContactPoint(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btVector3 & point, - const btVector3 & normal, - btScalar distance); - -//! Collision routines -//!@{ + //! Collision routines + //!@{ void collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count); + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count); void collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count); - - - + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count); void shape_vs_shape_collision( - const btCollisionObjectWrapper* body0, - const btCollisionObjectWrapper* body1, - const btCollisionShape * shape0, - const btCollisionShape * shape1); + const btCollisionObjectWrapper* body0, + const btCollisionObjectWrapper* body1, + const btCollisionShape* shape0, + const btCollisionShape* shape1); void convex_vs_convex_collision(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape* shape0, - const btCollisionShape* shape1); - - + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1); void gimpact_vs_gimpact_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1,btPairSet & pairset); + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1, btPairSet& pairset); void gimpact_vs_shape_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1, - btAlignedObjectArray & collided_primitives); - + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, + btAlignedObjectArray& collided_primitives); void gimpacttrimeshpart_vs_plane_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btStaticPlaneShape * shape1,bool swapped); - + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btStaticPlaneShape* shape1, bool swapped); public: - - btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btGImpactCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr) manifoldArray.push_back(m_manifoldPtr); } - btManifoldResult* internalGetResultOut() + btManifoldResult* internalGetResultOut() { return m_resultOut; } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm)); - return new(mem) btGImpactCollisionAlgorithm(ci,body0Wrap,body1Wrap); + return new (mem) btGImpactCollisionAlgorithm(ci, body0Wrap, body1Wrap); } }; //! Use this function for register the algorithm externally - static void registerAlgorithm(btCollisionDispatcher * dispatcher); + static void registerAlgorithm(btCollisionDispatcher* dispatcher); #ifdef TRI_COLLISION_PROFILING //! Gets the average time in miliseconds of tree collisions static float getAverageTreeCollisionTime(); //! Gets the average time in miliseconds of triangle collisions static float getAverageTriangleCollisionTime(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING //! Collides two gimpact shapes /*! \pre shape0 and shape1 couldn't be btGImpactMeshShape objects */ - void gimpact_vs_gimpact(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1); + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1); void gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1,bool swapped); + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, bool swapped); - void gimpact_vs_compoundshape(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btCompoundShape * shape1,bool swapped); + void gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCompoundShape* shape1, bool swapped); void gimpact_vs_concave( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btConcaveShape * shape1,bool swapped); - - - - - /// Accessor/Mutator pairs for Part and triangleID - void setFace0(int value) - { - m_triface0 = value; - } - int getFace0() - { - return m_triface0; - } - void setFace1(int value) - { - m_triface1 = value; - } - int getFace1() - { - return m_triface1; - } - void setPart0(int value) - { - m_part0 = value; - } - int getPart0() - { - return m_part0; - } - void setPart1(int value) - { - m_part1 = value; - } - int getPart1() - { - return m_part1; - } + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btConcaveShape* shape1, bool swapped); + /// Accessor/Mutator pairs for Part and triangleID + void setFace0(int value) + { + m_triface0 = value; + } + int getFace0() + { + return m_triface0; + } + void setFace1(int value) + { + m_triface1 = value; + } + int getFace1() + { + return m_triface1; + } + void setPart0(int value) + { + m_part0 = value; + } + int getPart0() + { + return m_part0; + } + void setPart1(int value) + { + m_part1 = value; + } + int getPart1() + { + return m_part1; + } }; - //algorithm details //#define BULLET_TRIANGLE_COLLISION 1 #define GIMPACT_VS_PLANE_COLLISION 1 - - -#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactMassUtil.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactMassUtil.h index 2543aefcf..1cde46ed8 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactMassUtil.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactMassUtil.h @@ -21,40 +21,36 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GIMPACT_MASS_UTIL_H #define GIMPACT_MASS_UTIL_H #include "LinearMath/btTransform.h" - - SIMD_FORCE_INLINE btVector3 gim_inertia_add_transformed( - const btVector3 & source_inertia, const btVector3 & added_inertia, const btTransform & transform) + const btVector3& source_inertia, const btVector3& added_inertia, const btTransform& transform) { - btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose(); + btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose(); btScalar x2 = transform.getOrigin()[0]; - x2*= x2; + x2 *= x2; btScalar y2 = transform.getOrigin()[1]; - y2*= y2; + y2 *= y2; btScalar z2 = transform.getOrigin()[2]; - z2*= z2; + z2 *= z2; - btScalar ix = rotatedTensor[0][0]*(y2+z2); - btScalar iy = rotatedTensor[1][1]*(x2+z2); - btScalar iz = rotatedTensor[2][2]*(x2+y2); + btScalar ix = rotatedTensor[0][0] * (y2 + z2); + btScalar iy = rotatedTensor[1][1] * (x2 + z2); + btScalar iz = rotatedTensor[2][2] * (x2 + y2); - return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz); + return btVector3(source_inertia[0] + ix, source_inertia[1] + iy, source_inertia[2] + iz); } -SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3 & point, btScalar mass) +SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3& point, btScalar mass) { - btScalar x2 = point[0]*point[0]; - btScalar y2 = point[1]*point[1]; - btScalar z2 = point[2]*point[2]; - return btVector3(mass*(y2+z2),mass*(x2+z2),mass*(x2+y2)); + btScalar x2 = point[0] * point[0]; + btScalar y2 = point[1] * point[1]; + btScalar z2 = point[2] * point[2]; + return btVector3(mass * (y2 + z2), mass * (x2 + z2), mass * (x2 + y2)); } - -#endif //GIMPACT_MESH_SHAPE_H +#endif //GIMPACT_MESH_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp index 4528758c3..b81fc9704 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp @@ -27,11 +27,9 @@ subject to the following restrictions: #ifdef TRI_COLLISION_PROFILING btClock g_q_tree_clock; - float g_q_accum_tree_collision_time = 0; int g_q_count_traversing = 0; - void bt_begin_gim02_q_tree_time() { g_q_tree_clock.reset(); @@ -43,11 +41,10 @@ void bt_end_gim02_q_tree_time() g_q_count_traversing++; } - //! Gets the average time in miliseconds of tree collisions float btGImpactQuantizedBvh::getAverageTreeCollisionTime() { - if(g_q_count_traversing == 0) return 0; + if (g_q_count_traversing == 0) return 0; float avgtime = g_q_accum_tree_collision_time; avgtime /= (float)g_q_count_traversing; @@ -56,99 +53,92 @@ float btGImpactQuantizedBvh::getAverageTreeCollisionTime() g_q_count_traversing = 0; return avgtime; -// float avgtime = g_q_count_traversing; -// g_q_count_traversing = 0; -// return avgtime; - + // float avgtime = g_q_count_traversing; + // g_q_count_traversing = 0; + // return avgtime; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING /////////////////////// btQuantizedBvhTree ///////////////////////////////// void btQuantizedBvhTree::calc_quantization( - GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) + GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin) { //calc globa box btAABB global_bound; global_bound.invalidate(); - for (int i=0;i splitValue) { //swap - primitive_boxes.swap(i,splitIndex); + primitive_boxes.swap(i, splitIndex); //swapLeafNodes(i,splitIndex); splitIndex++; } @@ -163,32 +153,30 @@ int btQuantizedBvhTree::_sort_and_calc_splitting_index( //bool unbalanced2 = true; //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + int rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); + btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); return splitIndex; - } - -void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex) { int curIndex = m_num_nodes; m_num_nodes++; - btAssert((endIndex-startIndex)>0); + btAssert((endIndex - startIndex) > 0); - if ((endIndex-startIndex)==1) + if ((endIndex - startIndex) == 1) { - //We have a leaf node - setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + //We have a leaf node + setNodeBound(curIndex, primitive_boxes[startIndex].m_bound); m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); return; @@ -196,48 +184,43 @@ void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, i //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. //split axis - int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); + int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex); splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex, - splitIndex//split axis - ); - + primitive_boxes, startIndex, endIndex, + splitIndex //split axis + ); //calc this node bounding box btAABB node_bound; node_bound.invalidate(); - for (int i=startIndex;iget_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); + m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox); + setNodeBound(nodecount, leafbox); } else { @@ -265,20 +248,20 @@ void btGImpactQuantizedBvh::refit() btAABB temp_box; int child_node = getLeftNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } child_node = getRightNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } - setNodeBound(nodecount,bound); + setNodeBound(nodecount, bound); } } } @@ -290,17 +273,17 @@ void btGImpactQuantizedBvh::buildSet() GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); - for (int i = 0;iget_primitive_box(i,primitive_boxes[i].m_bound); - primitive_boxes[i].m_data = i; + m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound); + primitive_boxes[i].m_data = i; } m_box_tree.build_tree(primitive_boxes); } //! returns the indices of the primitives in the m_primitive_manager -bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray & collided_results) const +bool btGImpactQuantizedBvh::boxQuery(const btAABB& box, btAlignedObjectArray& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -310,16 +293,14 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray0) return true; + if (collided_results.size() > 0) return true; return false; } - - //! returns the indices of the primitives in the m_primitive_manager bool btGImpactQuantizedBvh::rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray & collided_results) const + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -355,16 +334,16 @@ bool btGImpactQuantizedBvh::rayQuery( while (curIndex < numNodes) { btAABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) { - collided_results.push_back(getNodeData( curIndex)); + collided_results.push_back(getNodeData(curIndex)); } if (aabbOverlap || isleafnode) @@ -375,154 +354,133 @@ bool btGImpactQuantizedBvh::rayQuery( else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } - SIMD_FORCE_INLINE bool _quantized_node_collision( - const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, - int node0 ,int node1, bool complete_primitive_tests) + const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1, + const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, + int node0, int node1, bool complete_primitive_tests) { btAABB box0; - boxset0->getNodeBound(node0,box0); + boxset0->getNodeBound(node0, box0); btAABB box1; - boxset1->getNodeBound(node1,box1); - - return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests ); -// box1.appy_transform_trans_cache(trans_cache_1to0); -// return box0.has_collision(box1); + boxset1->getNodeBound(node1, box1); + return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests); + // box1.appy_transform_trans_cache(trans_cache_1to0); + // return box0.has_collision(box1); } - //stackless recursive collision routine static void _find_quantized_collision_pairs_recursive( - const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1, - btPairSet * collision_pairs, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, + const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1, + btPairSet* collision_pairs, + const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, int node0, int node1, bool complete_primitive_tests) { + if (_quantized_node_collision( + boxset0, boxset1, trans_cache_1to0, + node0, node1, complete_primitive_tests) == false) return; //avoid colliding internal nodes - - - if( _quantized_node_collision( - boxset0,boxset1,trans_cache_1to0, - node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes - - if(boxset0->isLeafNode(node0)) + if (boxset0->isLeafNode(node0)) { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { // collision result collision_pairs->push_pair( - boxset0->getNodeData(node0),boxset1->getNodeData(node1)); + boxset0->getNodeData(node0), boxset1->getNodeData(node1)); return; } else { - //collide left recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getLeftNode(node1), false); //collide right recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getRightNode(node1),false); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getRightNode(node1), false); } } else { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { - //collide left recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),node1,false); - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), node1, false); //collide right recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),node1,false); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), node1, false); } else { //collide left0 left1 - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false); //collide left0 right1 _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false); - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false); //collide right0 left1 _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false); //collide right0 right1 _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getRightNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), boxset1->getRightNode(node1), false); - }// else if node1 is not a leaf - }// else if node0 is not a leaf + } // else if node1 is not a leaf + } // else if node0 is not a leaf } - -void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0, - const btGImpactQuantizedBvh * boxset1, const btTransform & trans1, - btPairSet & collision_pairs) +void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh* boxset0, const btTransform& trans0, + const btGImpactQuantizedBvh* boxset1, const btTransform& trans1, + btPairSet& collision_pairs) { - - if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; + if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return; BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - trans_cache_1to0.calc_from_homogenic(trans0,trans1); + trans_cache_1to0.calc_from_homogenic(trans0, trans1); #ifdef TRI_COLLISION_PROFILING bt_begin_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - &collision_pairs,trans_cache_1to0,0,0,true); + boxset0, boxset1, + &collision_pairs, trans_cache_1to0, 0, 0, true); #ifdef TRI_COLLISION_PROFILING bt_end_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING - +#endif //TRI_COLLISION_PROFILING } - - diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h index 42e5520fc..b231c1e83 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h @@ -28,13 +28,10 @@ subject to the following restrictions: #include "btQuantization.h" #include "btGImpactQuantizedBvhStructs.h" -class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray +class GIM_QUANTIZED_BVH_NODE_ARRAY : public btAlignedObjectArray { }; - - - //! Basic Box tree structure class btQuantizedBvhTree { @@ -43,16 +40,18 @@ protected: GIM_QUANTIZED_BVH_NODE_ARRAY m_node_array; btAABB m_global_bound; btVector3 m_bvhQuantization; + protected: - void calc_quantization(GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) ); + void calc_quantization(GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin = btScalar(1.0)); int _sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, - int startIndex, int endIndex, int splitAxis); + GIM_BVH_DATA_ARRAY& primitive_boxes, + int startIndex, int endIndex, int splitAxis); - int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + int _calc_splitting_axis(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); + + void _build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); - void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); public: btQuantizedBvhTree() { @@ -61,20 +60,19 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); + void build_tree(GIM_BVH_DATA_ARRAY& primitive_boxes); SIMD_FORCE_INLINE void quantizePoint( - unsigned short * quantizedpoint, const btVector3 & point) const + unsigned short* quantizedpoint, const btVector3& point) const { - bt_quantize_clamp(quantizedpoint,point,m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization); + bt_quantize_clamp(quantizedpoint, point, m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization); } - SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( int node_index, - unsigned short * quantizedMin,unsigned short * quantizedMax) const + unsigned short* quantizedMin, unsigned short* quantizedMax) const { - return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax); + return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin, quantizedMax); } SIMD_FORCE_INLINE void clearNodes() @@ -100,41 +98,41 @@ public: return m_node_array[nodeindex].getDataIndex(); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const { bound.m_min = bt_unquantize( m_node_array[nodeindex].m_quantizedAabbMin, - m_global_bound.m_min,m_bvhQuantization); + m_global_bound.m_min, m_bvhQuantization); bound.m_max = bt_unquantize( m_node_array[nodeindex].m_quantizedAabbMax, - m_global_bound.m_min,m_bvhQuantization); + m_global_bound.m_min, m_bvhQuantization); } - SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound) { - bt_quantize_clamp( m_node_array[nodeindex].m_quantizedAabbMin, - bound.m_min, - m_global_bound.m_min, - m_global_bound.m_max, - m_bvhQuantization); + bt_quantize_clamp(m_node_array[nodeindex].m_quantizedAabbMin, + bound.m_min, + m_global_bound.m_min, + m_global_bound.m_max, + m_bvhQuantization); - bt_quantize_clamp( m_node_array[nodeindex].m_quantizedAabbMax, - bound.m_max, - m_global_bound.m_min, - m_global_bound.m_max, - m_bvhQuantization); + bt_quantize_clamp(m_node_array[nodeindex].m_quantizedAabbMax, + bound.m_max, + m_global_bound.m_min, + m_global_bound.m_max, + m_bvhQuantization); } SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const { - return nodeindex+1; + return nodeindex + 1; } SIMD_FORCE_INLINE int getRightNode(int nodeindex) const { - if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2; - return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex(); + if (m_node_array[nodeindex + 1].isLeafNode()) return nodeindex + 2; + return nodeindex + 1 + m_node_array[nodeindex + 1].getEscapeIndex(); } SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const @@ -142,7 +140,7 @@ public: return m_node_array[nodeindex].getEscapeIndex(); } - SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE* get_node_pointer(int index = 0) const { return &m_node_array[index]; } @@ -150,8 +148,6 @@ public: //!@} }; - - //! Structure for containing Boxes /*! This class offers an structure for managing a box tree of primitives. @@ -161,13 +157,13 @@ class btGImpactQuantizedBvh { protected: btQuantizedBvhTree m_box_tree; - btPrimitiveManagerBase * m_primitive_manager; + btPrimitiveManagerBase* m_primitive_manager; protected: //stackless refit void refit(); -public: +public: //! this constructor doesn't build the tree. you must call buildSet btGImpactQuantizedBvh() { @@ -175,31 +171,30 @@ public: } //! this constructor doesn't build the tree. you must call buildSet - btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager) + btGImpactQuantizedBvh(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btAABB getGlobalBox() const + SIMD_FORCE_INLINE btAABB getGlobalBox() const { btAABB totalbox; getNodeBound(0, totalbox); return totalbox; } - SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) + SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const + SIMD_FORCE_INLINE btPrimitiveManagerBase* getPrimitiveManager() const { return m_primitive_manager; } - -//! node manager prototype functions -///@{ + //! node manager prototype functions + ///@{ //! this attemps to refit the box set. SIMD_FORCE_INLINE void update() @@ -211,21 +206,21 @@ public: void buildSet(); //! returns the indices of the primitives in the m_primitive_manager - bool boxQuery(const btAABB & box, btAlignedObjectArray & collided_results) const; + bool boxQuery(const btAABB& box, btAlignedObjectArray& collided_results) const; //! returns the indices of the primitives in the m_primitive_manager - SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box, - const btTransform & transform, btAlignedObjectArray & collided_results) const + SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB& box, + const btTransform& transform, btAlignedObjectArray& collided_results) const { - btAABB transbox=box; + btAABB transbox = box; transbox.appy_transform(transform); - return boxQuery(transbox,collided_results); + return boxQuery(transbox, collided_results); } //! returns the indices of the primitives in the m_primitive_manager bool rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray & collided_results) const; + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray& collided_results) const; //! tells if this set has hierarcht SIMD_FORCE_INLINE bool hasHierarchy() const @@ -234,7 +229,7 @@ public: } //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const + SIMD_FORCE_INLINE bool isTrimesh() const { return m_primitive_manager->is_trimesh(); } @@ -256,17 +251,16 @@ public: return m_box_tree.getNodeData(nodeindex); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const { m_box_tree.getNodeBound(nodeindex, bound); } - SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound) { m_box_tree.setNodeBound(nodeindex, bound); } - SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const { return m_box_tree.getLeftNode(nodeindex); @@ -282,24 +276,23 @@ public: return m_box_tree.getEscapeNodeIndex(nodeindex); } - SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const + SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex, btPrimitiveTriangle& triangle) const { - m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); + m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex), triangle); } - - SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE* get_node_pointer(int index = 0) const { return m_box_tree.get_node_pointer(index); } #ifdef TRI_COLLISION_PROFILING static float getAverageTreeCollisionTime(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING - static void find_collision(const btGImpactQuantizedBvh * boxset1, const btTransform & trans1, - const btGImpactQuantizedBvh * boxset2, const btTransform & trans2, - btPairSet & collision_pairs); + static void find_collision(const btGImpactQuantizedBvh* boxset1, const btTransform& trans1, + const btGImpactQuantizedBvh* boxset2, const btTransform& trans2, + btPairSet& collision_pairs); }; -#endif // GIM_BOXPRUNING_H_INCLUDED +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h index 7dd5a1b9d..bd50cb5b8 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h @@ -29,13 +29,14 @@ subject to the following restrictions: ///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 +ATTRIBUTE_ALIGNED16(struct) +BT_QUANTIZED_BVH_NODE { //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; //4 bytes - int m_escapeIndexOrDataIndex; + int m_escapeIndexOrDataIndex; BT_QUANTIZED_BVH_NODE() { @@ -45,7 +46,7 @@ ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE SIMD_FORCE_INLINE bool isLeafNode() const { //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrDataIndex>=0); + return (m_escapeIndexOrDataIndex >= 0); } SIMD_FORCE_INLINE int getEscapeIndex() const @@ -72,20 +73,19 @@ ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE } SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( - unsigned short * quantizedMin,unsigned short * quantizedMax) const + 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]) + 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 +#endif // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactShape.cpp b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactShape.cpp index 30c85e3ff..34c229a3a 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactShape.cpp +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactShape.cpp @@ -18,178 +18,169 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btGImpactShape.h" #include "btGImpactMassUtil.h" - -btGImpactMeshShapePart::btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part ) +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 ); + // 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(); + // 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 + // moved from .h to .cpp because of conditional compilation #if BT_THREADSAFE - m_primitive_manager.unlock(); + 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(); + // 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 +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(); + // 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 - -void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btGImpactCompoundShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { lockChildShapes(); #ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(0.f, 0.f, 0.f); int i = this->getNumChildShapes(); - btScalar shapemass = mass/btScalar(i); + btScalar shapemass = mass / btScalar(i); - while(i--) + while (i--) { btVector3 temp_inertia; - m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia); - if(childrenHasTransform()) + m_childShapes[i]->calculateLocalInertia(shapemass, temp_inertia); + if (childrenHasTransform()) { - inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]); + inertia = gim_inertia_add_transformed(inertia, temp_inertia, m_childTransforms[i]); } else { - inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity()); + inertia = gim_inertia_add_transformed(inertia, temp_inertia, btTransform::getIdentity()); } - } #else // Calc box inertia - btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; - btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; - btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0]; + btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1]; + btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2]; + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(0.08333333); - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); #endif unlockChildShapes(); } - - -void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass, btVector3& inertia) const { lockChildShapes(); - #ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(0.f, 0.f, 0.f); int i = this->getVertexCount(); - btScalar pointmass = mass/btScalar(i); + btScalar pointmass = mass / btScalar(i); - while(i--) + while (i--) { btVector3 pointintertia; - this->getVertex(i,pointintertia); - pointintertia = gim_get_point_inertia(pointintertia,pointmass); - inertia+=pointintertia; + this->getVertex(i, pointintertia); + pointintertia = gim_get_point_inertia(pointintertia, pointmass); + inertia += pointintertia; } #else // Calc box inertia - btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; - btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; - btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0]; + btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1]; + btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2]; + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(0.08333333); - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); #endif unlockChildShapes(); } -void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btGImpactMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { - #ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(0.f, 0.f, 0.f); int i = this->getMeshPartCount(); - btScalar partmass = mass/btScalar(i); + btScalar partmass = mass / btScalar(i); - while(i--) + while (i--) { btVector3 partinertia; - getMeshPart(i)->calculateLocalInertia(partmass,partinertia); - inertia+=partinertia; + getMeshPart(i)->calculateLocalInertia(partmass, partinertia); + inertia += partinertia; } #else // Calc box inertia - btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; - btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; - btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0]; + btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1]; + btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2]; + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(0.08333333); - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); #endif } @@ -198,7 +189,7 @@ void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayT { } -void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const +void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const { lockChildShapes(); @@ -207,7 +198,7 @@ void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback rayDir.normalize(); m_box_set.rayQuery(rayDir, rayFrom, collided); - if(collided.size()==0) + if (collided.size() == 0) { unlockChildShapes(); return; @@ -216,15 +207,15 @@ void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback int part = (int)getPart(); btPrimitiveTriangle triangle; int i = collided.size(); - while(i--) + while (i--) { - getPrimitiveTriangle(collided[i],triangle); - callback->processTriangle(triangle.m_vertices,part,collided[i]); + getPrimitiveTriangle(collided[i], triangle); + callback->processTriangle(triangle.m_vertices, part, collided[i]); } unlockChildShapes(); } -void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { lockChildShapes(); btAABB box; @@ -232,9 +223,9 @@ void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,co box.m_max = aabbMax; btAlignedObjectArray collided; - m_box_set.boxQuery(box,collided); + m_box_set.boxQuery(box, collided); - if(collided.size()==0) + if (collided.size() == 0) { unlockChildShapes(); return; @@ -243,40 +234,38 @@ void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,co int part = (int)getPart(); btPrimitiveTriangle triangle; int i = collided.size(); - while(i--) + while (i--) { - this->getPrimitiveTriangle(collided[i],triangle); - callback->processTriangle(triangle.m_vertices,part,collided[i]); + this->getPrimitiveTriangle(collided[i], triangle); + callback->processTriangle(triangle.m_vertices, part, collided[i]); } unlockChildShapes(); - } -void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { int i = m_mesh_parts.size(); - while(i--) + while (i--) { - m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax); + m_mesh_parts[i]->processAllTriangles(callback, aabbMin, aabbMax); } } -void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const +void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const { int i = m_mesh_parts.size(); - while(i--) + while (i--) { m_mesh_parts[i]->processAllTrianglesRay(callback, rayFrom, rayTo); } } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*) dataBuffer; + btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*)dataBuffer; - btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer); + btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer); m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer); @@ -288,4 +277,3 @@ const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serial return "btGImpactMeshShapeData"; } - diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactShape.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactShape.h index 9d7e40562..5b85e8704 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactShape.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactShape.h @@ -21,7 +21,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GIMPACT_SHAPE_H #define GIMPACT_SHAPE_H @@ -37,8 +36,7 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btAlignedObjectArray.h" -#include "btGImpactQuantizedBvh.h" // box tree class - +#include "btGImpactQuantizedBvh.h" // box tree class //! declare Quantized trees, (you can change to float based trees) typedef btGImpactQuantizedBvh btGImpactBoxSet; @@ -50,10 +48,8 @@ enum eGIMPACT_SHAPE_TYPE CONST_GIMPACT_TRIMESH_SHAPE }; - - //! Helper class for tetrahedrons -class btTetrahedronShapeEx:public btBU_Simplex1to4 +class btTetrahedronShapeEx : public btBU_Simplex1to4 { public: btTetrahedronShapeEx() @@ -61,10 +57,9 @@ public: m_numVertices = 4; } - SIMD_FORCE_INLINE void setVertices( - const btVector3 & v0,const btVector3 & v1, - const btVector3 & v2,const btVector3 & v3) + const btVector3& v0, const btVector3& v1, + const btVector3& v2, const btVector3& v3) { m_vertices[0] = v0; m_vertices[1] = v1; @@ -74,45 +69,42 @@ public: } }; - //! Base class for gimpact shapes class btGImpactShapeInterface : public btConcaveShape { protected: - btAABB m_localAABB; - bool m_needs_update; - btVector3 localScaling; - btGImpactBoxSet m_box_set;// optionally boxset + btAABB m_localAABB; + bool m_needs_update; + btVector3 localScaling; + btGImpactBoxSet m_box_set; // optionally boxset //! use this function for perfofm refit in bounding boxes - //! use this function for perfofm refit in bounding boxes - virtual void calcLocalAABB() - { + //! use this function for perfofm refit in bounding boxes + virtual void calcLocalAABB() + { lockChildShapes(); - if(m_box_set.getNodeCount() == 0) - { - m_box_set.buildSet(); - } - else - { - m_box_set.update(); - } - unlockChildShapes(); - - m_localAABB = m_box_set.getGlobalBox(); - } + if (m_box_set.getNodeCount() == 0) + { + m_box_set.buildSet(); + } + else + { + m_box_set.update(); + } + unlockChildShapes(); + m_localAABB = m_box_set.getGlobalBox(); + } public: btGImpactShapeInterface() { - m_shapeType=GIMPACT_SHAPE_PROXYTYPE; + m_shapeType = GIMPACT_SHAPE_PROXYTYPE; m_localAABB.invalidate(); m_needs_update = true; - localScaling.setValue(1.f,1.f,1.f); + localScaling.setValue(1.f, 1.f, 1.f); } - //! performs refit operation /*! Updates the entire Box set of this shape. @@ -120,47 +112,46 @@ public: will does nothing. \post if m_needs_update == true, then it calls calcLocalAABB(); */ - SIMD_FORCE_INLINE void updateBound() - { - if(!m_needs_update) return; - calcLocalAABB(); - m_needs_update = false; - } + SIMD_FORCE_INLINE void updateBound() + { + if (!m_needs_update) return; + calcLocalAABB(); + m_needs_update = false; + } - //! If the Bounding box is not updated, then this class attemps to calculate it. - /*! + //! If the Bounding box is not updated, then this class attemps to calculate it. + /*! \post Calls updateBound() for update the box set. */ - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - btAABB transformedbox = m_localAABB; - transformedbox.appy_transform(t); - aabbMin = transformedbox.m_min; - aabbMax = transformedbox.m_max; - } + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + btAABB transformedbox = m_localAABB; + transformedbox.appy_transform(t); + aabbMin = transformedbox.m_min; + aabbMax = transformedbox.m_max; + } - //! Tells to this object that is needed to refit the box set - virtual void postUpdate() - { - m_needs_update = true; - } + //! Tells to this object that is needed to refit the box set + virtual void postUpdate() + { + m_needs_update = true; + } //! Obtains the local box, which is the global calculated box of the total of subshapes - SIMD_FORCE_INLINE const btAABB & getLocalBox() + SIMD_FORCE_INLINE const btAABB& getLocalBox() { return m_localAABB; } + virtual int getShapeType() const + { + return GIMPACT_SHAPE_PROXYTYPE; + } - virtual int getShapeType() const - { - return GIMPACT_SHAPE_PROXYTYPE; - } - - /*! + /*! \post You must call updateBound() for update the box set. */ - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { localScaling = scaling; postUpdate(); @@ -171,46 +162,43 @@ public: return localScaling; } - virtual void setMargin(btScalar margin) - { - m_collisionMargin = margin; - int i = getNumChildShapes(); - while(i--) - { + { + m_collisionMargin = margin; + int i = getNumChildShapes(); + while (i--) + { btCollisionShape* child = getChildShape(i); child->setMargin(margin); - } + } m_needs_update = true; - } - + } //! Subshape member functions //!@{ //! Base method for determinig which kind of GIMPACT shape we get - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const = 0 ; + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const = 0; //! gets boxset - SIMD_FORCE_INLINE const btGImpactBoxSet * getBoxSet() const + SIMD_FORCE_INLINE const btGImpactBoxSet* getBoxSet() const { return &m_box_set; } //! Determines if this class has a hierarchy structure for sorting its primitives - SIMD_FORCE_INLINE bool hasBoxSet() const + SIMD_FORCE_INLINE bool hasBoxSet() const { - if(m_box_set.getNodeCount() == 0) return false; + if (m_box_set.getNodeCount() == 0) return false; return true; } //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const = 0; - + virtual const btPrimitiveManagerBase* getPrimitiveManager() const = 0; //! Gets the number of children - virtual int getNumChildShapes() const = 0; + virtual int getNumChildShapes() const = 0; //! if true, then its children must get transforms. virtual bool childrenHasTransform() const = 0; @@ -221,11 +209,9 @@ public: //! Determines if this shape has tetrahedrons virtual bool needsRetrieveTetrahedrons() const = 0; - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const = 0; - - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const = 0; - + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const = 0; + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const = 0; //! call when reading child shapes virtual void lockChildShapes() const @@ -237,94 +223,91 @@ public: } //! if this trimesh - SIMD_FORCE_INLINE void getPrimitiveTriangle(int index,btPrimitiveTriangle & triangle) const + SIMD_FORCE_INLINE void getPrimitiveTriangle(int index, btPrimitiveTriangle& triangle) const { - getPrimitiveManager()->get_primitive_triangle(index,triangle); + getPrimitiveManager()->get_primitive_triangle(index, triangle); } - //! Retrieves the bound from a child - /*! + /*! */ - virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - btAABB child_aabb; - getPrimitiveManager()->get_primitive_box(child_index,child_aabb); - child_aabb.appy_transform(t); - aabbMin = child_aabb.m_min; - aabbMax = child_aabb.m_max; - } + virtual void getChildAabb(int child_index, const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + btAABB child_aabb; + getPrimitiveManager()->get_primitive_box(child_index, child_aabb); + child_aabb.appy_transform(t); + aabbMin = child_aabb.m_min; + aabbMax = child_aabb.m_max; + } //! Gets the children virtual btCollisionShape* getChildShape(int index) = 0; - //! Gets the child virtual const btCollisionShape* getChildShape(int index) const = 0; //! Gets the children transform - virtual btTransform getChildTransform(int index) const = 0; + virtual btTransform getChildTransform(int index) const = 0; //! Sets the children transform /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) = 0; + virtual void setChildTransform(int index, const btTransform& transform) = 0; //!@} - //! virtual method for ray collision - virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const { - (void) rayFrom; (void) rayTo; (void) resultCallback; + (void)rayFrom; + (void)rayTo; + (void)resultCallback; } //! Function for retrieve triangles. /*! It gives the triangles in local space */ - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const + virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - (void) callback; (void) aabbMin; (void) aabbMax; + (void)callback; + (void)aabbMin; + (void)aabbMax; } //! Function for retrieve triangles. /*! It gives the triangles in local space */ - virtual void processAllTrianglesRay(btTriangleCallback* /*callback*/,const btVector3& /*rayFrom*/, const btVector3& /*rayTo*/) const + virtual void processAllTrianglesRay(btTriangleCallback* /*callback*/, const btVector3& /*rayFrom*/, const btVector3& /*rayTo*/) const { - } //!@} - }; - //! btGImpactCompoundShape allows to handle multiple btCollisionShape objects at once /*! This class only can manage Convex subshapes */ -class btGImpactCompoundShape : public btGImpactShapeInterface +class btGImpactCompoundShape : public btGImpactShapeInterface { public: //! compound primitive manager - class CompoundPrimitiveManager:public btPrimitiveManagerBase + class CompoundPrimitiveManager : public btPrimitiveManagerBase { public: virtual ~CompoundPrimitiveManager() {} - btGImpactCompoundShape * m_compoundShape; - + btGImpactCompoundShape* m_compoundShape; CompoundPrimitiveManager(const CompoundPrimitiveManager& compound) - : btPrimitiveManagerBase() + : btPrimitiveManagerBase() { m_compoundShape = compound.m_compoundShape; } - CompoundPrimitiveManager(btGImpactCompoundShape * compoundShape) + CompoundPrimitiveManager(btGImpactCompoundShape* compoundShape) { m_compoundShape = compoundShape; } @@ -341,13 +324,13 @@ public: virtual int get_primitive_count() const { - return (int )m_compoundShape->getNumChildShapes(); + return (int)m_compoundShape->getNumChildShapes(); } - virtual void get_primitive_box(int prim_index ,btAABB & primbox) const + virtual void get_primitive_box(int prim_index, btAABB& primbox) const { btTransform prim_trans; - if(m_compoundShape->childrenHasTransform()) + if (m_compoundShape->childrenHasTransform()) { prim_trans = m_compoundShape->getChildTransform(prim_index); } @@ -356,30 +339,26 @@ public: prim_trans.setIdentity(); } const btCollisionShape* shape = m_compoundShape->getChildShape(prim_index); - shape->getAabb(prim_trans,primbox.m_min,primbox.m_max); + shape->getAabb(prim_trans, primbox.m_min, primbox.m_max); } - virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const + virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const { btAssert(0); - (void) prim_index; (void) triangle; + (void)prim_index; + (void)triangle; } - }; - - protected: CompoundPrimitiveManager m_primitive_manager; - btAlignedObjectArray m_childTransforms; - btAlignedObjectArray m_childShapes; - + btAlignedObjectArray m_childTransforms; + btAlignedObjectArray m_childShapes; public: - btGImpactCompoundShape(bool children_has_transform = true) { - (void) children_has_transform; + (void)children_has_transform; m_primitive_manager.m_compoundShape = this; m_box_set.setPrimitiveManager(&m_primitive_manager); } @@ -388,36 +367,33 @@ public: { } - //! if true, then its children must get transforms. virtual bool childrenHasTransform() const { - if(m_childTransforms.size()==0) return false; + if (m_childTransforms.size() == 0) return false; return true; } - //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const + virtual const btPrimitiveManagerBase* getPrimitiveManager() const { return &m_primitive_manager; } //! Obtains the compopund primitive manager - SIMD_FORCE_INLINE CompoundPrimitiveManager * getCompoundPrimitiveManager() + SIMD_FORCE_INLINE CompoundPrimitiveManager* getCompoundPrimitiveManager() { return &m_primitive_manager; } //! Gets the number of children - virtual int getNumChildShapes() const + virtual int getNumChildShapes() const { return m_childShapes.size(); } - //! Use this method for adding children. Only Convex shapes are allowed. - void addChildShape(const btTransform& localTransform,btCollisionShape* shape) + void addChildShape(const btTransform& localTransform, btCollisionShape* shape) { btAssert(shape->isConvex()); m_childTransforms.push_back(localTransform); @@ -444,24 +420,22 @@ public: } //! Retrieves the bound from a child - /*! + /*! */ - virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - - if(childrenHasTransform()) - { - m_childShapes[child_index]->getAabb(t*m_childTransforms[child_index],aabbMin,aabbMax); - } - else - { - m_childShapes[child_index]->getAabb(t,aabbMin,aabbMax); - } - } - + virtual void getChildAabb(int child_index, const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + if (childrenHasTransform()) + { + m_childShapes[child_index]->getAabb(t * m_childTransforms[child_index], aabbMin, aabbMax); + } + else + { + m_childShapes[child_index]->getAabb(t, aabbMin, aabbMax); + } + } //! Gets the children transform - virtual btTransform getChildTransform(int index) const + virtual btTransform getChildTransform(int index) const { btAssert(m_childTransforms.size() == m_childShapes.size()); return m_childTransforms[index]; @@ -471,7 +445,7 @@ public: /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) + virtual void setChildTransform(int index, const btTransform& transform) { btAssert(m_childTransforms.size() == m_childShapes.size()); m_childTransforms[index] = transform; @@ -490,24 +464,24 @@ public: return false; } - - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const { - (void) prim_index; (void) triangle; + (void)prim_index; + (void)triangle; btAssert(0); } - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const { - (void) prim_index; (void) tetrahedron; + (void)prim_index; + (void)tetrahedron; btAssert(0); } - //! Calculates the exact inertia tensor for this shape - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const; - virtual const char* getName()const + virtual const char* getName() const { return "GImpactCompound"; } @@ -516,11 +490,8 @@ public: { return CONST_GIMPACT_COMPOUND_SHAPE; } - }; - - //! This class manages a sub part of a mesh supplied by the btStridingMeshInterface interface. /*! - Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShapePart, then you must call updateBound() after creating the mesh @@ -535,21 +506,21 @@ public: /*! Manages the info from btStridingMeshInterface object and controls the Lock/Unlock mechanism */ - class TrimeshPrimitiveManager:public btPrimitiveManagerBase + class TrimeshPrimitiveManager : public btPrimitiveManagerBase { public: btScalar m_margin; - btStridingMeshInterface * m_meshInterface; + btStridingMeshInterface* m_meshInterface; btVector3 m_scale; int m_part; int m_lock_count; - const unsigned char *vertexbase; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; - int numfaces; + int numfaces; PHY_ScalarType indicestype; TrimeshPrimitiveManager() @@ -557,7 +528,7 @@ public: m_meshInterface = NULL; m_part = 0; m_margin = 0.01f; - m_scale = btVector3(1.f,1.f,1.f); + m_scale = btVector3(1.f, 1.f, 1.f); m_lock_count = 0; vertexbase = 0; numverts = 0; @@ -567,8 +538,8 @@ public: numfaces = 0; } - TrimeshPrimitiveManager(const TrimeshPrimitiveManager & manager) - : btPrimitiveManagerBase() + TrimeshPrimitiveManager(const TrimeshPrimitiveManager& manager) + : btPrimitiveManagerBase() { m_meshInterface = manager.m_meshInterface; m_part = manager.m_part; @@ -581,11 +552,10 @@ public: indexbase = 0; indexstride = 0; numfaces = 0; - } TrimeshPrimitiveManager( - btStridingMeshInterface * meshInterface, int part) + btStridingMeshInterface* meshInterface, int part) { m_meshInterface = meshInterface; m_part = part; @@ -598,29 +568,28 @@ public: indexbase = 0; indexstride = 0; numfaces = 0; - } virtual ~TrimeshPrimitiveManager() {} void lock() { - if(m_lock_count>0) + if (m_lock_count > 0) { m_lock_count++; return; } m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase,numverts, - type, stride,&indexbase, indexstride, numfaces,indicestype,m_part); + &vertexbase, numverts, + type, stride, &indexbase, indexstride, numfaces, indicestype, m_part); m_lock_count = 1; } void unlock() { - if(m_lock_count == 0) return; - if(m_lock_count>1) + if (m_lock_count == 0) return; + if (m_lock_count > 1) { --m_lock_count; return; @@ -637,93 +606,91 @@ public: virtual int get_primitive_count() const { - return (int )numfaces; + return (int)numfaces; } SIMD_FORCE_INLINE int get_vertex_count() const { - return (int )numverts; + return (int)numverts; } - SIMD_FORCE_INLINE void get_indices(int face_index,unsigned int &i0,unsigned int &i1,unsigned int &i2) const + SIMD_FORCE_INLINE void get_indices(int face_index, unsigned int& i0, unsigned int& i1, unsigned int& i2) const { - if(indicestype == PHY_SHORT) + if (indicestype == PHY_SHORT) { - unsigned short* s_indices = (unsigned short *)(indexbase + face_index * indexstride); + unsigned short* s_indices = (unsigned short*)(indexbase + face_index * indexstride); i0 = s_indices[0]; i1 = s_indices[1]; i2 = s_indices[2]; } else { - unsigned int * i_indices = (unsigned int *)(indexbase + face_index*indexstride); + unsigned int* i_indices = (unsigned int*)(indexbase + face_index * indexstride); i0 = i_indices[0]; i1 = i_indices[1]; i2 = i_indices[2]; } } - SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3 & vertex) const + SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3& vertex) const { - if(type == PHY_DOUBLE) + if (type == PHY_DOUBLE) { - double * dvertices = (double *)(vertexbase + vertex_index*stride); - vertex[0] = btScalar(dvertices[0]*m_scale[0]); - vertex[1] = btScalar(dvertices[1]*m_scale[1]); - vertex[2] = btScalar(dvertices[2]*m_scale[2]); + double* dvertices = (double*)(vertexbase + vertex_index * stride); + vertex[0] = btScalar(dvertices[0] * m_scale[0]); + vertex[1] = btScalar(dvertices[1] * m_scale[1]); + vertex[2] = btScalar(dvertices[2] * m_scale[2]); } else { - float * svertices = (float *)(vertexbase + vertex_index*stride); - vertex[0] = svertices[0]*m_scale[0]; - vertex[1] = svertices[1]*m_scale[1]; - vertex[2] = svertices[2]*m_scale[2]; + float* svertices = (float*)(vertexbase + vertex_index * stride); + vertex[0] = svertices[0] * m_scale[0]; + vertex[1] = svertices[1] * m_scale[1]; + vertex[2] = svertices[2] * m_scale[2]; } } - virtual void get_primitive_box(int prim_index ,btAABB & primbox) const + virtual void get_primitive_box(int prim_index, btAABB& primbox) const { - btPrimitiveTriangle triangle; - get_primitive_triangle(prim_index,triangle); + btPrimitiveTriangle triangle; + get_primitive_triangle(prim_index, triangle); primbox.calc_from_triangle_margin( triangle.m_vertices[0], - triangle.m_vertices[1],triangle.m_vertices[2],triangle.m_margin); + triangle.m_vertices[1], triangle.m_vertices[2], triangle.m_margin); } - virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const + virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const { unsigned int indices[3]; - get_indices(prim_index,indices[0],indices[1],indices[2]); - get_vertex(indices[0],triangle.m_vertices[0]); - get_vertex(indices[1],triangle.m_vertices[1]); - get_vertex(indices[2],triangle.m_vertices[2]); + get_indices(prim_index, indices[0], indices[1], indices[2]); + get_vertex(indices[0], triangle.m_vertices[0]); + get_vertex(indices[1], triangle.m_vertices[1]); + get_vertex(indices[2], triangle.m_vertices[2]); triangle.m_margin = m_margin; } - SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index,btTriangleShapeEx & triangle) const + SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index, btTriangleShapeEx& triangle) const { unsigned int indices[3]; - get_indices(prim_index,indices[0],indices[1],indices[2]); - get_vertex(indices[0],triangle.m_vertices1[0]); - get_vertex(indices[1],triangle.m_vertices1[1]); - get_vertex(indices[2],triangle.m_vertices1[2]); + get_indices(prim_index, indices[0], indices[1], indices[2]); + get_vertex(indices[0], triangle.m_vertices1[0]); + get_vertex(indices[1], triangle.m_vertices1[1]); + get_vertex(indices[2], triangle.m_vertices1[2]); triangle.setMargin(m_margin); } - }; - protected: TrimeshPrimitiveManager m_primitive_manager; -public: +public: btGImpactMeshShapePart() { m_box_set.setPrimitiveManager(&m_primitive_manager); } - btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part ); - virtual ~btGImpactMeshShapePart(); + btGImpactMeshShapePart(btStridingMeshInterface* meshInterface, int part); + virtual ~btGImpactMeshShapePart(); //! if true, then its children must get transforms. virtual bool childrenHasTransform() const @@ -731,40 +698,36 @@ public: return false; } - //! call when reading child shapes - virtual void lockChildShapes() const; - virtual void unlockChildShapes() const; + virtual void lockChildShapes() const; + virtual void unlockChildShapes() const; //! Gets the number of children - virtual int getNumChildShapes() const + virtual int getNumChildShapes() const { return m_primitive_manager.get_primitive_count(); } - //! Gets the children virtual btCollisionShape* getChildShape(int index) { - (void) index; + (void)index; btAssert(0); return NULL; } - - //! Gets the child virtual const btCollisionShape* getChildShape(int index) const { - (void) index; + (void)index; btAssert(0); return NULL; } //! Gets the children transform - virtual btTransform getChildTransform(int index) const + virtual btTransform getChildTransform(int index) const { - (void) index; + (void)index; btAssert(0); return btTransform(); } @@ -773,35 +736,27 @@ public: /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) + virtual void setChildTransform(int index, const btTransform& transform) { - (void) index; - (void) transform; + (void)index; + (void)transform; btAssert(0); } - //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const + virtual const btPrimitiveManagerBase* getPrimitiveManager() const { return &m_primitive_manager; } - SIMD_FORCE_INLINE TrimeshPrimitiveManager * getTrimeshPrimitiveManager() + SIMD_FORCE_INLINE TrimeshPrimitiveManager* getTrimeshPrimitiveManager() { return &m_primitive_manager; } + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const; - - - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - - - - virtual const char* getName()const + virtual const char* getName() const { return "GImpactMeshShapePart"; } @@ -823,62 +778,59 @@ public: return false; } - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const { - m_primitive_manager.get_bullet_triangle(prim_index,triangle); + m_primitive_manager.get_bullet_triangle(prim_index, triangle); } - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const { - (void) prim_index; - (void) tetrahedron; + (void)prim_index; + (void)tetrahedron; btAssert(0); } - - SIMD_FORCE_INLINE int getVertexCount() const { return m_primitive_manager.get_vertex_count(); } - SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3 & vertex) const + SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3& vertex) const { - m_primitive_manager.get_vertex(vertex_index,vertex); + m_primitive_manager.get_vertex(vertex_index, vertex); } SIMD_FORCE_INLINE void setMargin(btScalar margin) - { - m_primitive_manager.m_margin = margin; - postUpdate(); - } + { + m_primitive_manager.m_margin = margin; + postUpdate(); + } - SIMD_FORCE_INLINE btScalar getMargin() const - { - return m_primitive_manager.m_margin; - } + SIMD_FORCE_INLINE btScalar getMargin() const + { + return m_primitive_manager.m_margin; + } - virtual void setLocalScaling(const btVector3& scaling) - { - m_primitive_manager.m_scale = scaling; - postUpdate(); - } + virtual void setLocalScaling(const btVector3& scaling) + { + m_primitive_manager.m_scale = scaling; + postUpdate(); + } - virtual const btVector3& getLocalScaling() const - { - return m_primitive_manager.m_scale; - } + virtual const btVector3& getLocalScaling() const + { + return m_primitive_manager.m_scale; + } - SIMD_FORCE_INLINE int getPart() const - { - return (int)m_primitive_manager.m_part; - } + SIMD_FORCE_INLINE int getPart() const + { + return (int)m_primitive_manager.m_part; + } - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - virtual void processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom,const btVector3& rayTo) const; + virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const; + virtual void processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const; }; - //! This class manages a mesh supplied by the btStridingMeshInterface interface. /*! Set of btGImpactMeshShapePart parts @@ -893,29 +845,29 @@ class btGImpactMeshShape : public btGImpactShapeInterface protected: btAlignedObjectArray m_mesh_parts; - void buildMeshParts(btStridingMeshInterface * meshInterface) + void buildMeshParts(btStridingMeshInterface* meshInterface) { - for (int i=0;igetNumSubParts() ;++i ) + for (int i = 0; i < meshInterface->getNumSubParts(); ++i) { - btGImpactMeshShapePart * newpart = new btGImpactMeshShapePart(meshInterface,i); + btGImpactMeshShapePart* newpart = new btGImpactMeshShapePart(meshInterface, i); m_mesh_parts.push_back(newpart); } } //! use this function for perfofm refit in bounding boxes - virtual void calcLocalAABB() - { - m_localAABB.invalidate(); - int i = m_mesh_parts.size(); - while(i--) - { - m_mesh_parts[i]->updateBound(); - m_localAABB.merge(m_mesh_parts[i]->getLocalBox()); - } - } + virtual void calcLocalAABB() + { + m_localAABB.invalidate(); + int i = m_mesh_parts.size(); + while (i--) + { + m_mesh_parts[i]->updateBound(); + m_localAABB.merge(m_mesh_parts[i]->getLocalBox()); + } + } public: - btGImpactMeshShape(btStridingMeshInterface * meshInterface) + btGImpactMeshShape(btStridingMeshInterface* meshInterface) { m_meshInterface = meshInterface; buildMeshParts(meshInterface); @@ -924,15 +876,14 @@ public: virtual ~btGImpactMeshShape() { int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; delete part; - } + } m_mesh_parts.clear(); } - btStridingMeshInterface* getMeshInterface() { return m_meshInterface; @@ -948,79 +899,73 @@ public: return m_mesh_parts.size(); } - btGImpactMeshShapePart * getMeshPart(int index) + btGImpactMeshShapePart* getMeshPart(int index) { return m_mesh_parts[index]; } - - - const btGImpactMeshShapePart * getMeshPart(int index) const + const btGImpactMeshShapePart* getMeshPart(int index) const { return m_mesh_parts[index]; } - - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { localScaling = scaling; int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; part->setLocalScaling(scaling); - } + } m_needs_update = true; } virtual void setMargin(btScalar margin) - { - m_collisionMargin = margin; + { + m_collisionMargin = margin; int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; part->setMargin(margin); - } + } m_needs_update = true; - } + } //! Tells to this object that is needed to refit all the meshes - virtual void postUpdate() - { + virtual void postUpdate() + { int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; part->postUpdate(); - } + } - m_needs_update = true; - } - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + m_needs_update = true; + } + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const; //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const + virtual const btPrimitiveManagerBase* getPrimitiveManager() const { btAssert(0); return NULL; } - //! Gets the number of children - virtual int getNumChildShapes() const + virtual int getNumChildShapes() const { btAssert(0); return 0; } - //! if true, then its children must get transforms. virtual bool childrenHasTransform() const { @@ -1042,15 +987,17 @@ public: return false; } - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const { - (void) prim_index; (void) triangle; + (void)prim_index; + (void)triangle; btAssert(0); } - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const { - (void) prim_index; (void) tetrahedron; + (void)prim_index; + (void)tetrahedron; btAssert(0); } @@ -1065,39 +1012,38 @@ public: btAssert(0); } - - - //! Retrieves the bound from a child - /*! + /*! */ - virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - (void) child_index; (void) t; (void) aabbMin; (void) aabbMax; - btAssert(0); - } + virtual void getChildAabb(int child_index, const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + (void)child_index; + (void)t; + (void)aabbMin; + (void)aabbMax; + btAssert(0); + } //! Gets the children virtual btCollisionShape* getChildShape(int index) { - (void) index; + (void)index; btAssert(0); return NULL; } - //! Gets the child virtual const btCollisionShape* getChildShape(int index) const { - (void) index; + (void)index; btAssert(0); return NULL; } //! Gets the children transform - virtual btTransform getChildTransform(int index) const + virtual btTransform getChildTransform(int index) const { - (void) index; + (void)index; btAssert(0); return btTransform(); } @@ -1106,59 +1052,56 @@ public: /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) + virtual void setChildTransform(int index, const btTransform& transform) { - (void) index; (void) transform; + (void)index; + (void)transform; btAssert(0); } - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const { return CONST_GIMPACT_TRIMESH_SHAPE; } - - virtual const char* getName()const + virtual const char* getName() const { return "GImpactMesh"; } - virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const; + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const; //! Function for retrieve triangles. /*! It gives the triangles in local space */ - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void processAllTrianglesRay (btTriangleCallback* callback,const btVector3& rayFrom,const btVector3& rayTo) const; + virtual void processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const; - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btGImpactMeshShapeData +struct btGImpactMeshShapeData { - btCollisionShapeData m_collisionShapeData; + btCollisionShapeData m_collisionShapeData; btStridingMeshInterfaceData m_meshInterface; - btVector3FloatData m_localScaling; + btVector3FloatData m_localScaling; - float m_collisionMargin; + float m_collisionMargin; - int m_gimpactSubType; + int m_gimpactSubType; }; -SIMD_FORCE_INLINE int btGImpactMeshShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btGImpactMeshShape::calculateSerializeBufferSize() const { return sizeof(btGImpactMeshShapeData); } - -#endif //GIMPACT_MESH_SHAPE_H +#endif //GIMPACT_MESH_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp index 5d07d1adb..bfdb3db5d 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp @@ -20,48 +20,45 @@ subject to the following restrictions: #include "btGenericPoolAllocator.h" - - /// *************** btGenericMemoryPool ******************/////////// size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements) { size_t ptr = BT_UINT_MAX; - if(m_free_nodes_count == 0) return BT_UINT_MAX; + if (m_free_nodes_count == 0) return BT_UINT_MAX; // find an avaliable free node with the correct size size_t revindex = m_free_nodes_count; - while(revindex-- && ptr == BT_UINT_MAX) + while (revindex-- && ptr == BT_UINT_MAX) { - if(m_allocated_sizes[m_free_nodes[revindex]]>=num_elements) + if (m_allocated_sizes[m_free_nodes[revindex]] >= num_elements) { ptr = revindex; } } - if(ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found - + if (ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found revindex = ptr; ptr = m_free_nodes[revindex]; // post: ptr contains the node index, and revindex the index in m_free_nodes - size_t finalsize = m_allocated_sizes[ptr]; + size_t finalsize = m_allocated_sizes[ptr]; finalsize -= num_elements; m_allocated_sizes[ptr] = num_elements; // post: finalsize>=0, m_allocated_sizes[ptr] has the requested size - if(finalsize>0) // preserve free node, there are some free memory + if (finalsize > 0) // preserve free node, there are some free memory { m_free_nodes[revindex] = ptr + num_elements; m_allocated_sizes[ptr + num_elements] = finalsize; } - else // delete free node + else // delete free node { // swap with end - m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count-1]; + m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count - 1]; m_free_nodes_count--; } @@ -70,17 +67,16 @@ size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements) size_t btGenericMemoryPool::allocate_from_pool(size_t num_elements) { - if(m_allocated_count+num_elements>m_max_element_count) return BT_UINT_MAX; + if (m_allocated_count + num_elements > m_max_element_count) return BT_UINT_MAX; size_t ptr = m_allocated_count; m_allocated_sizes[m_allocated_count] = num_elements; - m_allocated_count+=num_elements; + m_allocated_count += num_elements; return ptr; } - void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count) { m_allocated_count = 0; @@ -89,14 +85,11 @@ void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count) m_element_size = element_size; m_max_element_count = element_count; + m_pool = (unsigned char *)btAlignedAlloc(m_element_size * m_max_element_count, 16); + m_free_nodes = (size_t *)btAlignedAlloc(sizeof(size_t) * m_max_element_count, 16); + m_allocated_sizes = (size_t *)btAlignedAlloc(sizeof(size_t) * m_max_element_count, 16); - - - m_pool = (unsigned char *) btAlignedAlloc(m_element_size*m_max_element_count,16); - m_free_nodes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16); - m_allocated_sizes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16); - - for (size_t i = 0;i< m_max_element_count;i++ ) + for (size_t i = 0; i < m_max_element_count; i++) { m_allocated_sizes[i] = 0; } @@ -111,150 +104,141 @@ void btGenericMemoryPool::end_pool() m_free_nodes_count = 0; } - //! Allocates memory in pool /*! \param size_bytes size in bytes of the buffer */ -void * btGenericMemoryPool::allocate(size_t size_bytes) +void *btGenericMemoryPool::allocate(size_t size_bytes) { - - size_t module = size_bytes%m_element_size; - size_t element_count = size_bytes/m_element_size; - if(module>0) element_count++; + size_t module = size_bytes % m_element_size; + size_t element_count = size_bytes / m_element_size; + if (module > 0) element_count++; size_t alloc_pos = allocate_from_free_nodes(element_count); // a free node is found - if(alloc_pos != BT_UINT_MAX) + if (alloc_pos != BT_UINT_MAX) { return get_element_data(alloc_pos); } // allocate directly on pool alloc_pos = allocate_from_pool(element_count); - if(alloc_pos == BT_UINT_MAX) return NULL; // not space + if (alloc_pos == BT_UINT_MAX) return NULL; // not space return get_element_data(alloc_pos); } -bool btGenericMemoryPool::freeMemory(void * pointer) +bool btGenericMemoryPool::freeMemory(void *pointer) { - unsigned char * pointer_pos = (unsigned char *)pointer; - unsigned char * pool_pos = (unsigned char *)m_pool; + unsigned char *pointer_pos = (unsigned char *)pointer; + unsigned char *pool_pos = (unsigned char *)m_pool; // calc offset - if(pointer_pos=get_pool_capacity()) return false;// far away + if (offset >= get_pool_capacity()) return false; // far away // find free position - m_free_nodes[m_free_nodes_count] = offset/m_element_size; + m_free_nodes[m_free_nodes_count] = offset / m_element_size; m_free_nodes_count++; return true; } - /// *******************! btGenericPoolAllocator *******************!/// - btGenericPoolAllocator::~btGenericPoolAllocator() { // destroy pools size_t i; - for (i=0;iend_pool(); btAlignedFree(m_pools[i]); } } - // creates a pool -btGenericMemoryPool * btGenericPoolAllocator::push_new_pool() +btGenericMemoryPool *btGenericPoolAllocator::push_new_pool() { - if(m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL; + if (m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL; - btGenericMemoryPool * newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool),16); + btGenericMemoryPool *newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool), 16); m_pools[m_pool_count] = newptr; - m_pools[m_pool_count]->init_pool(m_pool_element_size,m_pool_element_count); + m_pools[m_pool_count]->init_pool(m_pool_element_size, m_pool_element_count); m_pool_count++; return newptr; } -void * btGenericPoolAllocator::failback_alloc(size_t size_bytes) +void *btGenericPoolAllocator::failback_alloc(size_t size_bytes) { + btGenericMemoryPool *pool = NULL; - btGenericMemoryPool * pool = NULL; - - - if(size_bytes<=get_pool_capacity()) + if (size_bytes <= get_pool_capacity()) { - pool = push_new_pool(); + pool = push_new_pool(); } - if(pool==NULL) // failback + if (pool == NULL) // failback { - return btAlignedAlloc(size_bytes,16); + return btAlignedAlloc(size_bytes, 16); } return pool->allocate(size_bytes); } -bool btGenericPoolAllocator::failback_free(void * pointer) +bool btGenericPoolAllocator::failback_free(void *pointer) { btAlignedFree(pointer); return true; } - //! Allocates memory in pool /*! \param size_bytes size in bytes of the buffer */ -void * btGenericPoolAllocator::allocate(size_t size_bytes) +void *btGenericPoolAllocator::allocate(size_t size_bytes) { - void * ptr = NULL; + void *ptr = NULL; size_t i = 0; - while(iallocate(size_bytes); ++i; } - if(ptr) return ptr; + if (ptr) return ptr; return failback_alloc(size_bytes); } -bool btGenericPoolAllocator::freeMemory(void * pointer) +bool btGenericPoolAllocator::freeMemory(void *pointer) { bool result = false; size_t i = 0; - while(ifreeMemory(pointer); ++i; } - if(result) return true; + if (result) return true; return failback_free(pointer); } /// ************** STANDARD ALLOCATOR ***************************/// - #define BT_DEFAULT_POOL_SIZE 32768 #define BT_DEFAULT_POOL_ELEMENT_SIZE 8 // main allocator -class GIM_STANDARD_ALLOCATOR: public btGenericPoolAllocator +class GIM_STANDARD_ALLOCATOR : public btGenericPoolAllocator { public: - GIM_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE) + GIM_STANDARD_ALLOCATOR() : btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE, BT_DEFAULT_POOL_SIZE) { } }; @@ -262,19 +246,18 @@ public: // global allocator GIM_STANDARD_ALLOCATOR g_main_allocator; - -void * btPoolAlloc(size_t size) +void *btPoolAlloc(size_t size) { return g_main_allocator.allocate(size); } -void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize) +void *btPoolRealloc(void *ptr, size_t oldsize, size_t newsize) { - void * newptr = btPoolAlloc(newsize); - size_t copysize = oldsizemaxval?maxval:number)) +#define BT_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number)) /// Calc a plane from a triangle edge an a normal. plane is a vec4f -SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 & e2, const btVector3 & normal,btVector4 & plane) +SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 &e1, const btVector3 &e2, const btVector3 &normal, btVector4 &plane) { - btVector3 planenormal = (e2-e1).cross(normal); + btVector3 planenormal = (e2 - e1).cross(normal); planenormal.normalize(); - plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal)); + plane.setValue(planenormal[0], planenormal[1], planenormal[2], e2.dot(planenormal)); } - - //***************** SEGMENT and LINE FUNCTIONS **********************************/// /*! Finds the closest point(cp) to (v) on a segment (e1,e2) */ SIMD_FORCE_INLINE void bt_closest_point_on_segment( - btVector3 & cp, const btVector3 & v, - const btVector3 &e1,const btVector3 &e2) + btVector3 &cp, const btVector3 &v, + const btVector3 &e1, const btVector3 &e2) { - btVector3 n = e2-e1; - cp = v - e1; - btScalar _scalar = cp.dot(n)/n.dot(n); - if(_scalar <0.0f) + btVector3 n = e2 - e1; + cp = v - e1; + btScalar _scalar = cp.dot(n) / n.dot(n); + if (_scalar < 0.0f) { - cp = e1; + cp = e1; } - else if(_scalar >1.0f) + else if (_scalar > 1.0f) { - cp = e2; + cp = e2; } else { - cp = _scalar*n + e1; + cp = _scalar * n + e1; } } - //! line plane collision /*! *\return @@ -82,131 +74,125 @@ SIMD_FORCE_INLINE void bt_closest_point_on_segment( */ SIMD_FORCE_INLINE int bt_line_plane_collision( - const btVector4 & plane, - const btVector3 & vDir, - const btVector3 & vPoint, - btVector3 & pout, + const btVector4 &plane, + const btVector3 &vDir, + const btVector3 &vPoint, + btVector3 &pout, btScalar &tparam, btScalar tmin, btScalar tmax) { - btScalar _dotdir = vDir.dot(plane); - if(btFabs(_dotdir)tmax) + else if (tparam > tmax) { returnvalue = 0; tparam = tmax; } - pout = tparam*vDir + vPoint; + pout = tparam * vDir + vPoint; return returnvalue; } - //! Find closest points on segments SIMD_FORCE_INLINE void bt_segment_collision( - const btVector3 & vA1, - const btVector3 & vA2, - const btVector3 & vB1, - const btVector3 & vB2, - btVector3 & vPointA, - btVector3 & vPointB) + const btVector3 &vA1, + const btVector3 &vA2, + const btVector3 &vB1, + const btVector3 &vB2, + btVector3 &vPointA, + btVector3 &vPointB) { - btVector3 AD = vA2 - vA1; - btVector3 BD = vB2 - vB1; - btVector3 N = AD.cross(BD); - btScalar tp = N.length2(); + btVector3 AD = vA2 - vA1; + btVector3 BD = vB2 - vB1; + btVector3 N = AD.cross(BD); + btScalar tp = N.length2(); - btVector4 _M;//plane + btVector4 _M; //plane - if(tp_M[1]) - { - invert_b_order = true; - BT_SWAP_NUMBERS(_M[0],_M[1]); - } - _M[2] = vA1.dot(AD); - _M[3] = vA2.dot(AD); - //mid points - N[0] = (_M[0]+_M[1])*0.5f; - N[1] = (_M[2]+_M[3])*0.5f; + if (_M[0] > _M[1]) + { + invert_b_order = true; + BT_SWAP_NUMBERS(_M[0], _M[1]); + } + _M[2] = vA1.dot(AD); + _M[3] = vA2.dot(AD); + //mid points + N[0] = (_M[0] + _M[1]) * 0.5f; + N[1] = (_M[2] + _M[3]) * 0.5f; - if(N[0]=0.0f) - { - if (_dist>m_penetration_depth) - { - m_penetration_depth = _dist; - point_indices[0] = _k; - m_point_count=1; - } - else if ((_dist+SIMD_EPSILON)>=m_penetration_depth) - { - point_indices[m_point_count] = _k; - m_point_count++; - } - } - } + if (_dist >= 0.0f) + { + if (_dist > m_penetration_depth) + { + m_penetration_depth = _dist; + point_indices[0] = _k; + m_point_count = 1; + } + else if ((_dist + SIMD_EPSILON) >= m_penetration_depth) + { + point_indices[m_point_count] = _k; + m_point_count++; + } + } + } - for ( _k=0;_k0.0f&&dis1>0.0f&&dis2>0.0f) return false; + if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false; - // classify points on this triangle - dis0 = bt_distance_point_plane(other.m_plane,m_vertices[0]) - total_margin; + // classify points on this triangle + dis0 = bt_distance_point_plane(other.m_plane, m_vertices[0]) - total_margin; - dis1 = bt_distance_point_plane(other.m_plane,m_vertices[1]) - total_margin; + dis1 = bt_distance_point_plane(other.m_plane, m_vertices[1]) - total_margin; - dis2 = bt_distance_point_plane(other.m_plane,m_vertices[2]) - total_margin; + dis2 = bt_distance_point_plane(other.m_plane, m_vertices[2]) - total_margin; - if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false; - return true; + return true; } -int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points ) +int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle& other, btVector3* clipped_points) { - // edge 0 + // edge 0 - btVector3 temp_points[MAX_TRI_CLIPPING]; + btVector3 temp_points[MAX_TRI_CLIPPING]; + btVector4 edgeplane; - btVector4 edgeplane; + get_edge_plane(0, edgeplane); - get_edge_plane(0,edgeplane); + int clipped_count = bt_plane_clip_triangle( + edgeplane, other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], temp_points); + if (clipped_count == 0) return 0; - int clipped_count = bt_plane_clip_triangle( - edgeplane,other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],temp_points); + btVector3 temp_points1[MAX_TRI_CLIPPING]; - if (clipped_count == 0) return 0; + // edge 1 + get_edge_plane(1, edgeplane); - btVector3 temp_points1[MAX_TRI_CLIPPING]; + clipped_count = bt_plane_clip_polygon(edgeplane, temp_points, clipped_count, temp_points1); + if (clipped_count == 0) return 0; - // edge 1 - get_edge_plane(1,edgeplane); + // edge 2 + get_edge_plane(2, edgeplane); + clipped_count = bt_plane_clip_polygon( + edgeplane, temp_points1, clipped_count, clipped_points); - clipped_count = bt_plane_clip_polygon(edgeplane,temp_points,clipped_count,temp_points1); - - if (clipped_count == 0) return 0; - - // edge 2 - get_edge_plane(2,edgeplane); - - clipped_count = bt_plane_clip_polygon( - edgeplane,temp_points1,clipped_count,clipped_points); - - return clipped_count; + return clipped_count; } -bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts) +bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_TRIANGLE_CONTACT& contacts) { - btScalar margin = m_margin + other.m_margin; + btScalar margin = m_margin + other.m_margin; - btVector3 clipped_points[MAX_TRI_CLIPPING]; - int clipped_count; - //create planes - // plane v vs U points + btVector3 clipped_points[MAX_TRI_CLIPPING]; + int clipped_count; + //create planes + // plane v vs U points - GIM_TRIANGLE_CONTACT contacts1; + GIM_TRIANGLE_CONTACT contacts1; - contacts1.m_separating_normal = m_plane; + contacts1.m_separating_normal = m_plane; + clipped_count = clip_triangle(other, clipped_points); - clipped_count = clip_triangle(other,clipped_points); + if (clipped_count == 0) + { + return false; //Reject + } - if (clipped_count == 0 ) - { - return false;//Reject - } + //find most deep interval face1 + contacts1.merge_points(contacts1.m_separating_normal, margin, clipped_points, clipped_count); + if (contacts1.m_point_count == 0) return false; // too far + //Normal pointing to this triangle + contacts1.m_separating_normal *= -1.f; - //find most deep interval face1 - contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count); - if (contacts1.m_point_count == 0) return false; // too far - //Normal pointing to this triangle - contacts1.m_separating_normal *= -1.f; + //Clip tri1 by tri2 edges + GIM_TRIANGLE_CONTACT contacts2; + contacts2.m_separating_normal = other.m_plane; + clipped_count = other.clip_triangle(*this, clipped_points); - //Clip tri1 by tri2 edges - GIM_TRIANGLE_CONTACT contacts2; - contacts2.m_separating_normal = other.m_plane; + if (clipped_count == 0) + { + return false; //Reject + } - clipped_count = other.clip_triangle(*this,clipped_points); + //find most deep interval face1 + contacts2.merge_points(contacts2.m_separating_normal, margin, clipped_points, clipped_count); + if (contacts2.m_point_count == 0) return false; // too far - if (clipped_count == 0 ) - { - return false;//Reject - } - - //find most deep interval face1 - contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count); - if (contacts2.m_point_count == 0) return false; // too far - - - - - ////check most dir for contacts - if (contacts2.m_penetration_depth0.0f&&dis1>0.0f&&dis2>0.0f) return false; + if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false; - // classify points on this triangle - dis0 = bt_distance_point_plane(plane1,m_vertices1[0]) - total_margin; + // classify points on this triangle + dis0 = bt_distance_point_plane(plane1, m_vertices1[0]) - total_margin; - dis1 = bt_distance_point_plane(plane1,m_vertices1[1]) - total_margin; + dis1 = bt_distance_point_plane(plane1, m_vertices1[1]) - total_margin; - dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - total_margin; + dis2 = bt_distance_point_plane(plane1, m_vertices1[2]) - total_margin; - if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false; - return true; + return true; } - - diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/btTriangleShapeEx.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/btTriangleShapeEx.h index 973c2ed12..568a1ce81 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/btTriangleShapeEx.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/btTriangleShapeEx.h @@ -21,7 +21,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GIMPACT_TRIANGLE_SHAPE_EX_H #define GIMPACT_TRIANGLE_SHAPE_EX_H @@ -31,16 +30,15 @@ subject to the following restrictions: #include "btClipPolygon.h" #include "btGeometryOperations.h" - #define MAX_TRI_CLIPPING 16 //! Structure for collision struct GIM_TRIANGLE_CONTACT { - btScalar m_penetration_depth; - int m_point_count; - btVector4 m_separating_normal; - btVector3 m_points[MAX_TRI_CLIPPING]; + btScalar m_penetration_depth; + int m_point_count; + btVector4 m_separating_normal; + btVector3 m_points[MAX_TRI_CLIPPING]; SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT& other) { @@ -48,7 +46,7 @@ struct GIM_TRIANGLE_CONTACT m_separating_normal = other.m_separating_normal; m_point_count = other.m_point_count; int i = m_point_count; - while(i--) + while (i--) { m_points[i] = other.m_points[i]; } @@ -63,14 +61,11 @@ struct GIM_TRIANGLE_CONTACT copy_from(other); } - //! classify points that are closer - void merge_points(const btVector4 & plane, - btScalar margin, const btVector3 * points, int point_count); - + //! classify points that are closer + void merge_points(const btVector4& plane, + btScalar margin, const btVector3* points, int point_count); }; - - class btPrimitiveTriangle { public: @@ -78,17 +73,15 @@ public: btVector4 m_plane; btScalar m_margin; btScalar m_dummy; - btPrimitiveTriangle():m_margin(0.01f) + btPrimitiveTriangle() : m_margin(0.01f) { - } - SIMD_FORCE_INLINE void buildTriPlane() { - btVector3 normal = (m_vertices[1]-m_vertices[0]).cross(m_vertices[2]-m_vertices[0]); + btVector3 normal = (m_vertices[1] - m_vertices[0]).cross(m_vertices[2] - m_vertices[0]); normal.normalize(); - m_plane.setValue(normal[0],normal[1],normal[2],m_vertices[0].dot(normal)); + m_plane.setValue(normal[0], normal[1], normal[2], m_vertices[0].dot(normal)); } //! Test if triangles could collide @@ -98,14 +91,14 @@ public: /*! \pre this triangle must have its plane calculated. */ - SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4 &plane) const - { - const btVector3 & e0 = m_vertices[edge_index]; - const btVector3 & e1 = m_vertices[(edge_index+1)%3]; - bt_edge_plane(e0,e1,m_plane,plane); - } + SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4& plane) const + { + const btVector3& e0 = m_vertices[edge_index]; + const btVector3& e1 = m_vertices[(edge_index + 1) % 3]; + bt_edge_plane(e0, e1, m_plane, plane); + } - void applyTransform(const btTransform& t) + void applyTransform(const btTransform& t) { m_vertices[0] = t(m_vertices[0]); m_vertices[1] = t(m_vertices[1]); @@ -117,44 +110,41 @@ public: \pre clipped_points must have MAX_TRI_CLIPPING size, and this triangle must have its plane calculated. \return the number of clipped points */ - int clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points ); + int clip_triangle(btPrimitiveTriangle& other, btVector3* clipped_points); //! Find collision using the clipping method /*! \pre this triangle and other must have their triangles calculated */ - bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts); + bool find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_TRIANGLE_CONTACT& contacts); }; - - //! Helper class for colliding Bullet Triangle Shapes /*! This class implements a better getAabb method than the previous btTriangleShape class */ -class btTriangleShapeEx: public btTriangleShape +class btTriangleShapeEx : public btTriangleShape { public: - - btTriangleShapeEx():btTriangleShape(btVector3(0,0,0),btVector3(0,0,0),btVector3(0,0,0)) + btTriangleShapeEx() : btTriangleShape(btVector3(0, 0, 0), btVector3(0, 0, 0), btVector3(0, 0, 0)) { } - btTriangleShapeEx(const btVector3& p0,const btVector3& p1,const btVector3& p2): btTriangleShape(p0,p1,p2) + btTriangleShapeEx(const btVector3& p0, const btVector3& p1, const btVector3& p2) : btTriangleShape(p0, p1, p2) { } - btTriangleShapeEx(const btTriangleShapeEx & other): btTriangleShape(other.m_vertices1[0],other.m_vertices1[1],other.m_vertices1[2]) + btTriangleShapeEx(const btTriangleShapeEx& other) : btTriangleShape(other.m_vertices1[0], other.m_vertices1[1], other.m_vertices1[2]) { } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { btVector3 tv0 = t(m_vertices1[0]); btVector3 tv1 = t(m_vertices1[1]); btVector3 tv2 = t(m_vertices1[2]); - btAABB trianglebox(tv0,tv1,tv2,m_collisionMargin); + btAABB trianglebox(tv0, tv1, tv2, m_collisionMargin); aabbMin = trianglebox.m_min; aabbMax = trianglebox.m_max; } @@ -166,15 +156,14 @@ public: m_vertices1[2] = t(m_vertices1[2]); } - SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const + SIMD_FORCE_INLINE void buildTriPlane(btVector4& plane) const { - btVector3 normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]); + btVector3 normal = (m_vertices1[1] - m_vertices1[0]).cross(m_vertices1[2] - m_vertices1[0]); normal.normalize(); - plane.setValue(normal[0],normal[1],normal[2],m_vertices1[0].dot(normal)); + plane.setValue(normal[0], normal[1], normal[2], m_vertices1[0].dot(normal)); } bool overlap_test_conservative(const btTriangleShapeEx& other); }; - -#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H +#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_array.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_array.h index cda51a5fc..fc2dc38a3 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_array.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_array.h @@ -34,47 +34,46 @@ email: projectileman@yahoo.com #include "gim_memory.h" - #define GIM_ARRAY_GROW_INCREMENT 2 #define GIM_ARRAY_GROW_FACTOR 2 //! Very simple array container with fast access and simd memory -template +template class gim_array { public: -//! properties -//!@{ - T *m_data; - GUINT m_size; - GUINT m_allocated_size; -//!@} -//! protected operations -//!@{ + //! properties + //!@{ + T* m_data; + GUINT m_size; + GUINT m_allocated_size; + //!@} + //! protected operations + //!@{ - inline void destroyData() + inline void destroyData() { - m_allocated_size = 0; - if(m_data==NULL) return; + m_allocated_size = 0; + if (m_data == NULL) return; gim_free(m_data); m_data = NULL; } inline bool resizeData(GUINT newsize) { - if(newsize==0) + if (newsize == 0) { destroyData(); return true; } - if(m_size>0) + if (m_size > 0) { - m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T)); + m_data = (T*)gim_realloc(m_data, m_size * sizeof(T), newsize * sizeof(T)); } else { - m_data = (T*)gim_alloc(newsize*sizeof(T)); + m_data = (T*)gim_alloc(newsize * sizeof(T)); } m_allocated_size = newsize; return true; @@ -82,243 +81,238 @@ public: inline bool growingCheck() { - if(m_allocated_size<=m_size) + if (m_allocated_size <= m_size) { - GUINT requestsize = m_size; - m_size = m_allocated_size; - if(resizeData((requestsize+GIM_ARRAY_GROW_INCREMENT)*GIM_ARRAY_GROW_FACTOR)==false) return false; + GUINT requestsize = m_size; + m_size = m_allocated_size; + if (resizeData((requestsize + GIM_ARRAY_GROW_INCREMENT) * GIM_ARRAY_GROW_FACTOR) == false) return false; } return true; } -//!@} -//! public operations -//!@{ - inline bool reserve(GUINT size) - { - if(m_allocated_size>=size) return false; - return resizeData(size); - } + //!@} + //! public operations + //!@{ + inline bool reserve(GUINT size) + { + if (m_allocated_size >= size) return false; + return resizeData(size); + } - inline void clear_range(GUINT start_range) - { - while(m_size>start_range) - { - m_data[--m_size].~T(); - } - } + inline void clear_range(GUINT start_range) + { + while (m_size > start_range) + { + m_data[--m_size].~T(); + } + } - inline void clear() - { - if(m_size==0)return; - clear_range(0); - } + inline void clear() + { + if (m_size == 0) return; + clear_range(0); + } - inline void clear_memory() - { - clear(); - destroyData(); - } + inline void clear_memory() + { + clear(); + destroyData(); + } - gim_array() - { - m_data = 0; - m_size = 0; - m_allocated_size = 0; - } + gim_array() + { + m_data = 0; + m_size = 0; + m_allocated_size = 0; + } - gim_array(GUINT reservesize) - { - m_data = 0; - m_size = 0; + gim_array(GUINT reservesize) + { + m_data = 0; + m_size = 0; - m_allocated_size = 0; - reserve(reservesize); - } + m_allocated_size = 0; + reserve(reservesize); + } - ~gim_array() - { - clear_memory(); - } + ~gim_array() + { + clear_memory(); + } - inline GUINT size() const - { - return m_size; - } + inline GUINT size() const + { + return m_size; + } - inline GUINT max_size() const - { - return m_allocated_size; - } + inline GUINT max_size() const + { + return m_allocated_size; + } - inline T & operator[](size_t i) + inline T& operator[](size_t i) { return m_data[i]; } - inline const T & operator[](size_t i) const + inline const T& operator[](size_t i) const { return m_data[i]; } - inline T * pointer(){ return m_data;} - inline const T * pointer() const - { return m_data;} + inline T* pointer() { return m_data; } + inline const T* pointer() const + { + return m_data; + } - - inline T * get_pointer_at(GUINT i) + inline T* get_pointer_at(GUINT i) { return m_data + i; } - inline const T * get_pointer_at(GUINT i) const + inline const T* get_pointer_at(GUINT i) const { return m_data + i; } - inline T & at(GUINT i) + inline T& at(GUINT i) { return m_data[i]; } - inline const T & at(GUINT i) const + inline const T& at(GUINT i) const { return m_data[i]; } - inline T & front() + inline T& front() { return *m_data; } - inline const T & front() const + inline const T& front() const { return *m_data; } - inline T & back() + inline T& back() { - return m_data[m_size-1]; + return m_data[m_size - 1]; } - inline const T & back() const + inline const T& back() const { - return m_data[m_size-1]; + return m_data[m_size - 1]; } - inline void swap(GUINT i, GUINT j) { - gim_swap_elements(m_data,i,j); + gim_swap_elements(m_data, i, j); } - inline void push_back(const T & obj) + inline void push_back(const T& obj) { - this->growingCheck(); - m_data[m_size] = obj; - m_size++; + this->growingCheck(); + m_data[m_size] = obj; + m_size++; } //!Simply increase the m_size, doesn't call the new element constructor inline void push_back_mem() { - this->growingCheck(); - m_size++; + this->growingCheck(); + m_size++; } - inline void push_back_memcpy(const T & obj) + inline void push_back_memcpy(const T& obj) { - this->growingCheck(); - gim_simd_memcpy(&m_data[m_size],&obj,sizeof(T)); - m_size++; + this->growingCheck(); + gim_simd_memcpy(&m_data[m_size], &obj, sizeof(T)); + m_size++; } inline void pop_back() { - m_size--; - m_data[m_size].~T(); + m_size--; + m_data[m_size].~T(); } //!Simply decrease the m_size, doesn't call the deleted element destructor inline void pop_back_mem() { - m_size--; + m_size--; } - //! fast erase + //! fast erase inline void erase(GUINT index) { - if(indexgrowingCheck(); - for(GUINT i = m_size;i>index;i--) - { - gim_simd_memcpy(m_data+i,m_data+i-1,sizeof(T)); - } - m_size++; + this->growingCheck(); + for (GUINT i = m_size; i > index; i--) + { + gim_simd_memcpy(m_data + i, m_data + i - 1, sizeof(T)); + } + m_size++; } - inline void insert(const T & obj,GUINT index) + inline void insert(const T& obj, GUINT index) { - insert_mem(index); - m_data[index] = obj; + insert_mem(index); + m_data[index] = obj; } - inline void resize(GUINT size, bool call_constructor = true, const T& fillData=T()) + inline void resize(GUINT size, bool call_constructor = true, const T& fillData = T()) { - if(size>m_size) - { - reserve(size); - if(call_constructor) - { - while(m_size m_size) + { + reserve(size); + if (call_constructor) + { + while (m_size < size) + { + m_data[m_size] = fillData; + m_size++; + } + } + else + { + m_size = size; + } + } + else if (size < m_size) + { + if (call_constructor) clear_range(size); + m_size = size; + } } inline void refit() { - resizeData(m_size); + resizeData(m_size); } - }; - - - - -#endif // GIM_CONTAINERS_H_INCLUDED +#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h index 0c48cb60f..7ab783672 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h @@ -35,12 +35,8 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_linear_math.h" - - - #ifndef PLANEDIREPSILON #define PLANEDIREPSILON 0.0000001f #endif @@ -49,77 +45,82 @@ email: projectileman@yahoo.com #define PARALELENORMALS 0.000001f #endif -#define TRIANGLE_NORMAL(v1,v2,v3,n)\ -{\ - vec3f _dif1,_dif2;\ - VEC_DIFF(_dif1,v2,v1);\ - VEC_DIFF(_dif2,v3,v1);\ - VEC_CROSS(n,_dif1,_dif2);\ - VEC_NORMALIZE(n);\ -}\ +#define TRIANGLE_NORMAL(v1, v2, v3, n) \ + { \ + vec3f _dif1, _dif2; \ + VEC_DIFF(_dif1, v2, v1); \ + VEC_DIFF(_dif2, v3, v1); \ + VEC_CROSS(n, _dif1, _dif2); \ + VEC_NORMALIZE(n); \ + } -#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\ - vec3f _dif1,_dif2; \ - VEC_DIFF(_dif1,v2,v1); \ - VEC_DIFF(_dif2,v3,v1); \ - VEC_CROSS(n,_dif1,_dif2); \ -}\ +#define TRIANGLE_NORMAL_FAST(v1, v2, v3, n) \ + { \ + vec3f _dif1, _dif2; \ + VEC_DIFF(_dif1, v2, v1); \ + VEC_DIFF(_dif2, v3, v1); \ + VEC_CROSS(n, _dif1, _dif2); \ + } /// plane is a vec4f -#define TRIANGLE_PLANE(v1,v2,v3,plane) {\ - TRIANGLE_NORMAL(v1,v2,v3,plane);\ - plane[3] = VEC_DOT(v1,plane);\ -}\ +#define TRIANGLE_PLANE(v1, v2, v3, plane) \ + { \ + TRIANGLE_NORMAL(v1, v2, v3, plane); \ + plane[3] = VEC_DOT(v1, plane); \ + } /// plane is a vec4f -#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\ - TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\ - plane[3] = VEC_DOT(v1,plane);\ -}\ +#define TRIANGLE_PLANE_FAST(v1, v2, v3, plane) \ + { \ + TRIANGLE_NORMAL_FAST(v1, v2, v3, plane); \ + plane[3] = VEC_DOT(v1, plane); \ + } /// Calc a plane from an edge an a normal. plane is a vec4f -#define EDGE_PLANE(e1,e2,n,plane) {\ - vec3f _dif; \ - VEC_DIFF(_dif,e2,e1); \ - VEC_CROSS(plane,_dif,n); \ - VEC_NORMALIZE(plane); \ - plane[3] = VEC_DOT(e1,plane);\ -}\ +#define EDGE_PLANE(e1, e2, n, plane) \ + { \ + vec3f _dif; \ + VEC_DIFF(_dif, e2, e1); \ + VEC_CROSS(plane, _dif, n); \ + VEC_NORMALIZE(plane); \ + plane[3] = VEC_DOT(e1, plane); \ + } -#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3]) +#define DISTANCE_PLANE_POINT(plane, point) (VEC_DOT(plane, point) - plane[3]) -#define PROJECT_POINT_PLANE(point,plane,projected) {\ - GREAL _dis;\ - _dis = DISTANCE_PLANE_POINT(plane,point);\ - VEC_SCALE(projected,-_dis,plane);\ - VEC_SUM(projected,projected,point); \ -}\ +#define PROJECT_POINT_PLANE(point, plane, projected) \ + { \ + GREAL _dis; \ + _dis = DISTANCE_PLANE_POINT(plane, point); \ + VEC_SCALE(projected, -_dis, plane); \ + VEC_SUM(projected, projected, point); \ + } //! Verifies if a point is in the plane hull -template +template SIMD_FORCE_INLINE bool POINT_IN_HULL( - const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count) + const CLASS_POINT &point, const CLASS_PLANE *planes, GUINT plane_count) { GREAL _dis; - for (GUINT _i = 0;_i< plane_count;++_i) + for (GUINT _i = 0; _i < plane_count; ++_i) { - _dis = DISTANCE_PLANE_POINT(planes[_i],point); - if(_dis>0.0f) return false; + _dis = DISTANCE_PLANE_POINT(planes[_i], point); + if (_dis > 0.0f) return false; } return true; } -template +template SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT( - const CLASS_POINT& s1, - const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped) + const CLASS_POINT &s1, + const CLASS_POINT &s2, const CLASS_PLANE &plane, CLASS_POINT &clipped) { - GREAL _dis1,_dis2; - _dis1 = DISTANCE_PLANE_POINT(plane,s1); - VEC_DIFF(clipped,s2,s1); - _dis2 = VEC_DOT(clipped,plane); - VEC_SCALE(clipped,-_dis1/_dis2,clipped); - VEC_SUM(clipped,clipped,s1); + GREAL _dis1, _dis2; + _dis1 = DISTANCE_PLANE_POINT(plane, s1); + VEC_DIFF(clipped, s2, s1); + _dis2 = VEC_DOT(clipped, plane); + VEC_SCALE(clipped, -_dis1 / _dis2, clipped); + VEC_SUM(clipped, clipped, s1); } enum ePLANE_INTERSECTION_TYPE @@ -152,30 +153,30 @@ intersection type must have the following values */ -template +template SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2( - const CLASS_POINT& s1, + const CLASS_POINT &s1, const CLASS_POINT &s2, - const CLASS_PLANE &plane,CLASS_POINT &clipped) + const CLASS_PLANE &plane, CLASS_POINT &clipped) { - GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1); - GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2); - if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON) + GREAL _dis1 = DISTANCE_PLANE_POINT(plane, s1); + GREAL _dis2 = DISTANCE_PLANE_POINT(plane, s2); + if (_dis1 > -G_EPSILON && _dis2 > -G_EPSILON) { - if(_dis1<_dis2) return G_FRONT_PLANE_S1; - return G_FRONT_PLANE_S2; + if (_dis1 < _dis2) return G_FRONT_PLANE_S1; + return G_FRONT_PLANE_S2; } - else if(_dis1 _dis2) return G_BACK_PLANE_S1; - return G_BACK_PLANE_S2; + if (_dis1 > _dis2) return G_BACK_PLANE_S1; + return G_BACK_PLANE_S2; } - VEC_DIFF(clipped,s2,s1); - _dis2 = VEC_DOT(clipped,plane); - VEC_SCALE(clipped,-_dis1/_dis2,clipped); - VEC_SUM(clipped,clipped,s1); - if(_dis1<_dis2) return G_COLLIDE_PLANE_S1; + VEC_DIFF(clipped, s2, s1); + _dis2 = VEC_DOT(clipped, plane); + VEC_SCALE(clipped, -_dis1 / _dis2, clipped); + VEC_SUM(clipped, clipped, s1); + if (_dis1 < _dis2) return G_COLLIDE_PLANE_S1; return G_COLLIDE_PLANE_S2; } @@ -194,43 +195,42 @@ intersection_type must have the following values
  • 5 : Segment collides plane, s2 in back */ -template +template SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST( - const CLASS_POINT& s1, + const CLASS_POINT &s1, const CLASS_POINT &s2, const CLASS_PLANE &plane, - CLASS_POINT &clipped1,CLASS_POINT &clipped2) + CLASS_POINT &clipped1, CLASS_POINT &clipped2) { - eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1); - switch(intersection_type) + eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1, s2, plane, clipped1); + switch (intersection_type) { - case G_FRONT_PLANE_S1: - VEC_COPY(clipped1,s1); - VEC_COPY(clipped2,s2); - break; - case G_FRONT_PLANE_S2: - VEC_COPY(clipped1,s2); - VEC_COPY(clipped2,s1); - break; - case G_BACK_PLANE_S1: - VEC_COPY(clipped1,s1); - VEC_COPY(clipped2,s2); - break; - case G_BACK_PLANE_S2: - VEC_COPY(clipped1,s2); - VEC_COPY(clipped2,s1); - break; - case G_COLLIDE_PLANE_S1: - VEC_COPY(clipped2,s1); - break; - case G_COLLIDE_PLANE_S2: - VEC_COPY(clipped2,s2); - break; + case G_FRONT_PLANE_S1: + VEC_COPY(clipped1, s1); + VEC_COPY(clipped2, s2); + break; + case G_FRONT_PLANE_S2: + VEC_COPY(clipped1, s2); + VEC_COPY(clipped2, s1); + break; + case G_BACK_PLANE_S1: + VEC_COPY(clipped1, s1); + VEC_COPY(clipped2, s2); + break; + case G_BACK_PLANE_S2: + VEC_COPY(clipped1, s2); + VEC_COPY(clipped2, s1); + break; + case G_COLLIDE_PLANE_S1: + VEC_COPY(clipped2, s1); + break; + case G_COLLIDE_PLANE_S2: + VEC_COPY(clipped2, s2); + break; } return intersection_type; } - //! Finds the 2 smallest cartesian coordinates of a plane normal #define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1) @@ -239,23 +239,23 @@ SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST( Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).
    It uses the PLANEDIREPSILON constant. */ -template +template SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION( - const CLASS_PLANE & plane, - const CLASS_POINT & vDir, - const CLASS_POINT & vPoint, - CLASS_POINT & pout,T &tparam) + const CLASS_PLANE &plane, + const CLASS_POINT &vDir, + const CLASS_POINT &vPoint, + CLASS_POINT &pout, T &tparam) { - GREAL _dis,_dotdir; - _dotdir = VEC_DOT(plane,vDir); - if(_dotdir +template SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION( - const CLASS_PLANE & plane, - const CLASS_POINT & vDir, - const CLASS_POINT & vPoint, - CLASS_POINT & pout, + const CLASS_PLANE &plane, + const CLASS_POINT &vDir, + const CLASS_POINT &vPoint, + CLASS_POINT &pout, T &tparam, T tmin, T tmax) { - GREAL _dis,_dotdir; - _dotdir = VEC_DOT(plane,vDir); - if(btFabs(_dotdir)tmax) + else if (tparam > tmax) { returnvalue = 0; tparam = tmax; } - VEC_SCALE(pout,tparam,vDir); - VEC_SUM(pout,vPoint,pout); + VEC_SCALE(pout, tparam, vDir); + VEC_SUM(pout, vPoint, pout); return returnvalue; } @@ -312,24 +312,24 @@ SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION( \return true if the planes intersect, 0 if paralell. */ -template +template SIMD_FORCE_INLINE bool INTERSECT_PLANES( - const CLASS_PLANE &p1, - const CLASS_PLANE &p2, - CLASS_POINT &p, - CLASS_POINT &d) + const CLASS_PLANE &p1, + const CLASS_PLANE &p2, + CLASS_POINT &p, + CLASS_POINT &d) { - VEC_CROSS(d,p1,p2); - GREAL denom = VEC_DOT(d, d); - if(GIM_IS_ZERO(denom)) return false; + VEC_CROSS(d, p1, p2); + GREAL denom = VEC_DOT(d, d); + if (GIM_IS_ZERO(denom)) return false; vec3f _n; - _n[0]=p1[3]*p2[0] - p2[3]*p1[0]; - _n[1]=p1[3]*p2[1] - p2[3]*p1[1]; - _n[2]=p1[3]*p2[2] - p2[3]*p1[2]; - VEC_CROSS(p,_n,d); - p[0]/=denom; - p[1]/=denom; - p[2]/=denom; + _n[0] = p1[3] * p2[0] - p2[3] * p1[0]; + _n[1] = p1[3] * p2[1] - p2[3] * p1[1]; + _n[2] = p1[3] * p2[2] - p2[3] * p1[2]; + VEC_CROSS(p, _n, d); + p[0] /= denom; + p[1] /= denom; + p[2] /= denom; return true; } @@ -337,32 +337,31 @@ SIMD_FORCE_INLINE bool INTERSECT_PLANES( /*! Finds the closest point(cp) to (v) on a segment (e1,e2) */ -template +template SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT( - CLASS_POINT & cp, const CLASS_POINT & v, - const CLASS_POINT &e1,const CLASS_POINT &e2) + CLASS_POINT &cp, const CLASS_POINT &v, + const CLASS_POINT &e1, const CLASS_POINT &e2) { - vec3f _n; - VEC_DIFF(_n,e2,e1); - VEC_DIFF(cp,v,e1); + vec3f _n; + VEC_DIFF(_n, e2, e1); + VEC_DIFF(cp, v, e1); GREAL _scalar = VEC_DOT(cp, _n); - _scalar/= VEC_DOT(_n, _n); - if(_scalar <0.0f) + _scalar /= VEC_DOT(_n, _n); + if (_scalar < 0.0f) { - VEC_COPY(cp,e1); + VEC_COPY(cp, e1); } - else if(_scalar >1.0f) + else if (_scalar > 1.0f) { - VEC_COPY(cp,e2); + VEC_COPY(cp, e2); } else { - VEC_SCALE(cp,_scalar,_n); - VEC_SUM(cp,cp,e1); + VEC_SCALE(cp, _scalar, _n); + VEC_SUM(cp, cp, e1); } } - /*! \brief Finds the line params where these lines intersect. \param dir1 Direction of line 1 @@ -374,118 +373,114 @@ SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT( \param dointersect 0 if the lines won't intersect, else 1 */ -template +template SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS( - const CLASS_POINT & dir1, - CLASS_POINT & point1, - const CLASS_POINT & dir2, - CLASS_POINT & point2, - T& t1,T& t2) + const CLASS_POINT &dir1, + CLASS_POINT &point1, + const CLASS_POINT &dir2, + CLASS_POINT &point2, + T &t1, T &t2) { - GREAL det; - GREAL e1e1 = VEC_DOT(dir1,dir1); - GREAL e1e2 = VEC_DOT(dir1,dir2); - GREAL e2e2 = VEC_DOT(dir2,dir2); + GREAL det; + GREAL e1e1 = VEC_DOT(dir1, dir1); + GREAL e1e2 = VEC_DOT(dir1, dir2); + GREAL e2e2 = VEC_DOT(dir2, dir2); vec3f p1p2; - VEC_DIFF(p1p2,point1,point2); - GREAL p1p2e1 = VEC_DOT(p1p2,dir1); - GREAL p1p2e2 = VEC_DOT(p1p2,dir2); - det = e1e2*e1e2 - e1e1*e2e2; - if(GIM_IS_ZERO(det)) return false; - t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det; - t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det; + VEC_DIFF(p1p2, point1, point2); + GREAL p1p2e1 = VEC_DOT(p1p2, dir1); + GREAL p1p2e2 = VEC_DOT(p1p2, dir2); + det = e1e2 * e1e2 - e1e1 * e2e2; + if (GIM_IS_ZERO(det)) return false; + t1 = (e1e2 * p1p2e2 - e2e2 * p1p2e1) / det; + t2 = (e1e1 * p1p2e2 - e1e2 * p1p2e1) / det; return true; } //! Find closest points on segments -template +template SIMD_FORCE_INLINE void SEGMENT_COLLISION( - const CLASS_POINT & vA1, - const CLASS_POINT & vA2, - const CLASS_POINT & vB1, - const CLASS_POINT & vB2, - CLASS_POINT & vPointA, - CLASS_POINT & vPointB) + const CLASS_POINT &vA1, + const CLASS_POINT &vA2, + const CLASS_POINT &vB1, + const CLASS_POINT &vB2, + CLASS_POINT &vPointA, + CLASS_POINT &vPointB) { - CLASS_POINT _AD,_BD,n; - vec4f _M;//plane - VEC_DIFF(_AD,vA2,vA1); - VEC_DIFF(_BD,vB2,vB1); - VEC_CROSS(n,_AD,_BD); - GREAL _tp = VEC_DOT(n,n); - if(_tp_M[1]) - { - invert_b_order = true; - GIM_SWAP_NUMBERS(_M[0],_M[1]); - } - _M[2] = VEC_DOT(vA1,_AD); - _M[3] = VEC_DOT(vA2,_AD); - //mid points - n[0] = (_M[0]+_M[1])*0.5f; - n[1] = (_M[2]+_M[3])*0.5f; + CLASS_POINT _AD, _BD, n; + vec4f _M; //plane + VEC_DIFF(_AD, vA2, vA1); + VEC_DIFF(_BD, vB2, vB1); + VEC_CROSS(n, _AD, _BD); + GREAL _tp = VEC_DOT(n, n); + if (_tp < G_EPSILON) //ARE PARALELE + { + //project B over A + bool invert_b_order = false; + _M[0] = VEC_DOT(vB1, _AD); + _M[1] = VEC_DOT(vB2, _AD); + if (_M[0] > _M[1]) + { + invert_b_order = true; + GIM_SWAP_NUMBERS(_M[0], _M[1]); + } + _M[2] = VEC_DOT(vA1, _AD); + _M[3] = VEC_DOT(vA2, _AD); + //mid points + n[0] = (_M[0] + _M[1]) * 0.5f; + n[1] = (_M[2] + _M[3]) * 0.5f; - if(n[0] -SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast) +template +SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir, T bmin, T bmax, T &tfirst, T &tlast) { - if(GIM_IS_ZERO(dir)) + if (GIM_IS_ZERO(dir)) { - return !(pos < bmin || pos > bmax); + return !(pos < bmin || pos > bmax); } GREAL a0 = (bmin - pos) / dir; GREAL a1 = (bmax - pos) / dir; - if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1); + if (a0 > a1) GIM_SWAP_NUMBERS(a0, a1); tfirst = GIM_MAX(a0, tfirst); tlast = GIM_MIN(a1, tlast); if (tlast < tfirst) return false; return true; } - //! Sorts 3 componets -template +template SIMD_FORCE_INLINE void SORT_3_INDICES( - const T * values, - GUINT * order_indices) + const T *values, + GUINT *order_indices) { //get minimum order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2); //get second and third - GUINT i0 = (order_indices[0] + 1)%3; - GUINT i1 = (i0 + 1)%3; + GUINT i0 = (order_indices[0] + 1) % 3; + GUINT i1 = (i0 + 1) % 3; - if(values[i0] < values[i1]) + if (values[i0] < values[i1]) { order_indices[1] = i0; order_indices[2] = i1; @@ -539,8 +533,4 @@ SIMD_FORCE_INLINE void SORT_3_INDICES( } } - - - - -#endif // GIM_VECTOR_H_INCLUDED +#endif // GIM_VECTOR_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_bitset.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_bitset.h index 7dee48a4c..c1fb41a5c 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_bitset.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_bitset.h @@ -34,34 +34,32 @@ email: projectileman@yahoo.com #include "gim_array.h" - #define GUINT_BIT_COUNT 32 #define GUINT_EXPONENT 5 class gim_bitset { public: - gim_array m_container; + gim_array m_container; - gim_bitset() - { + gim_bitset() + { + } - } + gim_bitset(GUINT bits_count) + { + resize(bits_count); + } - gim_bitset(GUINT bits_count) - { - resize(bits_count); - } - - ~gim_bitset() - { - } + ~gim_bitset() + { + } inline bool resize(GUINT newsize) { GUINT oldsize = m_container.size(); - m_container.resize(newsize/GUINT_BIT_COUNT + 1,false); - while(oldsize=size()) + if (bit_index >= size()) { resize(bit_index); } - m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT-1))); + m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT - 1))); } ///Return 0 or 1 inline char get(GUINT bit_index) { - if(bit_index>=size()) + if (bit_index >= size()) { return 0; } char value = m_container[bit_index >> GUINT_EXPONENT] & - (1 << (bit_index & (GUINT_BIT_COUNT-1))); + (1 << (bit_index & (GUINT_BIT_COUNT - 1))); return value; } inline void clear(GUINT bit_index) { - m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT-1))); + m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT - 1))); } }; - - - - -#endif // GIM_CONTAINERS_H_INCLUDED +#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_collision.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_collision.h index a051b4fdb..9f7cbe732 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_collision.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_collision.h @@ -35,8 +35,6 @@ email: projectileman@yahoo.com #include "gim_basic_geometry_operations.h" #include "LinearMath/btTransform.h" - - //SIMD_FORCE_INLINE bool test_cross_edge_box( // const btVector3 & edge, // const btVector3 & absolute_edge, @@ -99,52 +97,50 @@ email: projectileman@yahoo.com #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];\ - const btScalar dir1 = edge[i_dir_1];\ - btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\ - btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\ - if(pmin>pmax)\ - {\ - GIM_SWAP_NUMBERS(pmin,pmax); \ - }\ - const btScalar abs_dir0 = absolute_edge[i_dir_0];\ - const btScalar abs_dir1 = absolute_edge[i_dir_1];\ - const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\ - if(pmin>rad || -rad>pmax) return false;\ -}\ +#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]; \ + const btScalar dir1 = edge[i_dir_1]; \ + btScalar pmin = pointa[i_comp_0] * dir0 + pointa[i_comp_1] * dir1; \ + btScalar pmax = pointb[i_comp_0] * dir0 + pointb[i_comp_1] * dir1; \ + if (pmin > pmax) \ + { \ + GIM_SWAP_NUMBERS(pmin, pmax); \ + } \ + const btScalar abs_dir0 = absolute_edge[i_dir_0]; \ + const btScalar abs_dir1 = absolute_edge[i_dir_1]; \ + const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1; \ + if (pmin > rad || -rad > pmax) return false; \ + } #endif -#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\ -}\ - -#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\ -}\ - -#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\ -}\ +#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \ + { \ + TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 2, 1, 1, 2); \ + } +#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \ + { \ + TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 0, 2, 2, 0); \ + } +#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \ + { \ + TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 1, 0, 0, 1); \ + } //! Class for transforming a model1 to the space of model0 class GIM_BOX_BOX_TRANSFORM_CACHE { public: - btVector3 m_T1to0;//!< Transforms translation of model1 to model 0 - btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal to R0' * R1 - btMatrix3x3 m_AR;//!< Absolute value of m_R1to0 + btVector3 m_T1to0; //!< Transforms translation of model1 to model 0 + btMatrix3x3 m_R1to0; //!< Transforms Rotation of model1 to model 0, equal to R0' * R1 + btMatrix3x3 m_AR; //!< Absolute value of m_R1to0 SIMD_FORCE_INLINE void calc_absolute_matrix() { - static const btVector3 vepsi(1e-6f,1e-6f,1e-6f); + static const btVector3 vepsi(1e-6f, 1e-6f, 1e-6f); m_AR[0] = vepsi + m_R1to0[0].absolute(); m_AR[1] = vepsi + m_R1to0[1].absolute(); m_AR[2] = vepsi + m_R1to0[2].absolute(); @@ -154,42 +150,40 @@ public: { } - - GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0) + GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0) { - COPY_MATRIX_3X3(m_R1to0,trans1_to_0) - MAT_GET_TRANSLATION(trans1_to_0,m_T1to0) + COPY_MATRIX_3X3(m_R1to0, trans1_to_0) + MAT_GET_TRANSLATION(trans1_to_0, m_T1to0) calc_absolute_matrix(); } //! Calc the transformation relative 1 to 0. Inverts matrics by transposing - SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1) + SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform &trans0, const btTransform &trans1) { - m_R1to0 = trans0.getBasis().transpose(); m_T1to0 = m_R1to0 * (-trans0.getOrigin()); - m_T1to0 += m_R1to0*trans1.getOrigin(); + m_T1to0 += m_R1to0 * trans1.getOrigin(); m_R1to0 *= trans1.getBasis(); calc_absolute_matrix(); } //! Calcs the full invertion of the matrices. Useful for scaling matrices - SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1) + SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform &trans0, const btTransform &trans1) { m_R1to0 = trans0.getBasis().inverse(); m_T1to0 = m_R1to0 * (-trans0.getOrigin()); - m_T1to0 += m_R1to0*trans1.getOrigin(); + m_T1to0 += m_R1to0 * trans1.getOrigin(); m_R1to0 *= trans1.getBasis(); calc_absolute_matrix(); } - SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) + SIMD_FORCE_INLINE btVector3 transform(const btVector3 &point) { - return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0; + return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0; } }; @@ -205,34 +199,34 @@ public: btVector3 m_max; GIM_AABB() - {} - - - GIM_AABB(const btVector3 & V1, - const btVector3 & V2, - const btVector3 & V3) { - m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); - - m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); } - GIM_AABB(const btVector3 & V1, - const btVector3 & V2, - const btVector3 & V3, + GIM_AABB(const btVector3 &V1, + const btVector3 &V2, + const btVector3 &V3) + { + m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]); + + m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]); + } + + GIM_AABB(const btVector3 &V1, + const btVector3 &V2, + const btVector3 &V3, GREAL margin) { - m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]); m_min[0] -= margin; m_min[1] -= margin; @@ -242,13 +236,11 @@ public: m_max[2] += margin; } - GIM_AABB(const GIM_AABB &other): - m_min(other.m_min),m_max(other.m_max) + GIM_AABB(const GIM_AABB &other) : m_min(other.m_min), m_max(other.m_max) { } - GIM_AABB(const GIM_AABB &other,btScalar margin ): - m_min(other.m_min),m_max(other.m_max) + GIM_AABB(const GIM_AABB &other, btScalar margin) : m_min(other.m_min), m_max(other.m_max) { m_min[0] -= margin; m_min[1] -= margin; @@ -289,34 +281,34 @@ public: m_max[2] = other.m_max[2] + margin; } - template + template SIMD_FORCE_INLINE void calc_from_triangle( - const CLASS_POINT & V1, - const CLASS_POINT & V2, - const CLASS_POINT & V3) + const CLASS_POINT &V1, + const CLASS_POINT &V2, + const CLASS_POINT &V3) { - m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]); } - template + template SIMD_FORCE_INLINE void calc_from_triangle_margin( - const CLASS_POINT & V1, - const CLASS_POINT & V2, - const CLASS_POINT & V3, btScalar margin) + const CLASS_POINT &V1, + const CLASS_POINT &V2, + const CLASS_POINT &V3, btScalar margin) { - m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]); m_min[0] -= margin; m_min[1] -= margin; @@ -327,74 +319,73 @@ public: } //! Apply a transform to an AABB - SIMD_FORCE_INLINE void appy_transform(const btTransform & trans) + SIMD_FORCE_INLINE void appy_transform(const btTransform &trans) { - btVector3 center = (m_max+m_min)*0.5f; + btVector3 center = (m_max + m_min) * 0.5f; btVector3 extends = m_max - center; // Compute new center center = trans(center); - btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), - trans.getBasis().getRow(1).absolute(), - trans.getBasis().getRow(2).absolute()); - + btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), + trans.getBasis().getRow(1).absolute(), + trans.getBasis().getRow(2).absolute()); + m_min = center - textends; m_max = center + textends; } //! Merges a Box - SIMD_FORCE_INLINE void merge(const GIM_AABB & box) + SIMD_FORCE_INLINE void merge(const GIM_AABB &box) { - m_min[0] = GIM_MIN(m_min[0],box.m_min[0]); - m_min[1] = GIM_MIN(m_min[1],box.m_min[1]); - m_min[2] = GIM_MIN(m_min[2],box.m_min[2]); + m_min[0] = GIM_MIN(m_min[0], box.m_min[0]); + m_min[1] = GIM_MIN(m_min[1], box.m_min[1]); + m_min[2] = GIM_MIN(m_min[2], box.m_min[2]); - m_max[0] = GIM_MAX(m_max[0],box.m_max[0]); - m_max[1] = GIM_MAX(m_max[1],box.m_max[1]); - m_max[2] = GIM_MAX(m_max[2],box.m_max[2]); + m_max[0] = GIM_MAX(m_max[0], box.m_max[0]); + m_max[1] = GIM_MAX(m_max[1], box.m_max[1]); + m_max[2] = GIM_MAX(m_max[2], box.m_max[2]); } //! Merges a point - template - SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point) + template + SIMD_FORCE_INLINE void merge_point(const CLASS_POINT &point) { - m_min[0] = GIM_MIN(m_min[0],point[0]); - m_min[1] = GIM_MIN(m_min[1],point[1]); - m_min[2] = GIM_MIN(m_min[2],point[2]); + m_min[0] = GIM_MIN(m_min[0], point[0]); + m_min[1] = GIM_MIN(m_min[1], point[1]); + m_min[2] = GIM_MIN(m_min[2], point[2]); - m_max[0] = GIM_MAX(m_max[0],point[0]); - m_max[1] = GIM_MAX(m_max[1],point[1]); - m_max[2] = GIM_MAX(m_max[2],point[2]); + m_max[0] = GIM_MAX(m_max[0], point[0]); + m_max[1] = GIM_MAX(m_max[1], point[1]); + m_max[2] = GIM_MAX(m_max[2], point[2]); } //! Gets the extend and center - SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const + SIMD_FORCE_INLINE void get_center_extend(btVector3 ¢er, btVector3 &extend) const { - center = (m_max+m_min)*0.5f; + center = (m_max + m_min) * 0.5f; extend = m_max - center; } //! Finds the intersecting box between this box and the other. - SIMD_FORCE_INLINE void find_intersection(const GIM_AABB & other, GIM_AABB & intersection) const + SIMD_FORCE_INLINE void find_intersection(const GIM_AABB &other, GIM_AABB &intersection) const { - intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]); - intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]); - intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]); + intersection.m_min[0] = GIM_MAX(other.m_min[0], m_min[0]); + intersection.m_min[1] = GIM_MAX(other.m_min[1], m_min[1]); + intersection.m_min[2] = GIM_MAX(other.m_min[2], m_min[2]); - intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]); - intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]); - intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]); + intersection.m_max[0] = GIM_MIN(other.m_max[0], m_max[0]); + intersection.m_max[1] = GIM_MIN(other.m_max[1], m_max[1]); + intersection.m_max[2] = GIM_MIN(other.m_max[2], m_max[2]); } - - SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const + SIMD_FORCE_INLINE bool has_collision(const GIM_AABB &other) const { - if(m_min[0] > other.m_max[0] || - m_max[0] < other.m_min[0] || - m_min[1] > other.m_max[1] || - m_max[1] < other.m_min[1] || - m_min[2] > other.m_max[2] || - m_max[2] < other.m_min[2]) + if (m_min[0] > other.m_max[0] || + m_max[0] < other.m_min[0] || + m_min[1] > other.m_max[1] || + m_max[1] < other.m_min[1] || + m_min[2] > other.m_max[2] || + m_max[2] < other.m_min[2]) { return false; } @@ -406,35 +397,34 @@ public: \param vorigin A vec3f with the origin of the ray \param vdir A vec3f with the direction of the ray */ - SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir) + SIMD_FORCE_INLINE bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) { - btVector3 extents,center; - this->get_center_extend(center,extents);; + btVector3 extents, center; + this->get_center_extend(center, extents); + ; btScalar Dx = vorigin[0] - center[0]; - if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false; + if (GIM_GREATER(Dx, extents[0]) && Dx * vdir[0] >= 0.0f) return false; btScalar Dy = vorigin[1] - center[1]; - if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false; + if (GIM_GREATER(Dy, extents[1]) && Dy * vdir[1] >= 0.0f) return false; btScalar Dz = vorigin[2] - center[2]; - if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false; - + if (GIM_GREATER(Dz, extents[2]) && Dz * vdir[2] >= 0.0f) return false; btScalar f = vdir[1] * Dz - vdir[2] * Dy; - if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false; + if (btFabs(f) > extents[1] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[1])) return false; f = vdir[2] * Dx - vdir[0] * Dz; - if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false; + if (btFabs(f) > extents[0] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[0])) return false; f = vdir[0] * Dy - vdir[1] * Dx; - if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false; + if (btFabs(f) > extents[0] * btFabs(vdir[1]) + extents[1] * btFabs(vdir[0])) return false; return true; } - - SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const + SIMD_FORCE_INLINE void projection_interval(const btVector3 &direction, btScalar &vmin, btScalar &vmax) const { - btVector3 center = (m_max+m_min)*0.5f; - btVector3 extend = m_max-center; + btVector3 center = (m_max + m_min) * 0.5f; + btVector3 extend = m_max - center; - btScalar _fOrigin = direction.dot(center); + btScalar _fOrigin = direction.dot(center); btScalar _fMaximumExtent = extend.dot(direction.absolute()); vmin = _fOrigin - _fMaximumExtent; vmax = _fOrigin + _fMaximumExtent; @@ -442,22 +432,22 @@ public: SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const { - btScalar _fmin,_fmax; - this->projection_interval(plane,_fmin,_fmax); + btScalar _fmin, _fmax; + this->projection_interval(plane, _fmin, _fmax); - if(plane[3] > _fmax + BOX_PLANE_EPSILON) + if (plane[3] > _fmax + BOX_PLANE_EPSILON) { - return G_BACK_PLANE; // 0 + return G_BACK_PLANE; // 0 } - if(plane[3]+BOX_PLANE_EPSILON >=_fmin) + if (plane[3] + BOX_PLANE_EPSILON >= _fmin) { - return G_COLLIDE_PLANE; //1 + return G_COLLIDE_PLANE; //1 } - return G_FRONT_PLANE;//2 + return G_FRONT_PLANE; //2 } - SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0) + SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB &box, btTransform &trans1_to_0) { GIM_AABB tbox = box; tbox.appy_transform(trans1_to_0); @@ -466,52 +456,50 @@ public: //! transcache is the transformation cache from box to this AABB SIMD_FORCE_INLINE bool overlapping_trans_cache( - const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) + const GIM_AABB &box, const GIM_BOX_BOX_TRANSFORM_CACHE &transcache, bool fulltest) { - //Taken from OPCODE - btVector3 ea,eb;//extends - btVector3 ca,cb;//extends - get_center_extend(ca,ea); - box.get_center_extend(cb,eb); - + btVector3 ea, eb; //extends + btVector3 ca, cb; //extends + get_center_extend(ca, ea); + box.get_center_extend(cb, eb); btVector3 T; - btScalar t,t2; + btScalar t, t2; int i; // Class I : A's basis vectors - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i]; + T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i]; t = transcache.m_AR[i].dot(eb) + ea[i]; - if(GIM_GREATER(T[i], t)) return false; + if (GIM_GREATER(T[i], t)) return false; } // Class II : B's basis vectors - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - t = MAT_DOT_COL(transcache.m_R1to0,T,i); - t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i]; - if(GIM_GREATER(t,t2)) return false; + t = MAT_DOT_COL(transcache.m_R1to0, T, i); + t2 = MAT_DOT_COL(transcache.m_AR, ea, i) + eb[i]; + if (GIM_GREATER(t, t2)) return false; } // Class III : 9 cross products - if(fulltest) + if (fulltest) { - int j,m,n,o,p,q,r; - for(i=0;i<3;i++) + int j, m, n, o, p, q, r; + for (i = 0; i < 3; i++) { - m = (i+1)%3; - n = (i+2)%3; - o = i==0?1:0; - p = i==2?1:2; - for(j=0;j<3;j++) + m = (i + 1) % 3; + n = (i + 2) % 3; + o = i == 0 ? 1 : 0; + p = i == 2 ? 1 : 2; + for (j = 0; j < 3; j++) { - q = j==2?1:2; - r = j==0?1:0; - t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j]; - t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] + - eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r]; - if(GIM_GREATER(t,t2)) return false; + q = j == 2 ? 1 : 2; + r = j == 0 ? 1 : 0; + t = T[n] * transcache.m_R1to0[m][j] - T[m] * transcache.m_R1to0[n][j]; + t2 = ea[o] * transcache.m_AR[p][j] + ea[p] * transcache.m_AR[o][j] + + eb[r] * transcache.m_AR[i][q] + eb[q] * transcache.m_AR[i][r]; + if (GIM_GREATER(t, t2)) return false; } } } @@ -520,7 +508,7 @@ public: //! Simple test for planes. SIMD_FORCE_INLINE bool collide_plane( - const btVector4 & plane) + const btVector4 &plane) { ePLANE_INTERSECTION_TYPE classify = plane_classify(plane); return (classify == G_COLLIDE_PLANE); @@ -528,15 +516,15 @@ public: //! test for a triangle, with edges SIMD_FORCE_INLINE bool collide_triangle_exact( - const btVector3 & p1, - const btVector3 & p2, - const btVector3 & p3, - const btVector4 & triangle_plane) + const btVector3 &p1, + const btVector3 &p2, + const btVector3 &p3, + const btVector4 &triangle_plane) { - if(!collide_plane(triangle_plane)) return false; + if (!collide_plane(triangle_plane)) return false; - btVector3 center,extends; - this->get_center_extend(center,extends); + btVector3 center, extends; + this->get_center_extend(center, extends); const btVector3 v1(p1 - center); const btVector3 v2(p2 - center); @@ -546,30 +534,29 @@ public: btVector3 diff(v2 - v1); btVector3 abs_diff = diff.absolute(); //Test With X axis - TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends); + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v1, v3, extends); //Test With Y axis - TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends); + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v1, v3, extends); //Test With Z axis - TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends); - + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v1, v3, extends); diff = v3 - v2; abs_diff = diff.absolute(); //Test With X axis - TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends); + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v2, v1, extends); //Test With Y axis - TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends); + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v2, v1, extends); //Test With Z axis - TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends); + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v2, v1, extends); diff = v1 - v3; abs_diff = diff.absolute(); //Test With X axis - TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends); + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v3, v2, extends); //Test With Y axis - TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends); + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v3, v2, extends); //Test With Z axis - TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends); + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v3, v2, extends); return true; } @@ -577,17 +564,15 @@ public: #ifndef BT_BOX_COLLISION_H_INCLUDED //! Compairison of transformation objects -SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) +SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform &t1, const btTransform &t2) { - if(!(t1.getOrigin() == t2.getOrigin()) ) return false; + if (!(t1.getOrigin() == t2.getOrigin())) return false; - if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false; - if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false; - if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false; + if (!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0))) return false; + if (!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1))) return false; + if (!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2))) return false; return true; } #endif - - -#endif // GIM_BOX_COLLISION_H_INCLUDED +#endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_set.cpp b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_set.cpp index 0c3d7ba8d..0c7a6b7fc 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_set.cpp +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_set.cpp @@ -28,67 +28,64 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_box_set.h" - GUINT GIM_BOX_TREE::_calc_splitting_axis( - gim_array & primitive_boxes, GUINT startIndex, GUINT endIndex) + gim_array& primitive_boxes, GUINT startIndex, GUINT endIndex) { GUINT i; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); - GUINT numIndices = endIndex-startIndex; + btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.)); + GUINT numIndices = endIndex - startIndex; - for (i=startIndex;i & primitive_boxes, GUINT startIndex, + gim_array& primitive_boxes, GUINT startIndex, GUINT endIndex, GUINT splitAxis) { GUINT i; - GUINT splitIndex =startIndex; + GUINT splitIndex = startIndex; GUINT numIndices = endIndex - startIndex; // average of centers btScalar splitValue = 0.0f; - for (i=startIndex;i splitValue) { //swap - primitive_boxes.swap(i,splitIndex); + primitive_boxes.swap(i, splitIndex); splitIndex++; } } @@ -102,28 +99,27 @@ GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index( //bool unbalanced2 = true; //this should be safe too: - GUINT rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + GUINT rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); + btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); return splitIndex; } - -void GIM_BOX_TREE::_build_sub_tree(gim_array & primitive_boxes, GUINT startIndex, GUINT endIndex) +void GIM_BOX_TREE::_build_sub_tree(gim_array& primitive_boxes, GUINT startIndex, GUINT endIndex) { GUINT current_index = m_num_nodes++; - btAssert((endIndex-startIndex)>0); + btAssert((endIndex - startIndex) > 0); - if((endIndex-startIndex) == 1) //we got a leaf - { + if ((endIndex - startIndex) == 1) //we got a leaf + { m_node_array[current_index].m_left = 0; m_node_array[current_index].m_right = 0; m_node_array[current_index].m_escapeIndex = 0; @@ -138,8 +134,8 @@ void GIM_BOX_TREE::_build_sub_tree(gim_array & primitive_boxes, G GUINT splitIndex; //calc this node bounding box - m_node_array[current_index].m_bound.invalidate(); - for (splitIndex=startIndex;splitIndex & primitive_boxes, G //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. //split axis - splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); + splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex); splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex,splitIndex); + primitive_boxes, startIndex, endIndex, splitIndex); //configure this inner node : the left node index m_node_array[current_index].m_left = m_num_nodes; //build left child tree - _build_sub_tree(primitive_boxes, startIndex, splitIndex ); + _build_sub_tree(primitive_boxes, startIndex, splitIndex); //configure this inner node : the right node index m_node_array[current_index].m_right = m_num_nodes; //build right child tree - _build_sub_tree(primitive_boxes, splitIndex ,endIndex); + _build_sub_tree(primitive_boxes, splitIndex, endIndex); //configure this inner node : the escape index - m_node_array[current_index].m_escapeIndex = m_num_nodes - current_index; + m_node_array[current_index].m_escapeIndex = m_num_nodes - current_index; } //! stackless build tree void GIM_BOX_TREE::build_tree( - gim_array & primitive_boxes) + gim_array& primitive_boxes) { // initialize node count to 0 m_num_nodes = 0; // allocate nodes - m_node_array.resize(primitive_boxes.size()*2); - + m_node_array.resize(primitive_boxes.size() * 2); + _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); } - - diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_set.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_set.h index 61d190a7d..afc591dac 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_set.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_set.h @@ -33,54 +33,30 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_array.h" #include "gim_radixsort.h" #include "gim_box_collision.h" #include "gim_tri_collision.h" - - - -//! Overlapping pair -struct GIM_PAIR -{ - GUINT m_index1; - GUINT m_index2; - GIM_PAIR() - {} - - GIM_PAIR(const GIM_PAIR & p) - { - m_index1 = p.m_index1; - m_index2 = p.m_index2; - } - - GIM_PAIR(GUINT index1, GUINT index2) - { - m_index1 = index1; - m_index2 = index2; - } -}; +#include "gim_pair.h" //! A pairset array -class gim_pair_set: public gim_array +class gim_pair_set : public gim_array { public: - gim_pair_set():gim_array(32) + gim_pair_set() : gim_array(32) { } - inline void push_pair(GUINT index1,GUINT index2) + inline void push_pair(GUINT index1, GUINT index2) { - push_back(GIM_PAIR(index1,index2)); + push_back(GIM_PAIR(index1, index2)); } - inline void push_pair_inv(GUINT index1,GUINT index2) + inline void push_pair_inv(GUINT index1, GUINT index2) { - push_back(GIM_PAIR(index2,index1)); + push_back(GIM_PAIR(index2, index1)); } }; - //! Prototype Base class for primitive classification /*! This class is a wrapper for primitive collections. @@ -90,16 +66,14 @@ This class can manage Compound shapes and trimeshes, and if it is managing trime class GIM_PRIMITIVE_MANAGER_PROTOTYPE { public: - virtual ~GIM_PRIMITIVE_MANAGER_PROTOTYPE() {} //! determines if this manager consist on only triangles, which special case will be optimized virtual bool is_trimesh() = 0; virtual GUINT get_primitive_count() = 0; - virtual void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) = 0; - virtual void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) = 0; + virtual void get_primitive_box(GUINT prim_index, GIM_AABB& primbox) = 0; + virtual void get_primitive_triangle(GUINT prim_index, GIM_TRIANGLE& triangle) = 0; }; - struct GIM_AABB_DATA { GIM_AABB m_bound; @@ -110,22 +84,22 @@ struct GIM_AABB_DATA struct GIM_BOX_TREE_NODE { GIM_AABB m_bound; - GUINT m_left;//!< Left subtree - GUINT m_right;//!< Right subtree - GUINT m_escapeIndex;//!< Scape index for traversing - GUINT m_data;//!< primitive index if apply + GUINT m_left; //!< Left subtree + GUINT m_right; //!< Right subtree + GUINT m_escapeIndex; //!< Scape index for traversing + GUINT m_data; //!< primitive index if apply GIM_BOX_TREE_NODE() { - m_left = 0; - m_right = 0; - m_escapeIndex = 0; - m_data = 0; + m_left = 0; + m_right = 0; + m_escapeIndex = 0; + m_data = 0; } SIMD_FORCE_INLINE bool is_leaf_node() const { - return (!m_left && !m_right); + return (!m_left && !m_right); } }; @@ -135,14 +109,16 @@ class GIM_BOX_TREE protected: GUINT m_num_nodes; gim_array m_node_array; + protected: GUINT _sort_and_calc_splitting_index( - gim_array & primitive_boxes, - GUINT startIndex, GUINT endIndex, GUINT splitAxis); + gim_array& primitive_boxes, + GUINT startIndex, GUINT endIndex, GUINT splitAxis); - GUINT _calc_splitting_axis(gim_array & primitive_boxes, GUINT startIndex, GUINT endIndex); + GUINT _calc_splitting_axis(gim_array& primitive_boxes, GUINT startIndex, GUINT endIndex); + + void _build_sub_tree(gim_array& primitive_boxes, GUINT startIndex, GUINT endIndex); - void _build_sub_tree(gim_array & primitive_boxes, GUINT startIndex, GUINT endIndex); public: GIM_BOX_TREE() { @@ -151,7 +127,7 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(gim_array & primitive_boxes); + void build_tree(gim_array& primitive_boxes); SIMD_FORCE_INLINE void clearNodes() { @@ -176,22 +152,22 @@ public: return m_node_array[nodeindex].m_data; } - SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const { bound = m_node_array[nodeindex].m_bound; } - SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound) + SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound) { m_node_array[nodeindex].m_bound = bound; } - SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const + SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const { return m_node_array[nodeindex].m_left; } - SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const + SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const { return m_node_array[nodeindex].m_right; } @@ -204,78 +180,78 @@ public: //!@} }; - //! Generic Box Tree Template /*! This class offers an structure for managing a box tree of primitives. Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and a Box tree structure ( like GIM_BOX_TREE). */ -template +template class GIM_BOX_TREE_TEMPLATE_SET { protected: _GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager; _GIM_BOX_TREE_PROTOTYPE m_box_tree; + protected: //stackless refit SIMD_FORCE_INLINE void refit() { GUINT nodecount = getNodeCount(); - while(nodecount--) + while (nodecount--) { - if(isLeafNode(nodecount)) + if (isLeafNode(nodecount)) { GIM_AABB leafbox; - m_primitive_manager.get_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); + m_primitive_manager.get_primitive_box(getNodeData(nodecount), leafbox); + setNodeBound(nodecount, leafbox); } else { //get left bound GUINT childindex = getLeftNodeIndex(nodecount); GIM_AABB bound; - getNodeBound(childindex,bound); + getNodeBound(childindex, bound); //get right bound childindex = getRightNodeIndex(nodecount); GIM_AABB bound2; - getNodeBound(childindex,bound2); + getNodeBound(childindex, bound2); bound.merge(bound2); - setNodeBound(nodecount,bound); + setNodeBound(nodecount, bound); } } } -public: +public: GIM_BOX_TREE_TEMPLATE_SET() { } - SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const + SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const { GIM_AABB totalbox; getNodeBound(0, totalbox); return totalbox; } - SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & primitive_manager) + SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& primitive_manager) { m_primitive_manager = primitive_manager; } - const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const + const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() const { return m_primitive_manager; } - _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() + _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() { return m_primitive_manager; } -//! node manager prototype functions -///@{ + //! node manager prototype functions + ///@{ //! this attemps to refit the box set. SIMD_FORCE_INLINE void update() @@ -288,19 +264,19 @@ public: { //obtain primitive boxes gim_array primitive_boxes; - primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false); + primitive_boxes.resize(m_primitive_manager.get_primitive_count(), false); - for (GUINT i = 0;i & collided_results) const + SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB& box, gim_array& collided_results) const { GUINT curIndex = 0; GUINT numNodes = getNodeCount(); @@ -308,7 +284,7 @@ public: while (curIndex < numNodes) { GIM_AABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data @@ -328,26 +304,26 @@ public: else { //skip node - curIndex+= getScapeNodeIndex(curIndex); + curIndex += getScapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } //! returns the indices of the primitives in the m_primitive_manager - SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB & box, - const btTransform & transform, gim_array & collided_results) const + SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB& box, + const btTransform& transform, gim_array& collided_results) const { - GIM_AABB transbox=box; + GIM_AABB transbox = box; transbox.appy_transform(transform); - return boxQuery(transbox,collided_results); + return boxQuery(transbox, collided_results); } //! returns the indices of the primitives in the m_primitive_manager SIMD_FORCE_INLINE bool rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - gim_array & collided_results) const + const btVector3& ray_dir, const btVector3& ray_origin, + gim_array& collided_results) const { GUINT curIndex = 0; GUINT numNodes = getNodeCount(); @@ -355,16 +331,16 @@ public: while (curIndex < numNodes) { GIM_AABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) { - collided_results.push_back(getNodeData( curIndex)); + collided_results.push_back(getNodeData(curIndex)); } if (aabbOverlap || isleafnode) @@ -375,10 +351,10 @@ public: else { //skip node - curIndex+= getScapeNodeIndex(curIndex); + curIndex += getScapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } @@ -389,7 +365,7 @@ public: } //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const + SIMD_FORCE_INLINE bool isTrimesh() const { return m_primitive_manager.is_trimesh(); } @@ -411,12 +387,12 @@ public: return m_box_tree.getNodeData(nodeindex); } - SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const { m_box_tree.getNodeBound(nodeindex, bound); } - SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound) + SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound) { m_box_tree.setNodeBound(nodeindex, bound); } @@ -436,36 +412,30 @@ public: return m_box_tree.getScapeNodeIndex(nodeindex); } - SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const + SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex, GIM_TRIANGLE& triangle) const { - m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle); + m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex), triangle); } - }; //! Class for Box Tree Sets /*! this has the GIM_BOX_TREE implementation for bounding boxes. */ -template -class GIM_BOX_TREE_SET: public GIM_BOX_TREE_TEMPLATE_SET< _GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE> +template +class GIM_BOX_TREE_SET : public GIM_BOX_TREE_TEMPLATE_SET<_GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE> { public: - }; - - - - /// GIM_BOX_SET collision methods -template +template class GIM_TREE_TREE_COLLIDER { public: - gim_pair_set * m_collision_pairs; - BOX_SET_CLASS0 * m_boxset0; - BOX_SET_CLASS1 * m_boxset1; + gim_pair_set* m_collision_pairs; + BOX_SET_CLASS0* m_boxset0; + BOX_SET_CLASS1* m_boxset1; GUINT current_node0; GUINT current_node1; bool node0_is_leaf; @@ -483,18 +453,18 @@ public: GIM_TRIANGLE m_tri1; btVector4 m_tri1_plane; - public: GIM_TREE_TREE_COLLIDER() { current_node0 = G_UINT_INFINITY; current_node1 = G_UINT_INFINITY; } + protected: SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0) { - if(node0_has_triangle) return; - m_boxset0->getNodeTriangle(node0,m_tri0); + if (node0_has_triangle) return; + m_boxset0->getNodeTriangle(node0, m_tri0); //transform triangle m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]); m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]); @@ -506,8 +476,8 @@ protected: SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1) { - if(node1_has_triangle) return; - m_boxset1->getNodeTriangle(node1,m_tri1); + if (node1_has_triangle) return; + m_boxset1->getNodeTriangle(node1, m_tri1); //transform triangle m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]); m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]); @@ -519,8 +489,8 @@ protected: SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0) { - if(node0 == current_node0) return; - m_boxset0->getNodeBound(node0,m_box0); + if (node0 == current_node0) return; + m_boxset0->getNodeBound(node0, m_box0); node0_is_leaf = m_boxset0->isLeafNode(node0); node0_has_triangle = false; current_node0 = node0; @@ -528,21 +498,21 @@ protected: SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1) { - if(node1 == current_node1) return; - m_boxset1->getNodeBound(node1,m_box1); + if (node1 == current_node1) return; + m_boxset1->getNodeBound(node1, m_box1); node1_is_leaf = m_boxset1->isLeafNode(node1); node1_has_triangle = false; current_node1 = node1; } - SIMD_FORCE_INLINE bool node_collision(GUINT node0 ,GUINT node1) + SIMD_FORCE_INLINE bool node_collision(GUINT node0, GUINT node1) { retrieve_node0_info(node0); retrieve_node1_info(node1); - bool result = m_box0.overlapping_trans_cache(m_box1,trans_cache_1to0,true); - if(!result) return false; + bool result = m_box0.overlapping_trans_cache(m_box1, trans_cache_1to0, true); + if (!result) return false; - if(t0_is_trimesh && node0_is_leaf) + if (t0_is_trimesh && node0_is_leaf) { //perform primitive vs box collision retrieve_node0_triangle(node0); @@ -550,14 +520,14 @@ protected: m_box1.increment_margin(m_tri0.m_margin); result = m_box1.collide_triangle_exact( - m_tri0.m_vertices[0],m_tri0.m_vertices[1],m_tri0.m_vertices[2],m_tri0_plane); + m_tri0.m_vertices[0], m_tri0.m_vertices[1], m_tri0.m_vertices[2], m_tri0_plane); m_box1.increment_margin(-m_tri0.m_margin); - if(!result) return false; + if (!result) return false; return true; } - else if(t1_is_trimesh && node1_is_leaf) + else if (t1_is_trimesh && node1_is_leaf) { //perform primitive vs box collision retrieve_node1_triangle(node1); @@ -565,11 +535,11 @@ protected: m_box0.increment_margin(m_tri1.m_margin); result = m_box0.collide_triangle_exact( - m_tri1.m_vertices[0],m_tri1.m_vertices[1],m_tri1.m_vertices[2],m_tri1_plane); + m_tri1.m_vertices[0], m_tri1.m_vertices[1], m_tri1.m_vertices[2], m_tri1_plane); m_box0.increment_margin(-m_tri1.m_margin); - if(!result) return false; + if (!result) return false; return true; } return true; @@ -582,40 +552,39 @@ protected: stack_collisions.reserve(32); //add the first pair - stack_collisions.push_pair(0,0); + stack_collisions.push_pair(0, 0); - - while(stack_collisions.size()) + while (stack_collisions.size()) { //retrieve the last pair and pop GUINT node0 = stack_collisions.back().m_index1; GUINT node1 = stack_collisions.back().m_index2; stack_collisions.pop_back(); - if(node_collision(node0,node1)) // a collision is found + if (node_collision(node0, node1)) // a collision is found { - if(node0_is_leaf) + if (node0_is_leaf) { - if(node1_is_leaf) + if (node1_is_leaf) { - m_collision_pairs->push_pair(m_boxset0->getNodeData(node0),m_boxset1->getNodeData(node1)); + m_collision_pairs->push_pair(m_boxset0->getNodeData(node0), m_boxset1->getNodeData(node1)); } else { //collide left - stack_collisions.push_pair(node0,m_boxset1->getLeftNodeIndex(node1)); + stack_collisions.push_pair(node0, m_boxset1->getLeftNodeIndex(node1)); //collide right - stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1)); + stack_collisions.push_pair(node0, m_boxset1->getRightNodeIndex(node1)); } } else { - if(node1_is_leaf) + if (node1_is_leaf) { //collide left - stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1); + stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0), node1); //collide right - stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1); + stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0), node1); } else { @@ -624,36 +593,36 @@ protected: GUINT left1 = m_boxset1->getLeftNodeIndex(node1); GUINT right1 = m_boxset1->getRightNodeIndex(node1); //collide left - stack_collisions.push_pair(left0,left1); + stack_collisions.push_pair(left0, left1); //collide right - stack_collisions.push_pair(left0,right1); + stack_collisions.push_pair(left0, right1); //collide left - stack_collisions.push_pair(right0,left1); + stack_collisions.push_pair(right0, left1); //collide right - stack_collisions.push_pair(right0,right1); + stack_collisions.push_pair(right0, right1); - }// else if node1 is not a leaf - }// else if node0 is not a leaf + } // else if node1 is not a leaf + } // else if node0 is not a leaf - }// if(node_collision(node0,node1)) - }//while(stack_collisions.size()) + } // if(node_collision(node0,node1)) + } //while(stack_collisions.size()) } + public: - void find_collision(BOX_SET_CLASS0 * boxset1, const btTransform & trans1, - BOX_SET_CLASS1 * boxset2, const btTransform & trans2, - gim_pair_set & collision_pairs, bool complete_primitive_tests = true) + void find_collision(BOX_SET_CLASS0* boxset1, const btTransform& trans1, + BOX_SET_CLASS1* boxset2, const btTransform& trans2, + gim_pair_set& collision_pairs, bool complete_primitive_tests = true) { m_collision_pairs = &collision_pairs; m_boxset0 = boxset1; m_boxset1 = boxset2; - trans_cache_1to0.calc_from_homogenic(trans1,trans2); + trans_cache_1to0.calc_from_homogenic(trans1, trans2); - trans_cache_0to1 = trans2.inverse(); + trans_cache_0to1 = trans2.inverse(); trans_cache_0to1 *= trans1; - - if(complete_primitive_tests) + if (complete_primitive_tests) { t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh(); t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh(); @@ -668,7 +637,4 @@ public: } }; - -#endif // GIM_BOXPRUNING_H_INCLUDED - - +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_clip_polygon.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_clip_polygon.h index e342459ce..57b9c5c91 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_clip_polygon.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_clip_polygon.h @@ -33,91 +33,86 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - //! This function calcs the distance from a 3D plane class DISTANCE_PLANE_3D_FUNC { public: - template - inline GREAL operator()(const CLASS_PLANE & plane, const CLASS_POINT & point) + template + inline GREAL operator()(const CLASS_PLANE& plane, const CLASS_POINT& point) { return DISTANCE_PLANE_POINT(plane, point); } }; - - -template +template SIMD_FORCE_INLINE void PLANE_CLIP_POLYGON_COLLECT( - const CLASS_POINT & point0, - const CLASS_POINT & point1, - GREAL dist0, - GREAL dist1, - CLASS_POINT * clipped, - GUINT & clipped_count) + const CLASS_POINT& point0, + const CLASS_POINT& point1, + GREAL dist0, + GREAL dist1, + CLASS_POINT* clipped, + GUINT& clipped_count) { - GUINT _prevclassif = (dist0>G_EPSILON); - GUINT _classif = (dist1>G_EPSILON); - if(_classif!=_prevclassif) + GUINT _prevclassif = (dist0 > G_EPSILON); + GUINT _classif = (dist1 > G_EPSILON); + if (_classif != _prevclassif) { - GREAL blendfactor = -dist0/(dist1-dist0); - VEC_BLEND(clipped[clipped_count],point0,point1,blendfactor); + GREAL blendfactor = -dist0 / (dist1 - dist0); + VEC_BLEND(clipped[clipped_count], point0, point1, blendfactor); clipped_count++; } - if(!_classif) + if (!_classif) { - VEC_COPY(clipped[clipped_count],point1); + VEC_COPY(clipped[clipped_count], point1); clipped_count++; } } - //! Clips a polygon by a plane /*! *\return The count of the clipped counts */ -template +template SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC( - const CLASS_PLANE & plane, - const CLASS_POINT * polygon_points, - GUINT polygon_point_count, - CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func) + const CLASS_PLANE& plane, + const CLASS_POINT* polygon_points, + GUINT polygon_point_count, + CLASS_POINT* clipped, DISTANCE_PLANE_FUNC distance_func) { - GUINT clipped_count = 0; + GUINT clipped_count = 0; - - //clip first point - GREAL firstdist = distance_func(plane,polygon_points[0]);; - if(!(firstdist>G_EPSILON)) + //clip first point + GREAL firstdist = distance_func(plane, polygon_points[0]); + ; + if (!(firstdist > G_EPSILON)) { - VEC_COPY(clipped[clipped_count],polygon_points[0]); + VEC_COPY(clipped[clipped_count], polygon_points[0]); clipped_count++; } GREAL olddist = firstdist; - for(GUINT _i=1;_i +template SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE_GENERIC( - const CLASS_PLANE & plane, - const CLASS_POINT & point0, - const CLASS_POINT & point1, - const CLASS_POINT & point2, - CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func) + const CLASS_PLANE& plane, + const CLASS_POINT& point0, + const CLASS_POINT& point1, + const CLASS_POINT& point2, + CLASS_POINT* clipped, DISTANCE_PLANE_FUNC distance_func) { - GUINT clipped_count = 0; + GUINT clipped_count = 0; - //clip first point - GREAL firstdist = distance_func(plane,point0);; - if(!(firstdist>G_EPSILON)) + //clip first point + GREAL firstdist = distance_func(plane, point0); + ; + if (!(firstdist > G_EPSILON)) { - VEC_COPY(clipped[clipped_count],point0); + VEC_COPY(clipped[clipped_count], point0); clipped_count++; } // point 1 GREAL olddist = firstdist; - GREAL dist = distance_func(plane,point1); + GREAL dist = distance_func(plane, point1); PLANE_CLIP_POLYGON_COLLECT( - point0,point1, - olddist, - dist, - clipped, - clipped_count); + point0, point1, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - // point 2 - dist = distance_func(plane,point2); + dist = distance_func(plane, point2); PLANE_CLIP_POLYGON_COLLECT( - point1,point2, - olddist, - dist, - clipped, - clipped_count); + point1, point2, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - - //RETURN TO FIRST point PLANE_CLIP_POLYGON_COLLECT( - point2,point0, - olddist, - firstdist, - clipped, - clipped_count); + point2, point0, + olddist, + firstdist, + clipped, + clipped_count); return clipped_count; } - -template +template SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON3D( - const CLASS_PLANE & plane, - const CLASS_POINT * polygon_points, - GUINT polygon_point_count, - CLASS_POINT * clipped) + const CLASS_PLANE& plane, + const CLASS_POINT* polygon_points, + GUINT polygon_point_count, + CLASS_POINT* clipped) { - return PLANE_CLIP_POLYGON_GENERIC(plane,polygon_points,polygon_point_count,clipped,DISTANCE_PLANE_3D_FUNC()); + return PLANE_CLIP_POLYGON_GENERIC(plane, polygon_points, polygon_point_count, clipped, DISTANCE_PLANE_3D_FUNC()); } - -template +template SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D( - const CLASS_PLANE & plane, - const CLASS_POINT & point0, - const CLASS_POINT & point1, - const CLASS_POINT & point2, - CLASS_POINT * clipped) + const CLASS_PLANE& plane, + const CLASS_POINT& point0, + const CLASS_POINT& point1, + const CLASS_POINT& point2, + CLASS_POINT* clipped) { - return PLANE_CLIP_TRIANGLE_GENERIC(plane,point0,point1,point2,clipped,DISTANCE_PLANE_3D_FUNC()); + return PLANE_CLIP_TRIANGLE_GENERIC(plane, point0, point1, point2, clipped, DISTANCE_PLANE_3D_FUNC()); } - - -#endif // GIM_TRI_COLLISION_H_INCLUDED +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_contact.cpp b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_contact.cpp index 20e41de08..390225709 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_contact.cpp +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_contact.cpp @@ -33,91 +33,89 @@ email: projectileman@yahoo.com #define MAX_COINCIDENT 8 void gim_contact_array::merge_contacts( - const gim_contact_array & contacts, bool normal_contact_average) + const gim_contact_array& contacts, bool normal_contact_average) { clear(); - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts.back()); return; } gim_array keycontacts(contacts.size()); - keycontacts.resize(contacts.size(),false); + keycontacts.resize(contacts.size(), false); //fill key contacts GUINT i; - for (i = 0;im_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) + if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth) //) { *pcontact = *scontact; - coincident_count = 0; + coincident_count = 0; } - else if(normal_contact_average) + else if (normal_contact_average) { - if(btFabs(pcontact->m_depth - scontact->m_depth)m_normal; - coincident_count++; - } - } + if (btFabs(pcontact->m_depth - scontact->m_depth) < CONTACT_DIFF_EPSILON) + { + if (coincident_count < MAX_COINCIDENT) + { + coincident_normals[coincident_count] = scontact->m_normal; + coincident_count++; + } + } } } else - {//add new contact + { //add new contact - if(normal_contact_average && coincident_count>0) - { - pcontact->interpolate_normals(coincident_normals,coincident_count); - coincident_count = 0; - } + if (normal_contact_average && coincident_count > 0) + { + pcontact->interpolate_normals(coincident_normals, coincident_count); + coincident_count = 0; + } - push_back(*scontact); - pcontact = &back(); - } + push_back(*scontact); + pcontact = &back(); + } last_key = key; } } -void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts) +void gim_contact_array::merge_contacts_unique(const gim_contact_array& contacts) { clear(); - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts.back()); return; @@ -125,14 +123,14 @@ void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts GIM_CONTACT average_contact = contacts.back(); - for (GUINT i=1;i +class gim_contact_array : public gim_array { public: - gim_contact_array():gim_array(64) + gim_contact_array() : gim_array(64) { } - SIMD_FORCE_INLINE void push_contact(const btVector3 &point,const btVector3 & normal, - GREAL depth, GUINT feature1, GUINT feature2) + SIMD_FORCE_INLINE void push_contact(const btVector3 &point, const btVector3 &normal, + GREAL depth, GUINT feature1, GUINT feature2) { push_back_mem(); - GIM_CONTACT & newele = back(); + GIM_CONTACT &newele = back(); newele.m_point = point; newele.m_normal = normal; newele.m_depth = depth; @@ -150,13 +146,13 @@ public: } SIMD_FORCE_INLINE void push_triangle_contacts( - const GIM_TRIANGLE_CONTACT_DATA & tricontact, - GUINT feature1,GUINT feature2) + const GIM_TRIANGLE_CONTACT_DATA &tricontact, + GUINT feature1, GUINT feature2) { - for(GUINT i = 0;i +template struct GIM_HASH_TABLE_NODE { - GUINT m_key; - T m_data; - GIM_HASH_TABLE_NODE() - { - } + GUINT m_key; + T m_data; + GIM_HASH_TABLE_NODE() + { + } - GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE & value) - { - m_key = value.m_key; - m_data = value.m_data; - } + GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE& value) + { + m_key = value.m_key; + m_data = value.m_data; + } - GIM_HASH_TABLE_NODE(GUINT key, const T & data) - { - m_key = key; - m_data = data; - } + GIM_HASH_TABLE_NODE(GUINT key, const T& data) + { + m_key = key; + m_data = data; + } - bool operator <(const GIM_HASH_TABLE_NODE & other) const + bool operator<(const GIM_HASH_TABLE_NODE& other) const { ///inverse order, further objects are first - if(m_key < other.m_key) return true; + if (m_key < other.m_key) return true; return false; } - bool operator >(const GIM_HASH_TABLE_NODE & other) const + bool operator>(const GIM_HASH_TABLE_NODE& other) const { ///inverse order, further objects are first - if(m_key > other.m_key) return true; + if (m_key > other.m_key) return true; return false; } - bool operator ==(const GIM_HASH_TABLE_NODE & other) const + bool operator==(const GIM_HASH_TABLE_NODE& other) const { ///inverse order, further objects are first - if(m_key == other.m_key) return true; + if (m_key == other.m_key) return true; return false; } }; @@ -89,21 +88,19 @@ struct GIM_HASH_TABLE_NODE class GIM_HASH_NODE_GET_KEY { public: - template - inline GUINT operator()( const T& a) + template + inline GUINT operator()(const T& a) { return a.m_key; } }; - - ///Macro for comparing the key and the element class GIM_HASH_NODE_CMP_KEY_MACRO { public: - template - inline int operator() ( const T& a, GUINT key) + template + inline int operator()(const T& a, GUINT key) { return ((int)(a.m_key - key)); } @@ -113,65 +110,53 @@ public: class GIM_HASH_NODE_CMP_MACRO { public: - template - inline int operator() ( const T& a, const T& b ) + template + inline int operator()(const T& a, const T& b) { return ((int)(a.m_key - b.m_key)); } }; - - - - //! Sorting for hash table /*! switch automatically between quicksort and radixsort */ -template -void gim_sort_hash_node_array(T * array, GUINT array_count) +template +void gim_sort_hash_node_array(T* array, GUINT array_count) { - if(array_count */ -template +template class gim_hash_table { protected: - typedef GIM_HASH_TABLE_NODE _node_type; + typedef GIM_HASH_TABLE_NODE _node_type; - //!The nodes - //array< _node_type, SuperAllocator<_node_type> > m_nodes; - gim_array< _node_type > m_nodes; - //SuperBufferedArray< _node_type > m_nodes; - bool m_sorted; + //!The nodes + //array< _node_type, SuperAllocator<_node_type> > m_nodes; + gim_array<_node_type> m_nodes; + //SuperBufferedArray< _node_type > m_nodes; + bool m_sorted; - ///Hash table data management. The hash table has the indices to the corresponding m_nodes array - GUINT * m_hash_table;//!< - GUINT m_table_size;//!< - GUINT m_node_size;//!< - GUINT m_min_hash_table_size; + ///Hash table data management. The hash table has the indices to the corresponding m_nodes array + GUINT* m_hash_table; //!< + GUINT m_table_size; //!< + GUINT m_node_size; //!< + GUINT m_min_hash_table_size; + //! Returns the cell index + inline GUINT _find_cell(GUINT hashkey) + { + _node_type* nodesptr = m_nodes.pointer(); + GUINT start_index = (hashkey % m_table_size) * m_node_size; + GUINT end_index = start_index + m_node_size; + while (start_index < end_index) + { + GUINT value = m_hash_table[start_index]; + if (value != GIM_INVALID_HASH) + { + if (nodesptr[value].m_key == hashkey) return start_index; + } + start_index++; + } + return GIM_INVALID_HASH; + } - //! Returns the cell index - inline GUINT _find_cell(GUINT hashkey) - { - _node_type * nodesptr = m_nodes.pointer(); - GUINT start_index = (hashkey%m_table_size)*m_node_size; - GUINT end_index = start_index + m_node_size; + //! Find the avaliable cell for the hashkey, and return an existing cell if it has the same hash key + inline GUINT _find_avaliable_cell(GUINT hashkey) + { + _node_type* nodesptr = m_nodes.pointer(); + GUINT avaliable_index = GIM_INVALID_HASH; + GUINT start_index = (hashkey % m_table_size) * m_node_size; + GUINT end_index = start_index + m_node_size; - while(start_index= m_nodes.size()) return false; - if(m_nodes[index].m_key != GIM_INVALID_HASH) - { - //Search for the avaliable cell in buffer - GUINT cell_index = _find_cell(m_nodes[index].m_key); + //! erase by index in hash table + inline bool _erase_by_index_hash_table(GUINT index) + { + if (index >= m_nodes.size()) return false; + if (m_nodes[index].m_key != GIM_INVALID_HASH) + { + //Search for the avaliable cell in buffer + GUINT cell_index = _find_cell(m_nodes[index].m_key); - btAssert(cell_index!=GIM_INVALID_HASH); - btAssert(m_hash_table[cell_index]==index); + btAssert(cell_index != GIM_INVALID_HASH); + btAssert(m_hash_table[cell_index] == index); - m_hash_table[cell_index] = GIM_INVALID_HASH; - } + m_hash_table[cell_index] = GIM_INVALID_HASH; + } - return this->_erase_unsorted(index); - } + return this->_erase_unsorted(index); + } - //! erase by key in hash table - inline bool _erase_hash_table(GUINT hashkey) - { - if(hashkey == GIM_INVALID_HASH) return false; + //! erase by key in hash table + inline bool _erase_hash_table(GUINT hashkey) + { + if (hashkey == GIM_INVALID_HASH) return false; - //Search for the avaliable cell in buffer - GUINT cell_index = _find_cell(hashkey); - if(cell_index ==GIM_INVALID_HASH) return false; + //Search for the avaliable cell in buffer + GUINT cell_index = _find_cell(hashkey); + if (cell_index == GIM_INVALID_HASH) return false; - GUINT index = m_hash_table[cell_index]; - m_hash_table[cell_index] = GIM_INVALID_HASH; + GUINT index = m_hash_table[cell_index]; + m_hash_table[cell_index] = GIM_INVALID_HASH; - return this->_erase_unsorted(index); - } + return this->_erase_unsorted(index); + } - - - //! insert an element in hash table - /*! + //! insert an element in hash table + /*! If the element exists, this won't insert the element \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted If so, the element has been inserted at the last position of the array. */ - inline GUINT _insert_hash_table(GUINT hashkey, const T & value) - { - if(hashkey==GIM_INVALID_HASH) - { - //Insert anyway - _insert_unsorted(hashkey,value); - return GIM_INVALID_HASH; - } + inline GUINT _insert_hash_table(GUINT hashkey, const T& value) + { + if (hashkey == GIM_INVALID_HASH) + { + //Insert anyway + _insert_unsorted(hashkey, value); + return GIM_INVALID_HASH; + } - GUINT cell_index = _assign_hash_table_cell(hashkey); + GUINT cell_index = _assign_hash_table_cell(hashkey); - GUINT value_key = m_hash_table[cell_index]; + GUINT value_key = m_hash_table[cell_index]; - if(value_key!= GIM_INVALID_HASH) return value_key;// Not overrited + if (value_key != GIM_INVALID_HASH) return value_key; // Not overrited - m_hash_table[cell_index] = m_nodes.size(); + m_hash_table[cell_index] = m_nodes.size(); - _insert_unsorted(hashkey,value); - return GIM_INVALID_HASH; - } + _insert_unsorted(hashkey, value); + return GIM_INVALID_HASH; + } - //! insert an element in hash table. - /*! + //! insert an element in hash table. + /*! If the element exists, this replaces the element. \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted If so, the element has been inserted at the last position of the array. */ - inline GUINT _insert_hash_table_replace(GUINT hashkey, const T & value) - { - if(hashkey==GIM_INVALID_HASH) - { - //Insert anyway - _insert_unsorted(hashkey,value); - return GIM_INVALID_HASH; - } + inline GUINT _insert_hash_table_replace(GUINT hashkey, const T& value) + { + if (hashkey == GIM_INVALID_HASH) + { + //Insert anyway + _insert_unsorted(hashkey, value); + return GIM_INVALID_HASH; + } - GUINT cell_index = _assign_hash_table_cell(hashkey); + GUINT cell_index = _assign_hash_table_cell(hashkey); - GUINT value_key = m_hash_table[cell_index]; + GUINT value_key = m_hash_table[cell_index]; - if(value_key!= GIM_INVALID_HASH) - {//replaces the existing - m_nodes[value_key] = _node_type(hashkey,value); - return value_key;// index of the replaced element - } + if (value_key != GIM_INVALID_HASH) + { //replaces the existing + m_nodes[value_key] = _node_type(hashkey, value); + return value_key; // index of the replaced element + } - m_hash_table[cell_index] = m_nodes.size(); + m_hash_table[cell_index] = m_nodes.size(); - _insert_unsorted(hashkey,value); - return GIM_INVALID_HASH; + _insert_unsorted(hashkey, value); + return GIM_INVALID_HASH; + } - } + ///Sorted array data management. The hash table has the indices to the corresponding m_nodes array + inline bool _erase_sorted(GUINT index) + { + if (index >= (GUINT)m_nodes.size()) return false; + m_nodes.erase_sorted(index); + if (m_nodes.size() < 2) m_sorted = false; + return true; + } - - ///Sorted array data management. The hash table has the indices to the corresponding m_nodes array - inline bool _erase_sorted(GUINT index) - { - if(index>=(GUINT)m_nodes.size()) return false; - m_nodes.erase_sorted(index); - if(m_nodes.size()<2) m_sorted = false; - return true; - } + //! faster, but unsorted + inline bool _erase_unsorted(GUINT index) + { + if (index >= m_nodes.size()) return false; - //! faster, but unsorted - inline bool _erase_unsorted(GUINT index) - { - if(index>=m_nodes.size()) return false; - - GUINT lastindex = m_nodes.size()-1; - if(indexcheck_for_switching_to_hashtable(); - } + inline void _insert_in_pos(GUINT hashkey, const T& value, GUINT pos) + { + m_nodes.insert(_node_type(hashkey, value), pos); + this->check_for_switching_to_hashtable(); + } - //! Insert an element in an ordered array - inline GUINT _insert_sorted(GUINT hashkey, const T & value) - { - if(hashkey==GIM_INVALID_HASH || size()==0) - { - m_nodes.push_back(_node_type(hashkey,value)); - return GIM_INVALID_HASH; - } - //Insert at last position - //Sort element + //! Insert an element in an ordered array + inline GUINT _insert_sorted(GUINT hashkey, const T& value) + { + if (hashkey == GIM_INVALID_HASH || size() == 0) + { + m_nodes.push_back(_node_type(hashkey, value)); + return GIM_INVALID_HASH; + } + //Insert at last position + //Sort element + GUINT result_ind = 0; + GUINT last_index = m_nodes.size() - 1; + _node_type* ptr = m_nodes.pointer(); - GUINT result_ind=0; - GUINT last_index = m_nodes.size()-1; - _node_type * ptr = m_nodes.pointer(); + bool found = gim_binary_search_ex( + ptr, 0, last_index, result_ind, hashkey, GIM_HASH_NODE_CMP_KEY_MACRO()); - bool found = gim_binary_search_ex( - ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO()); + //Insert before found index + if (found) + { + return result_ind; + } + else + { + _insert_in_pos(hashkey, value, result_ind); + } + return GIM_INVALID_HASH; + } + inline GUINT _insert_sorted_replace(GUINT hashkey, const T& value) + { + if (hashkey == GIM_INVALID_HASH || size() == 0) + { + m_nodes.push_back(_node_type(hashkey, value)); + return GIM_INVALID_HASH; + } + //Insert at last position + //Sort element + GUINT result_ind; + GUINT last_index = m_nodes.size() - 1; + _node_type* ptr = m_nodes.pointer(); - //Insert before found index - if(found) - { - return result_ind; - } - else - { - _insert_in_pos(hashkey, value, result_ind); - } - return GIM_INVALID_HASH; - } + bool found = gim_binary_search_ex( + ptr, 0, last_index, result_ind, hashkey, GIM_HASH_NODE_CMP_KEY_MACRO()); - inline GUINT _insert_sorted_replace(GUINT hashkey, const T & value) - { - if(hashkey==GIM_INVALID_HASH || size()==0) - { - m_nodes.push_back(_node_type(hashkey,value)); - return GIM_INVALID_HASH; - } - //Insert at last position - //Sort element - GUINT result_ind; - GUINT last_index = m_nodes.size()-1; - _node_type * ptr = m_nodes.pointer(); + //Insert before found index + if (found) + { + m_nodes[result_ind] = _node_type(hashkey, value); + } + else + { + _insert_in_pos(hashkey, value, result_ind); + } + return result_ind; + } - bool found = gim_binary_search_ex( - ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO()); - - //Insert before found index - if(found) - { - m_nodes[result_ind] = _node_type(hashkey,value); - } - else - { - _insert_in_pos(hashkey, value, result_ind); - } - return result_ind; - } - - //! Fast insertion in m_nodes array - inline GUINT _insert_unsorted(GUINT hashkey, const T & value) - { - m_nodes.push_back(_node_type(hashkey,value)); - m_sorted = false; - return GIM_INVALID_HASH; - } - - + //! Fast insertion in m_nodes array + inline GUINT _insert_unsorted(GUINT hashkey, const T& value) + { + m_nodes.push_back(_node_type(hashkey, value)); + m_sorted = false; + return GIM_INVALID_HASH; + } public: - - /*! + /*!
  • if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes. When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable.
  • If node_size != 0, then this container becomes a hash table for ever */ - gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE, - GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE, - GUINT min_hash_table_size = GIM_INVALID_HASH) - { - m_hash_table = NULL; - m_table_size = 0; - m_sorted = false; - m_node_size = node_size; - m_min_hash_table_size = min_hash_table_size; + gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE, + GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE, + GUINT min_hash_table_size = GIM_INVALID_HASH) + { + m_hash_table = NULL; + m_table_size = 0; + m_sorted = false; + m_node_size = node_size; + m_min_hash_table_size = min_hash_table_size; - if(m_node_size!=0) - { - if(reserve_size!=0) - { - m_nodes.reserve(reserve_size); - _reserve_table_memory(reserve_size); - _invalidate_keys(); - } - else - { - m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE); - _reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE); - _invalidate_keys(); - } - } - else if(reserve_size!=0) - { - m_nodes.reserve(reserve_size); - } + if (m_node_size != 0) + { + if (reserve_size != 0) + { + m_nodes.reserve(reserve_size); + _reserve_table_memory(reserve_size); + _invalidate_keys(); + } + else + { + m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE); + _reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE); + _invalidate_keys(); + } + } + else if (reserve_size != 0) + { + m_nodes.reserve(reserve_size); + } + } - } + ~gim_hash_table() + { + _destroy(); + } - ~gim_hash_table() - { - _destroy(); - } + inline bool is_hash_table() + { + if (m_hash_table) return true; + return false; + } - inline bool is_hash_table() - { - if(m_hash_table) return true; - return false; - } + inline bool is_sorted() + { + if (size() < 2) return true; + return m_sorted; + } - inline bool is_sorted() - { - if(size()<2) return true; - return m_sorted; - } + bool sort() + { + if (is_sorted()) return true; + if (m_nodes.size() < 2) return false; - bool sort() - { - if(is_sorted()) return true; - if(m_nodes.size()<2) return false; + _node_type* ptr = m_nodes.pointer(); + GUINT siz = m_nodes.size(); + gim_sort_hash_node_array(ptr, siz); + m_sorted = true; + if (m_hash_table) + { + _rehash(); + } + return true; + } - _node_type * ptr = m_nodes.pointer(); - GUINT siz = m_nodes.size(); - gim_sort_hash_node_array(ptr,siz); - m_sorted=true; + bool switch_to_hashtable() + { + if (m_hash_table) return false; + if (m_node_size == 0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE; + if (m_nodes.size() < GIM_DEFAULT_HASH_TABLE_SIZE) + { + _resize_table(GIM_DEFAULT_HASH_TABLE_SIZE); + } + else + { + _resize_table(m_nodes.size() + 1); + } + return true; + } + bool switch_to_sorted_array() + { + if (m_hash_table == NULL) return true; + _clear_table_memory(); + return sort(); + } - if(m_hash_table) - { - _rehash(); - } - return true; - } + //!If the container reaches the + bool check_for_switching_to_hashtable() + { + if (this->m_hash_table) return true; - bool switch_to_hashtable() - { - if(m_hash_table) return false; - if(m_node_size==0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE; - if(m_nodes.size()m_hash_table) return true; + //! Retrieves the amount of keys. + inline GUINT size() const + { + return m_nodes.size(); + } - if(!(m_nodes.size()< m_min_hash_table_size)) - { - if(m_node_size == 0) - { - m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE; - } + //! Retrieves the hash key. + inline GUINT get_key(GUINT index) const + { + return m_nodes[index].m_key; + } - _resize_table(m_nodes.size()+1); - return true; - } - return false; - } - - inline void set_sorted(bool value) - { - m_sorted = value; - } - - //! Retrieves the amount of keys. - inline GUINT size() const - { - return m_nodes.size(); - } - - //! Retrieves the hash key. - inline GUINT get_key(GUINT index) const - { - return m_nodes[index].m_key; - } - - //! Retrieves the value by index - /*! + //! Retrieves the value by index + /*! */ - inline T * get_value_by_index(GUINT index) - { - return &m_nodes[index].m_data; - } + inline T* get_value_by_index(GUINT index) + { + return &m_nodes[index].m_data; + } - inline const T& operator[](GUINT index) const - { - return m_nodes[index].m_data; - } + inline const T& operator[](GUINT index) const + { + return m_nodes[index].m_data; + } - inline T& operator[](GUINT index) - { - return m_nodes[index].m_data; - } + inline T& operator[](GUINT index) + { + return m_nodes[index].m_data; + } - //! Finds the index of the element with the key - /*! + //! Finds the index of the element with the key + /*! \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted If so, the element has been inserted at the last position of the array. */ - inline GUINT find(GUINT hashkey) - { - if(m_hash_table) - { - GUINT cell_index = _find_cell(hashkey); - if(cell_index==GIM_INVALID_HASH) return GIM_INVALID_HASH; - return m_hash_table[cell_index]; - } + inline GUINT find(GUINT hashkey) + { + if (m_hash_table) + { + GUINT cell_index = _find_cell(hashkey); + if (cell_index == GIM_INVALID_HASH) return GIM_INVALID_HASH; + return m_hash_table[cell_index]; + } GUINT last_index = m_nodes.size(); - if(last_index<2) - { - if(last_index==0) return GIM_INVALID_HASH; - if(m_nodes[0].m_key == hashkey) return 0; - return GIM_INVALID_HASH; - } - else if(m_sorted) - { - //Binary search - GUINT result_ind = 0; + if (last_index < 2) + { + if (last_index == 0) return GIM_INVALID_HASH; + if (m_nodes[0].m_key == hashkey) return 0; + return GIM_INVALID_HASH; + } + else if (m_sorted) + { + //Binary search + GUINT result_ind = 0; last_index--; - _node_type * ptr = m_nodes.pointer(); + _node_type* ptr = m_nodes.pointer(); - bool found = gim_binary_search_ex(ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO()); + bool found = gim_binary_search_ex(ptr, 0, last_index, result_ind, hashkey, GIM_HASH_NODE_CMP_KEY_MACRO()); + if (found) return result_ind; + } + return GIM_INVALID_HASH; + } - if(found) return result_ind; - } - return GIM_INVALID_HASH; - } - - //! Retrieves the value associated with the index - /*! + //! Retrieves the value associated with the index + /*! \return the found element, or null */ - inline T * get_value(GUINT hashkey) - { - GUINT index = find(hashkey); - if(index == GIM_INVALID_HASH) return NULL; - return &m_nodes[index].m_data; - } + inline T* get_value(GUINT hashkey) + { + GUINT index = find(hashkey); + if (index == GIM_INVALID_HASH) return NULL; + return &m_nodes[index].m_data; + } - - /*! + /*! */ - inline bool erase_by_index(GUINT index) - { - if(index > m_nodes.size()) return false; + inline bool erase_by_index(GUINT index) + { + if (index > m_nodes.size()) return false; - if(m_hash_table == NULL) - { - if(is_sorted()) - { - return this->_erase_sorted(index); - } - else - { - return this->_erase_unsorted(index); - } - } - else - { - return this->_erase_by_index_hash_table(index); - } - return false; - } + if (m_hash_table == NULL) + { + if (is_sorted()) + { + return this->_erase_sorted(index); + } + else + { + return this->_erase_unsorted(index); + } + } + else + { + return this->_erase_by_index_hash_table(index); + } + return false; + } + inline bool erase_by_index_unsorted(GUINT index) + { + if (index > m_nodes.size()) return false; + if (m_hash_table == NULL) + { + return this->_erase_unsorted(index); + } + else + { + return this->_erase_by_index_hash_table(index); + } + return false; + } - inline bool erase_by_index_unsorted(GUINT index) - { - if(index > m_nodes.size()) return false; - - if(m_hash_table == NULL) - { - return this->_erase_unsorted(index); - } - else - { - return this->_erase_by_index_hash_table(index); - } - return false; - } - - - - /*! + /*! */ - inline bool erase_by_key(GUINT hashkey) - { - if(size()==0) return false; + inline bool erase_by_key(GUINT hashkey) + { + if (size() == 0) return false; - if(m_hash_table) - { - return this->_erase_hash_table(hashkey); - } - //Binary search + if (m_hash_table) + { + return this->_erase_hash_table(hashkey); + } + //Binary search - if(is_sorted()==false) return false; + if (is_sorted() == false) return false; - GUINT result_ind = find(hashkey); - if(result_ind!= GIM_INVALID_HASH) - { - return this->_erase_sorted(result_ind); - } - return false; - } + GUINT result_ind = find(hashkey); + if (result_ind != GIM_INVALID_HASH) + { + return this->_erase_sorted(result_ind); + } + return false; + } - void clear() - { - m_nodes.clear(); + void clear() + { + m_nodes.clear(); - if(m_hash_table==NULL) return; - GUINT datasize = m_table_size*m_node_size; - //Initialize the hashkeys. - GUINT i; - for(i=0;i_insert_hash_table(hashkey,element); - } - if(this->is_sorted()) - { - return this->_insert_sorted(hashkey,element); - } - return this->_insert_unsorted(hashkey,element); - } + inline GUINT insert(GUINT hashkey, const T& element) + { + if (m_hash_table) + { + return this->_insert_hash_table(hashkey, element); + } + if (this->is_sorted()) + { + return this->_insert_sorted(hashkey, element); + } + return this->_insert_unsorted(hashkey, element); + } - //! Insert an element into the hash, and could overrite an existing object with the same hash. - /*! + //! Insert an element into the hash, and could overrite an existing object with the same hash. + /*! \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position of the replaced element. */ - inline GUINT insert_override(GUINT hashkey, const T & element) - { - if(m_hash_table) - { - return this->_insert_hash_table_replace(hashkey,element); - } - if(this->is_sorted()) - { - return this->_insert_sorted_replace(hashkey,element); - } - this->_insert_unsorted(hashkey,element); - return m_nodes.size(); - } + inline GUINT insert_override(GUINT hashkey, const T& element) + { + if (m_hash_table) + { + return this->_insert_hash_table_replace(hashkey, element); + } + if (this->is_sorted()) + { + return this->_insert_sorted_replace(hashkey, element); + } + this->_insert_unsorted(hashkey, element); + return m_nodes.size(); + } - - - //! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted - /*! + //! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted + /*! */ - inline GUINT insert_unsorted(GUINT hashkey,const T & element) - { - if(m_hash_table) - { - return this->_insert_hash_table(hashkey,element); - } - return this->_insert_unsorted(hashkey,element); - } - - + inline GUINT insert_unsorted(GUINT hashkey, const T& element) + { + if (m_hash_table) + { + return this->_insert_hash_table(hashkey, element); + } + return this->_insert_unsorted(hashkey, element); + } }; - - -#endif // GIM_CONTAINERS_H_INCLUDED +#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_linear_math.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_linear_math.h index 64f11b495..98401a404 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_linear_math.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_linear_math.h @@ -34,962 +34,900 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_math.h" #include "gim_geom_types.h" - - - //! Zero out a 2D vector -#define VEC_ZERO_2(a) \ -{ \ - (a)[0] = (a)[1] = 0.0f; \ -}\ - +#define VEC_ZERO_2(a) \ + { \ + (a)[0] = (a)[1] = 0.0f; \ + } //! Zero out a 3D vector -#define VEC_ZERO(a) \ -{ \ - (a)[0] = (a)[1] = (a)[2] = 0.0f; \ -}\ - +#define VEC_ZERO(a) \ + { \ + (a)[0] = (a)[1] = (a)[2] = 0.0f; \ + } /// Zero out a 4D vector -#define VEC_ZERO_4(a) \ -{ \ - (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f; \ -}\ - +#define VEC_ZERO_4(a) \ + { \ + (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f; \ + } /// Vector copy -#define VEC_COPY_2(b,a) \ -{ \ - (b)[0] = (a)[0]; \ - (b)[1] = (a)[1]; \ -}\ - +#define VEC_COPY_2(b, a) \ + { \ + (b)[0] = (a)[0]; \ + (b)[1] = (a)[1]; \ + } /// Copy 3D vector -#define VEC_COPY(b,a) \ -{ \ - (b)[0] = (a)[0]; \ - (b)[1] = (a)[1]; \ - (b)[2] = (a)[2]; \ -}\ - +#define VEC_COPY(b, a) \ + { \ + (b)[0] = (a)[0]; \ + (b)[1] = (a)[1]; \ + (b)[2] = (a)[2]; \ + } /// Copy 4D vector -#define VEC_COPY_4(b,a) \ -{ \ - (b)[0] = (a)[0]; \ - (b)[1] = (a)[1]; \ - (b)[2] = (a)[2]; \ - (b)[3] = (a)[3]; \ -}\ +#define VEC_COPY_4(b, a) \ + { \ + (b)[0] = (a)[0]; \ + (b)[1] = (a)[1]; \ + (b)[2] = (a)[2]; \ + (b)[3] = (a)[3]; \ + } /// VECTOR SWAP -#define VEC_SWAP(b,a) \ -{ \ - GIM_SWAP_NUMBERS((b)[0],(a)[0]);\ - GIM_SWAP_NUMBERS((b)[1],(a)[1]);\ - GIM_SWAP_NUMBERS((b)[2],(a)[2]);\ -}\ +#define VEC_SWAP(b, a) \ + { \ + GIM_SWAP_NUMBERS((b)[0], (a)[0]); \ + GIM_SWAP_NUMBERS((b)[1], (a)[1]); \ + GIM_SWAP_NUMBERS((b)[2], (a)[2]); \ + } /// Vector difference -#define VEC_DIFF_2(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] - (v1)[0]; \ - (v21)[1] = (v2)[1] - (v1)[1]; \ -}\ - +#define VEC_DIFF_2(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] - (v1)[0]; \ + (v21)[1] = (v2)[1] - (v1)[1]; \ + } /// Vector difference -#define VEC_DIFF(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] - (v1)[0]; \ - (v21)[1] = (v2)[1] - (v1)[1]; \ - (v21)[2] = (v2)[2] - (v1)[2]; \ -}\ - +#define VEC_DIFF(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] - (v1)[0]; \ + (v21)[1] = (v2)[1] - (v1)[1]; \ + (v21)[2] = (v2)[2] - (v1)[2]; \ + } /// Vector difference -#define VEC_DIFF_4(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] - (v1)[0]; \ - (v21)[1] = (v2)[1] - (v1)[1]; \ - (v21)[2] = (v2)[2] - (v1)[2]; \ - (v21)[3] = (v2)[3] - (v1)[3]; \ -}\ - +#define VEC_DIFF_4(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] - (v1)[0]; \ + (v21)[1] = (v2)[1] - (v1)[1]; \ + (v21)[2] = (v2)[2] - (v1)[2]; \ + (v21)[3] = (v2)[3] - (v1)[3]; \ + } /// Vector sum -#define VEC_SUM_2(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] + (v1)[0]; \ - (v21)[1] = (v2)[1] + (v1)[1]; \ -}\ - +#define VEC_SUM_2(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] + (v1)[0]; \ + (v21)[1] = (v2)[1] + (v1)[1]; \ + } /// Vector sum -#define VEC_SUM(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] + (v1)[0]; \ - (v21)[1] = (v2)[1] + (v1)[1]; \ - (v21)[2] = (v2)[2] + (v1)[2]; \ -}\ - +#define VEC_SUM(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] + (v1)[0]; \ + (v21)[1] = (v2)[1] + (v1)[1]; \ + (v21)[2] = (v2)[2] + (v1)[2]; \ + } /// Vector sum -#define VEC_SUM_4(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] + (v1)[0]; \ - (v21)[1] = (v2)[1] + (v1)[1]; \ - (v21)[2] = (v2)[2] + (v1)[2]; \ - (v21)[3] = (v2)[3] + (v1)[3]; \ -}\ - +#define VEC_SUM_4(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] + (v1)[0]; \ + (v21)[1] = (v2)[1] + (v1)[1]; \ + (v21)[2] = (v2)[2] + (v1)[2]; \ + (v21)[3] = (v2)[3] + (v1)[3]; \ + } /// scalar times vector -#define VEC_SCALE_2(c,a,b) \ -{ \ - (c)[0] = (a)*(b)[0]; \ - (c)[1] = (a)*(b)[1]; \ -}\ - +#define VEC_SCALE_2(c, a, b) \ + { \ + (c)[0] = (a) * (b)[0]; \ + (c)[1] = (a) * (b)[1]; \ + } /// scalar times vector -#define VEC_SCALE(c,a,b) \ -{ \ - (c)[0] = (a)*(b)[0]; \ - (c)[1] = (a)*(b)[1]; \ - (c)[2] = (a)*(b)[2]; \ -}\ - +#define VEC_SCALE(c, a, b) \ + { \ + (c)[0] = (a) * (b)[0]; \ + (c)[1] = (a) * (b)[1]; \ + (c)[2] = (a) * (b)[2]; \ + } /// scalar times vector -#define VEC_SCALE_4(c,a,b) \ -{ \ - (c)[0] = (a)*(b)[0]; \ - (c)[1] = (a)*(b)[1]; \ - (c)[2] = (a)*(b)[2]; \ - (c)[3] = (a)*(b)[3]; \ -}\ - +#define VEC_SCALE_4(c, a, b) \ + { \ + (c)[0] = (a) * (b)[0]; \ + (c)[1] = (a) * (b)[1]; \ + (c)[2] = (a) * (b)[2]; \ + (c)[3] = (a) * (b)[3]; \ + } /// accumulate scaled vector -#define VEC_ACCUM_2(c,a,b) \ -{ \ - (c)[0] += (a)*(b)[0]; \ - (c)[1] += (a)*(b)[1]; \ -}\ - +#define VEC_ACCUM_2(c, a, b) \ + { \ + (c)[0] += (a) * (b)[0]; \ + (c)[1] += (a) * (b)[1]; \ + } /// accumulate scaled vector -#define VEC_ACCUM(c,a,b) \ -{ \ - (c)[0] += (a)*(b)[0]; \ - (c)[1] += (a)*(b)[1]; \ - (c)[2] += (a)*(b)[2]; \ -}\ - +#define VEC_ACCUM(c, a, b) \ + { \ + (c)[0] += (a) * (b)[0]; \ + (c)[1] += (a) * (b)[1]; \ + (c)[2] += (a) * (b)[2]; \ + } /// accumulate scaled vector -#define VEC_ACCUM_4(c,a,b) \ -{ \ - (c)[0] += (a)*(b)[0]; \ - (c)[1] += (a)*(b)[1]; \ - (c)[2] += (a)*(b)[2]; \ - (c)[3] += (a)*(b)[3]; \ -}\ - +#define VEC_ACCUM_4(c, a, b) \ + { \ + (c)[0] += (a) * (b)[0]; \ + (c)[1] += (a) * (b)[1]; \ + (c)[2] += (a) * (b)[2]; \ + (c)[3] += (a) * (b)[3]; \ + } /// Vector dot product -#define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1]) - +#define VEC_DOT_2(a, b) ((a)[0] * (b)[0] + (a)[1] * (b)[1]) /// Vector dot product -#define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]) +#define VEC_DOT(a, b) ((a)[0] * (b)[0] + (a)[1] * (b)[1] + (a)[2] * (b)[2]) /// Vector dot product -#define VEC_DOT_4(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3]) +#define VEC_DOT_4(a, b) ((a)[0] * (b)[0] + (a)[1] * (b)[1] + (a)[2] * (b)[2] + (a)[3] * (b)[3]) /// vector impact parameter (squared) -#define VEC_IMPACT_SQ(bsq,direction,position) {\ - GREAL _llel_ = VEC_DOT(direction, position);\ - bsq = VEC_DOT(position, position) - _llel_*_llel_;\ -}\ - +#define VEC_IMPACT_SQ(bsq, direction, position) \ + { \ + GREAL _llel_ = VEC_DOT(direction, position); \ + bsq = VEC_DOT(position, position) - _llel_ * _llel_; \ + } /// vector impact parameter -#define VEC_IMPACT(bsq,direction,position) {\ - VEC_IMPACT_SQ(bsq,direction,position); \ - GIM_SQRT(bsq,bsq); \ -}\ +#define VEC_IMPACT(bsq, direction, position) \ + { \ + VEC_IMPACT_SQ(bsq, direction, position); \ + GIM_SQRT(bsq, bsq); \ + } /// Vector length -#define VEC_LENGTH_2(a,l)\ -{\ - GREAL _pp = VEC_DOT_2(a,a);\ - GIM_SQRT(_pp,l);\ -}\ - +#define VEC_LENGTH_2(a, l) \ + { \ + GREAL _pp = VEC_DOT_2(a, a); \ + GIM_SQRT(_pp, l); \ + } /// Vector length -#define VEC_LENGTH(a,l)\ -{\ - GREAL _pp = VEC_DOT(a,a);\ - GIM_SQRT(_pp,l);\ -}\ - +#define VEC_LENGTH(a, l) \ + { \ + GREAL _pp = VEC_DOT(a, a); \ + GIM_SQRT(_pp, l); \ + } /// Vector length -#define VEC_LENGTH_4(a,l)\ -{\ - GREAL _pp = VEC_DOT_4(a,a);\ - GIM_SQRT(_pp,l);\ -}\ +#define VEC_LENGTH_4(a, l) \ + { \ + GREAL _pp = VEC_DOT_4(a, a); \ + GIM_SQRT(_pp, l); \ + } /// Vector inv length -#define VEC_INV_LENGTH_2(a,l)\ -{\ - GREAL _pp = VEC_DOT_2(a,a);\ - GIM_INV_SQRT(_pp,l);\ -}\ - +#define VEC_INV_LENGTH_2(a, l) \ + { \ + GREAL _pp = VEC_DOT_2(a, a); \ + GIM_INV_SQRT(_pp, l); \ + } /// Vector inv length -#define VEC_INV_LENGTH(a,l)\ -{\ - GREAL _pp = VEC_DOT(a,a);\ - GIM_INV_SQRT(_pp,l);\ -}\ - +#define VEC_INV_LENGTH(a, l) \ + { \ + GREAL _pp = VEC_DOT(a, a); \ + GIM_INV_SQRT(_pp, l); \ + } /// Vector inv length -#define VEC_INV_LENGTH_4(a,l)\ -{\ - GREAL _pp = VEC_DOT_4(a,a);\ - GIM_INV_SQRT(_pp,l);\ -}\ - - +#define VEC_INV_LENGTH_4(a, l) \ + { \ + GREAL _pp = VEC_DOT_4(a, a); \ + GIM_INV_SQRT(_pp, l); \ + } /// distance between two points -#define VEC_DISTANCE(_len,_va,_vb) {\ - vec3f _tmp_; \ - VEC_DIFF(_tmp_, _vb, _va); \ - VEC_LENGTH(_tmp_,_len); \ -}\ - +#define VEC_DISTANCE(_len, _va, _vb) \ + { \ + vec3f _tmp_; \ + VEC_DIFF(_tmp_, _vb, _va); \ + VEC_LENGTH(_tmp_, _len); \ + } /// Vector length -#define VEC_CONJUGATE_LENGTH(a,l)\ -{\ - GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\ - GIM_SQRT(_pp,l);\ -}\ - +#define VEC_CONJUGATE_LENGTH(a, l) \ + { \ + GREAL _pp = 1.0 - a[0] * a[0] - a[1] * a[1] - a[2] * a[2]; \ + GIM_SQRT(_pp, l); \ + } /// Vector length -#define VEC_NORMALIZE(a) { \ - GREAL len;\ - VEC_INV_LENGTH(a,len); \ - if(lenA[1]?(A[0]>A[2]?0:2):(A[1]>A[2]?1:2);\ -}\ +#define VEC_MAYOR_COORD(vec, maxc) \ + { \ + GREAL A[] = {fabs(vec[0]), fabs(vec[1]), fabs(vec[2])}; \ + maxc = A[0] > A[1] ? (A[0] > A[2] ? 0 : 2) : (A[1] > A[2] ? 1 : 2); \ + } //! Finds the 2 smallest cartesian coordinates from a vector -#define VEC_MINOR_AXES(vec, i0, i1)\ -{\ - VEC_MAYOR_COORD(vec,i0);\ - i0 = (i0+1)%3;\ - i1 = (i0+1)%3;\ -}\ +#define VEC_MINOR_AXES(vec, i0, i1) \ + { \ + VEC_MAYOR_COORD(vec, i0); \ + i0 = (i0 + 1) % 3; \ + i1 = (i0 + 1) % 3; \ + } +#define VEC_EQUAL(v1, v2) (v1[0] == v2[0] && v1[1] == v2[1] && v1[2] == v2[2]) - - -#define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2]) - -#define VEC_NEAR_EQUAL(v1,v2) (GIM_NEAR_EQUAL(v1[0],v2[0])&&GIM_NEAR_EQUAL(v1[1],v2[1])&&GIM_NEAR_EQUAL(v1[2],v2[2])) - +#define VEC_NEAR_EQUAL(v1, v2) (GIM_NEAR_EQUAL(v1[0], v2[0]) && GIM_NEAR_EQUAL(v1[1], v2[1]) && GIM_NEAR_EQUAL(v1[2], v2[2])) /// Vector cross -#define X_AXIS_CROSS_VEC(dst,src)\ -{ \ - dst[0] = 0.0f; \ - dst[1] = -src[2]; \ - dst[2] = src[1]; \ -}\ - -#define Y_AXIS_CROSS_VEC(dst,src)\ -{ \ - dst[0] = src[2]; \ - dst[1] = 0.0f; \ - dst[2] = -src[0]; \ -}\ - -#define Z_AXIS_CROSS_VEC(dst,src)\ -{ \ - dst[0] = -src[1]; \ - dst[1] = src[0]; \ - dst[2] = 0.0f; \ -}\ - - - +#define X_AXIS_CROSS_VEC(dst, src) \ + { \ + dst[0] = 0.0f; \ + dst[1] = -src[2]; \ + dst[2] = src[1]; \ + } +#define Y_AXIS_CROSS_VEC(dst, src) \ + { \ + dst[0] = src[2]; \ + dst[1] = 0.0f; \ + dst[2] = -src[0]; \ + } +#define Z_AXIS_CROSS_VEC(dst, src) \ + { \ + dst[0] = -src[1]; \ + dst[1] = src[0]; \ + dst[2] = 0.0f; \ + } /// initialize matrix -#define IDENTIFY_MATRIX_3X3(m) \ -{ \ - m[0][0] = 1.0; \ - m[0][1] = 0.0; \ - m[0][2] = 0.0; \ - \ - m[1][0] = 0.0; \ - m[1][1] = 1.0; \ - m[1][2] = 0.0; \ - \ - m[2][0] = 0.0; \ - m[2][1] = 0.0; \ - m[2][2] = 1.0; \ -}\ +#define IDENTIFY_MATRIX_3X3(m) \ + { \ + m[0][0] = 1.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 1.0; \ + m[1][2] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 1.0; \ + } /*! initialize matrix */ -#define IDENTIFY_MATRIX_4X4(m) \ -{ \ - m[0][0] = 1.0; \ - m[0][1] = 0.0; \ - m[0][2] = 0.0; \ - m[0][3] = 0.0; \ - \ - m[1][0] = 0.0; \ - m[1][1] = 1.0; \ - m[1][2] = 0.0; \ - m[1][3] = 0.0; \ - \ - m[2][0] = 0.0; \ - m[2][1] = 0.0; \ - m[2][2] = 1.0; \ - m[2][3] = 0.0; \ - \ - m[3][0] = 0.0; \ - m[3][1] = 0.0; \ - m[3][2] = 0.0; \ - m[3][3] = 1.0; \ -}\ +#define IDENTIFY_MATRIX_4X4(m) \ + { \ + m[0][0] = 1.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 1.0; \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 1.0; \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ + } /*! initialize matrix */ -#define ZERO_MATRIX_4X4(m) \ -{ \ - m[0][0] = 0.0; \ - m[0][1] = 0.0; \ - m[0][2] = 0.0; \ - m[0][3] = 0.0; \ - \ - m[1][0] = 0.0; \ - m[1][1] = 0.0; \ - m[1][2] = 0.0; \ - m[1][3] = 0.0; \ - \ - m[2][0] = 0.0; \ - m[2][1] = 0.0; \ - m[2][2] = 0.0; \ - m[2][3] = 0.0; \ - \ - m[3][0] = 0.0; \ - m[3][1] = 0.0; \ - m[3][2] = 0.0; \ - m[3][3] = 0.0; \ -}\ +#define ZERO_MATRIX_4X4(m) \ + { \ + m[0][0] = 0.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 0.0; \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 0.0; \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 0.0; \ + } /*! matrix rotation X */ -#define ROTX_CS(m,cosine,sine) \ -{ \ - /* rotation about the x-axis */ \ - \ - m[0][0] = 1.0; \ - m[0][1] = 0.0; \ - m[0][2] = 0.0; \ - m[0][3] = 0.0; \ - \ - m[1][0] = 0.0; \ - m[1][1] = (cosine); \ - m[1][2] = (sine); \ - m[1][3] = 0.0; \ - \ - m[2][0] = 0.0; \ - m[2][1] = -(sine); \ - m[2][2] = (cosine); \ - m[2][3] = 0.0; \ - \ - m[3][0] = 0.0; \ - m[3][1] = 0.0; \ - m[3][2] = 0.0; \ - m[3][3] = 1.0; \ -}\ +#define ROTX_CS(m, cosine, sine) \ + { \ + /* rotation about the x-axis */ \ + \ + m[0][0] = 1.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = (cosine); \ + m[1][2] = (sine); \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = -(sine); \ + m[2][2] = (cosine); \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ + } /*! matrix rotation Y */ -#define ROTY_CS(m,cosine,sine) \ -{ \ - /* rotation about the y-axis */ \ - \ - m[0][0] = (cosine); \ - m[0][1] = 0.0; \ - m[0][2] = -(sine); \ - m[0][3] = 0.0; \ - \ - m[1][0] = 0.0; \ - m[1][1] = 1.0; \ - m[1][2] = 0.0; \ - m[1][3] = 0.0; \ - \ - m[2][0] = (sine); \ - m[2][1] = 0.0; \ - m[2][2] = (cosine); \ - m[2][3] = 0.0; \ - \ - m[3][0] = 0.0; \ - m[3][1] = 0.0; \ - m[3][2] = 0.0; \ - m[3][3] = 1.0; \ -}\ +#define ROTY_CS(m, cosine, sine) \ + { \ + /* rotation about the y-axis */ \ + \ + m[0][0] = (cosine); \ + m[0][1] = 0.0; \ + m[0][2] = -(sine); \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 1.0; \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = (sine); \ + m[2][1] = 0.0; \ + m[2][2] = (cosine); \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ + } /*! matrix rotation Z */ -#define ROTZ_CS(m,cosine,sine) \ -{ \ - /* rotation about the z-axis */ \ - \ - m[0][0] = (cosine); \ - m[0][1] = (sine); \ - m[0][2] = 0.0; \ - m[0][3] = 0.0; \ - \ - m[1][0] = -(sine); \ - m[1][1] = (cosine); \ - m[1][2] = 0.0; \ - m[1][3] = 0.0; \ - \ - m[2][0] = 0.0; \ - m[2][1] = 0.0; \ - m[2][2] = 1.0; \ - m[2][3] = 0.0; \ - \ - m[3][0] = 0.0; \ - m[3][1] = 0.0; \ - m[3][2] = 0.0; \ - m[3][3] = 1.0; \ -}\ +#define ROTZ_CS(m, cosine, sine) \ + { \ + /* rotation about the z-axis */ \ + \ + m[0][0] = (cosine); \ + m[0][1] = (sine); \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = -(sine); \ + m[1][1] = (cosine); \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 1.0; \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ + } /*! matrix copy */ -#define COPY_MATRIX_2X2(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[0][1]; \ - \ - b[1][0] = a[1][0]; \ - b[1][1] = a[1][1]; \ - \ -}\ - +#define COPY_MATRIX_2X2(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + } /*! matrix copy */ -#define COPY_MATRIX_2X3(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[0][1]; \ - b[0][2] = a[0][2]; \ - \ - b[1][0] = a[1][0]; \ - b[1][1] = a[1][1]; \ - b[1][2] = a[1][2]; \ -}\ - +#define COPY_MATRIX_2X3(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + b[0][2] = a[0][2]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[1][2]; \ + } /*! matrix copy */ -#define COPY_MATRIX_3X3(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[0][1]; \ - b[0][2] = a[0][2]; \ - \ - b[1][0] = a[1][0]; \ - b[1][1] = a[1][1]; \ - b[1][2] = a[1][2]; \ - \ - b[2][0] = a[2][0]; \ - b[2][1] = a[2][1]; \ - b[2][2] = a[2][2]; \ -}\ - +#define COPY_MATRIX_3X3(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + b[0][2] = a[0][2]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[1][2]; \ + \ + b[2][0] = a[2][0]; \ + b[2][1] = a[2][1]; \ + b[2][2] = a[2][2]; \ + } /*! matrix copy */ -#define COPY_MATRIX_4X4(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[0][1]; \ - b[0][2] = a[0][2]; \ - b[0][3] = a[0][3]; \ - \ - b[1][0] = a[1][0]; \ - b[1][1] = a[1][1]; \ - b[1][2] = a[1][2]; \ - b[1][3] = a[1][3]; \ - \ - b[2][0] = a[2][0]; \ - b[2][1] = a[2][1]; \ - b[2][2] = a[2][2]; \ - b[2][3] = a[2][3]; \ - \ - b[3][0] = a[3][0]; \ - b[3][1] = a[3][1]; \ - b[3][2] = a[3][2]; \ - b[3][3] = a[3][3]; \ -}\ - +#define COPY_MATRIX_4X4(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + b[0][2] = a[0][2]; \ + b[0][3] = a[0][3]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[1][2]; \ + b[1][3] = a[1][3]; \ + \ + b[2][0] = a[2][0]; \ + b[2][1] = a[2][1]; \ + b[2][2] = a[2][2]; \ + b[2][3] = a[2][3]; \ + \ + b[3][0] = a[3][0]; \ + b[3][1] = a[3][1]; \ + b[3][2] = a[3][2]; \ + b[3][3] = a[3][3]; \ + } /*! matrix transpose */ -#define TRANSPOSE_MATRIX_2X2(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[1][0]; \ - \ - b[1][0] = a[0][1]; \ - b[1][1] = a[1][1]; \ -}\ - +#define TRANSPOSE_MATRIX_2X2(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[1][0]; \ + \ + b[1][0] = a[0][1]; \ + b[1][1] = a[1][1]; \ + } /*! matrix transpose */ -#define TRANSPOSE_MATRIX_3X3(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[1][0]; \ - b[0][2] = a[2][0]; \ - \ - b[1][0] = a[0][1]; \ - b[1][1] = a[1][1]; \ - b[1][2] = a[2][1]; \ - \ - b[2][0] = a[0][2]; \ - b[2][1] = a[1][2]; \ - b[2][2] = a[2][2]; \ -}\ - +#define TRANSPOSE_MATRIX_3X3(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[1][0]; \ + b[0][2] = a[2][0]; \ + \ + b[1][0] = a[0][1]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[2][1]; \ + \ + b[2][0] = a[0][2]; \ + b[2][1] = a[1][2]; \ + b[2][2] = a[2][2]; \ + } /*! matrix transpose */ -#define TRANSPOSE_MATRIX_4X4(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[1][0]; \ - b[0][2] = a[2][0]; \ - b[0][3] = a[3][0]; \ - \ - b[1][0] = a[0][1]; \ - b[1][1] = a[1][1]; \ - b[1][2] = a[2][1]; \ - b[1][3] = a[3][1]; \ - \ - b[2][0] = a[0][2]; \ - b[2][1] = a[1][2]; \ - b[2][2] = a[2][2]; \ - b[2][3] = a[3][2]; \ - \ - b[3][0] = a[0][3]; \ - b[3][1] = a[1][3]; \ - b[3][2] = a[2][3]; \ - b[3][3] = a[3][3]; \ -}\ - +#define TRANSPOSE_MATRIX_4X4(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[1][0]; \ + b[0][2] = a[2][0]; \ + b[0][3] = a[3][0]; \ + \ + b[1][0] = a[0][1]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[2][1]; \ + b[1][3] = a[3][1]; \ + \ + b[2][0] = a[0][2]; \ + b[2][1] = a[1][2]; \ + b[2][2] = a[2][2]; \ + b[2][3] = a[3][2]; \ + \ + b[3][0] = a[0][3]; \ + b[3][1] = a[1][3]; \ + b[3][2] = a[2][3]; \ + b[3][3] = a[3][3]; \ + } /*! multiply matrix by scalar */ -#define SCALE_MATRIX_2X2(b,s,a) \ -{ \ - b[0][0] = (s) * a[0][0]; \ - b[0][1] = (s) * a[0][1]; \ - \ - b[1][0] = (s) * a[1][0]; \ - b[1][1] = (s) * a[1][1]; \ -}\ - +#define SCALE_MATRIX_2X2(b, s, a) \ + { \ + b[0][0] = (s)*a[0][0]; \ + b[0][1] = (s)*a[0][1]; \ + \ + b[1][0] = (s)*a[1][0]; \ + b[1][1] = (s)*a[1][1]; \ + } /*! multiply matrix by scalar */ -#define SCALE_MATRIX_3X3(b,s,a) \ -{ \ - b[0][0] = (s) * a[0][0]; \ - b[0][1] = (s) * a[0][1]; \ - b[0][2] = (s) * a[0][2]; \ - \ - b[1][0] = (s) * a[1][0]; \ - b[1][1] = (s) * a[1][1]; \ - b[1][2] = (s) * a[1][2]; \ - \ - b[2][0] = (s) * a[2][0]; \ - b[2][1] = (s) * a[2][1]; \ - b[2][2] = (s) * a[2][2]; \ -}\ - +#define SCALE_MATRIX_3X3(b, s, a) \ + { \ + b[0][0] = (s)*a[0][0]; \ + b[0][1] = (s)*a[0][1]; \ + b[0][2] = (s)*a[0][2]; \ + \ + b[1][0] = (s)*a[1][0]; \ + b[1][1] = (s)*a[1][1]; \ + b[1][2] = (s)*a[1][2]; \ + \ + b[2][0] = (s)*a[2][0]; \ + b[2][1] = (s)*a[2][1]; \ + b[2][2] = (s)*a[2][2]; \ + } /*! multiply matrix by scalar */ -#define SCALE_MATRIX_4X4(b,s,a) \ -{ \ - b[0][0] = (s) * a[0][0]; \ - b[0][1] = (s) * a[0][1]; \ - b[0][2] = (s) * a[0][2]; \ - b[0][3] = (s) * a[0][3]; \ - \ - b[1][0] = (s) * a[1][0]; \ - b[1][1] = (s) * a[1][1]; \ - b[1][2] = (s) * a[1][2]; \ - b[1][3] = (s) * a[1][3]; \ - \ - b[2][0] = (s) * a[2][0]; \ - b[2][1] = (s) * a[2][1]; \ - b[2][2] = (s) * a[2][2]; \ - b[2][3] = (s) * a[2][3]; \ - \ - b[3][0] = s * a[3][0]; \ - b[3][1] = s * a[3][1]; \ - b[3][2] = s * a[3][2]; \ - b[3][3] = s * a[3][3]; \ -}\ - +#define SCALE_MATRIX_4X4(b, s, a) \ + { \ + b[0][0] = (s)*a[0][0]; \ + b[0][1] = (s)*a[0][1]; \ + b[0][2] = (s)*a[0][2]; \ + b[0][3] = (s)*a[0][3]; \ + \ + b[1][0] = (s)*a[1][0]; \ + b[1][1] = (s)*a[1][1]; \ + b[1][2] = (s)*a[1][2]; \ + b[1][3] = (s)*a[1][3]; \ + \ + b[2][0] = (s)*a[2][0]; \ + b[2][1] = (s)*a[2][1]; \ + b[2][2] = (s)*a[2][2]; \ + b[2][3] = (s)*a[2][3]; \ + \ + b[3][0] = s * a[3][0]; \ + b[3][1] = s * a[3][1]; \ + b[3][2] = s * a[3][2]; \ + b[3][3] = s * a[3][3]; \ + } /*! multiply matrix by scalar */ -#define SCALE_VEC_MATRIX_2X2(b,svec,a) \ -{ \ - b[0][0] = svec[0] * a[0][0]; \ - b[1][0] = svec[0] * a[1][0]; \ - \ - b[0][1] = svec[1] * a[0][1]; \ - b[1][1] = svec[1] * a[1][1]; \ -}\ - +#define SCALE_VEC_MATRIX_2X2(b, svec, a) \ + { \ + b[0][0] = svec[0] * a[0][0]; \ + b[1][0] = svec[0] * a[1][0]; \ + \ + b[0][1] = svec[1] * a[0][1]; \ + b[1][1] = svec[1] * a[1][1]; \ + } /*! multiply matrix by scalar. Each columns is scaled by each scalar vector component */ -#define SCALE_VEC_MATRIX_3X3(b,svec,a) \ -{ \ - b[0][0] = svec[0] * a[0][0]; \ - b[1][0] = svec[0] * a[1][0]; \ - b[2][0] = svec[0] * a[2][0]; \ - \ - b[0][1] = svec[1] * a[0][1]; \ - b[1][1] = svec[1] * a[1][1]; \ - b[2][1] = svec[1] * a[2][1]; \ - \ - b[0][2] = svec[2] * a[0][2]; \ - b[1][2] = svec[2] * a[1][2]; \ - b[2][2] = svec[2] * a[2][2]; \ -}\ - +#define SCALE_VEC_MATRIX_3X3(b, svec, a) \ + { \ + b[0][0] = svec[0] * a[0][0]; \ + b[1][0] = svec[0] * a[1][0]; \ + b[2][0] = svec[0] * a[2][0]; \ + \ + b[0][1] = svec[1] * a[0][1]; \ + b[1][1] = svec[1] * a[1][1]; \ + b[2][1] = svec[1] * a[2][1]; \ + \ + b[0][2] = svec[2] * a[0][2]; \ + b[1][2] = svec[2] * a[1][2]; \ + b[2][2] = svec[2] * a[2][2]; \ + } /*! multiply matrix by scalar */ -#define SCALE_VEC_MATRIX_4X4(b,svec,a) \ -{ \ - b[0][0] = svec[0] * a[0][0]; \ - b[1][0] = svec[0] * a[1][0]; \ - b[2][0] = svec[0] * a[2][0]; \ - b[3][0] = svec[0] * a[3][0]; \ - \ - b[0][1] = svec[1] * a[0][1]; \ - b[1][1] = svec[1] * a[1][1]; \ - b[2][1] = svec[1] * a[2][1]; \ - b[3][1] = svec[1] * a[3][1]; \ - \ - b[0][2] = svec[2] * a[0][2]; \ - b[1][2] = svec[2] * a[1][2]; \ - b[2][2] = svec[2] * a[2][2]; \ - b[3][2] = svec[2] * a[3][2]; \ - \ - b[0][3] = svec[3] * a[0][3]; \ - b[1][3] = svec[3] * a[1][3]; \ - b[2][3] = svec[3] * a[2][3]; \ - b[3][3] = svec[3] * a[3][3]; \ -}\ - +#define SCALE_VEC_MATRIX_4X4(b, svec, a) \ + { \ + b[0][0] = svec[0] * a[0][0]; \ + b[1][0] = svec[0] * a[1][0]; \ + b[2][0] = svec[0] * a[2][0]; \ + b[3][0] = svec[0] * a[3][0]; \ + \ + b[0][1] = svec[1] * a[0][1]; \ + b[1][1] = svec[1] * a[1][1]; \ + b[2][1] = svec[1] * a[2][1]; \ + b[3][1] = svec[1] * a[3][1]; \ + \ + b[0][2] = svec[2] * a[0][2]; \ + b[1][2] = svec[2] * a[1][2]; \ + b[2][2] = svec[2] * a[2][2]; \ + b[3][2] = svec[2] * a[3][2]; \ + \ + b[0][3] = svec[3] * a[0][3]; \ + b[1][3] = svec[3] * a[1][3]; \ + b[2][3] = svec[3] * a[2][3]; \ + b[3][3] = svec[3] * a[3][3]; \ + } /*! multiply matrix by scalar */ -#define ACCUM_SCALE_MATRIX_2X2(b,s,a) \ -{ \ - b[0][0] += (s) * a[0][0]; \ - b[0][1] += (s) * a[0][1]; \ - \ - b[1][0] += (s) * a[1][0]; \ - b[1][1] += (s) * a[1][1]; \ -}\ - +#define ACCUM_SCALE_MATRIX_2X2(b, s, a) \ + { \ + b[0][0] += (s)*a[0][0]; \ + b[0][1] += (s)*a[0][1]; \ + \ + b[1][0] += (s)*a[1][0]; \ + b[1][1] += (s)*a[1][1]; \ + } /*! multiply matrix by scalar */ -#define ACCUM_SCALE_MATRIX_3X3(b,s,a) \ -{ \ - b[0][0] += (s) * a[0][0]; \ - b[0][1] += (s) * a[0][1]; \ - b[0][2] += (s) * a[0][2]; \ - \ - b[1][0] += (s) * a[1][0]; \ - b[1][1] += (s) * a[1][1]; \ - b[1][2] += (s) * a[1][2]; \ - \ - b[2][0] += (s) * a[2][0]; \ - b[2][1] += (s) * a[2][1]; \ - b[2][2] += (s) * a[2][2]; \ -}\ - +#define ACCUM_SCALE_MATRIX_3X3(b, s, a) \ + { \ + b[0][0] += (s)*a[0][0]; \ + b[0][1] += (s)*a[0][1]; \ + b[0][2] += (s)*a[0][2]; \ + \ + b[1][0] += (s)*a[1][0]; \ + b[1][1] += (s)*a[1][1]; \ + b[1][2] += (s)*a[1][2]; \ + \ + b[2][0] += (s)*a[2][0]; \ + b[2][1] += (s)*a[2][1]; \ + b[2][2] += (s)*a[2][2]; \ + } /*! multiply matrix by scalar */ -#define ACCUM_SCALE_MATRIX_4X4(b,s,a) \ -{ \ - b[0][0] += (s) * a[0][0]; \ - b[0][1] += (s) * a[0][1]; \ - b[0][2] += (s) * a[0][2]; \ - b[0][3] += (s) * a[0][3]; \ - \ - b[1][0] += (s) * a[1][0]; \ - b[1][1] += (s) * a[1][1]; \ - b[1][2] += (s) * a[1][2]; \ - b[1][3] += (s) * a[1][3]; \ - \ - b[2][0] += (s) * a[2][0]; \ - b[2][1] += (s) * a[2][1]; \ - b[2][2] += (s) * a[2][2]; \ - b[2][3] += (s) * a[2][3]; \ - \ - b[3][0] += (s) * a[3][0]; \ - b[3][1] += (s) * a[3][1]; \ - b[3][2] += (s) * a[3][2]; \ - b[3][3] += (s) * a[3][3]; \ -}\ +#define ACCUM_SCALE_MATRIX_4X4(b, s, a) \ + { \ + b[0][0] += (s)*a[0][0]; \ + b[0][1] += (s)*a[0][1]; \ + b[0][2] += (s)*a[0][2]; \ + b[0][3] += (s)*a[0][3]; \ + \ + b[1][0] += (s)*a[1][0]; \ + b[1][1] += (s)*a[1][1]; \ + b[1][2] += (s)*a[1][2]; \ + b[1][3] += (s)*a[1][3]; \ + \ + b[2][0] += (s)*a[2][0]; \ + b[2][1] += (s)*a[2][1]; \ + b[2][2] += (s)*a[2][2]; \ + b[2][3] += (s)*a[2][3]; \ + \ + b[3][0] += (s)*a[3][0]; \ + b[3][1] += (s)*a[3][1]; \ + b[3][2] += (s)*a[3][2]; \ + b[3][3] += (s)*a[3][3]; \ + } /*! matrix product */ /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ -#define MATRIX_PRODUCT_2X2(c,a,b) \ -{ \ - c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]; \ - c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]; \ - \ - c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]; \ - c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]; \ - \ -}\ +#define MATRIX_PRODUCT_2X2(c, a, b) \ + { \ + c[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0]; \ + c[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1]; \ + \ + c[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0]; \ + c[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1]; \ + } /*! matrix product */ /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ -#define MATRIX_PRODUCT_3X3(c,a,b) \ -{ \ - c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]; \ - c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]; \ - c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]; \ - \ - c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]; \ - c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]; \ - c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]; \ - \ - c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]; \ - c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]; \ - c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]; \ -}\ - +#define MATRIX_PRODUCT_3X3(c, a, b) \ + { \ + c[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0]; \ + c[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1]; \ + c[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2]; \ + \ + c[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0]; \ + c[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1]; \ + c[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2]; \ + \ + c[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0]; \ + c[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1]; \ + c[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2]; \ + } /*! matrix product */ /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ -#define MATRIX_PRODUCT_4X4(c,a,b) \ -{ \ - c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\ - c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\ - c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\ - c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\ - \ - c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\ - c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\ - c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\ - c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\ - \ - c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\ - c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\ - c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\ - c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\ - \ - c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\ - c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\ - c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\ - c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\ -}\ - +#define MATRIX_PRODUCT_4X4(c, a, b) \ + { \ + c[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0] + a[0][3] * b[3][0]; \ + c[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1] + a[0][3] * b[3][1]; \ + c[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2] + a[0][3] * b[3][2]; \ + c[0][3] = a[0][0] * b[0][3] + a[0][1] * b[1][3] + a[0][2] * b[2][3] + a[0][3] * b[3][3]; \ + \ + c[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0] + a[1][3] * b[3][0]; \ + c[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1] + a[1][3] * b[3][1]; \ + c[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2] + a[1][3] * b[3][2]; \ + c[1][3] = a[1][0] * b[0][3] + a[1][1] * b[1][3] + a[1][2] * b[2][3] + a[1][3] * b[3][3]; \ + \ + c[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0] + a[2][3] * b[3][0]; \ + c[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1] + a[2][3] * b[3][1]; \ + c[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2] + a[2][3] * b[3][2]; \ + c[2][3] = a[2][0] * b[0][3] + a[2][1] * b[1][3] + a[2][2] * b[2][3] + a[2][3] * b[3][3]; \ + \ + c[3][0] = a[3][0] * b[0][0] + a[3][1] * b[1][0] + a[3][2] * b[2][0] + a[3][3] * b[3][0]; \ + c[3][1] = a[3][0] * b[0][1] + a[3][1] * b[1][1] + a[3][2] * b[2][1] + a[3][3] * b[3][1]; \ + c[3][2] = a[3][0] * b[0][2] + a[3][1] * b[1][2] + a[3][2] * b[2][2] + a[3][3] * b[3][2]; \ + c[3][3] = a[3][0] * b[0][3] + a[3][1] * b[1][3] + a[3][2] * b[2][3] + a[3][3] * b[3][3]; \ + } /*! matrix times vector */ -#define MAT_DOT_VEC_2X2(p,m,v) \ -{ \ - p[0] = m[0][0]*v[0] + m[0][1]*v[1]; \ - p[1] = m[1][0]*v[0] + m[1][1]*v[1]; \ -}\ - +#define MAT_DOT_VEC_2X2(p, m, v) \ + { \ + p[0] = m[0][0] * v[0] + m[0][1] * v[1]; \ + p[1] = m[1][0] * v[0] + m[1][1] * v[1]; \ + } /*! matrix times vector */ -#define MAT_DOT_VEC_3X3(p,m,v) \ -{ \ - p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; \ - p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; \ - p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; \ -}\ - +#define MAT_DOT_VEC_3X3(p, m, v) \ + { \ + p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2]; \ + p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2]; \ + p[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2]; \ + } /*! matrix times vector v is a vec4f */ -#define MAT_DOT_VEC_4X4(p,m,v) \ -{ \ - p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3]; \ - p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3]; \ - p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3]; \ - p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3]; \ -}\ +#define MAT_DOT_VEC_4X4(p, m, v) \ + { \ + p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3] * v[3]; \ + p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3] * v[3]; \ + p[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3] * v[3]; \ + p[3] = m[3][0] * v[0] + m[3][1] * v[1] + m[3][2] * v[2] + m[3][3] * v[3]; \ + } /*! matrix times vector v is a vec3f and m is a mat4f
    Last column is added as the position */ -#define MAT_DOT_VEC_3X4(p,m,v) \ -{ \ - p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]; \ - p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]; \ - p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]; \ -}\ - +#define MAT_DOT_VEC_3X4(p, m, v) \ + { \ + p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3]; \ + p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3]; \ + p[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3]; \ + } /*! vector transpose times matrix */ /*! p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */ -#define VEC_DOT_MAT_3X3(p,v,m) \ -{ \ - p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0]; \ - p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1]; \ - p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2]; \ -}\ - +#define VEC_DOT_MAT_3X3(p, v, m) \ + { \ + p[0] = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0]; \ + p[1] = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1]; \ + p[2] = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2]; \ + } /*! affine matrix times vector */ /** The matrix is assumed to be an affine matrix, with last two * entries representing a translation */ -#define MAT_DOT_VEC_2X3(p,m,v) \ -{ \ - p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]; \ - p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]; \ -}\ +#define MAT_DOT_VEC_2X3(p, m, v) \ + { \ + p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2]; \ + p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2]; \ + } //! Transform a plane -#define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\ -{ \ - pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1] + m[0][2]*plane[2];\ - pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1] + m[1][2]*plane[2];\ - pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1] + m[2][2]*plane[2];\ - pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1] + m[2][3]*pout[2] + plane[3];\ -}\ - - +#define MAT_TRANSFORM_PLANE_4X4(pout, m, plane) \ + { \ + pout[0] = m[0][0] * plane[0] + m[0][1] * plane[1] + m[0][2] * plane[2]; \ + pout[1] = m[1][0] * plane[0] + m[1][1] * plane[1] + m[1][2] * plane[2]; \ + pout[2] = m[2][0] * plane[0] + m[2][1] * plane[1] + m[2][2] * plane[2]; \ + pout[3] = m[0][3] * pout[0] + m[1][3] * pout[1] + m[2][3] * pout[2] + plane[3]; \ + } /** inverse transpose of matrix times vector * @@ -1000,22 +938,22 @@ Last column is added as the position * It will leave normals the wrong length !!! * See macro below for use on normals. */ -#define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v) \ -{ \ - GREAL det; \ - \ - det = m[0][0]*m[1][1] - m[0][1]*m[1][0]; \ - p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ - p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ - \ - /* if matrix not singular, and not orthonormal, then renormalize */ \ - if ((det!=1.0f) && (det != 0.0f)) { \ - det = 1.0f / det; \ - p[0] *= det; \ - p[1] *= det; \ - } \ -}\ - +#define INV_TRANSP_MAT_DOT_VEC_2X2(p, m, v) \ + { \ + GREAL det; \ + \ + det = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \ + p[0] = m[1][1] * v[0] - m[1][0] * v[1]; \ + p[1] = -m[0][1] * v[0] + m[0][0] * v[1]; \ + \ + /* if matrix not singular, and not orthonormal, then renormalize */ \ + if ((det != 1.0f) && (det != 0.0f)) \ + { \ + det = 1.0f / det; \ + p[0] *= det; \ + p[1] *= det; \ + } \ + } /** transform normal vector by inverse transpose of matrix * and then renormalize the vector @@ -1024,550 +962,527 @@ Last column is added as the position * and multiplies vector v into it, to yeild vector p * Vector p is then normalized. */ -#define NORM_XFORM_2X2(p,m,v) \ -{ \ - GREAL len; \ - \ - /* do nothing if off-diagonals are zero and diagonals are \ - * equal */ \ - if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \ - p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ - p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ - \ - len = p[0]*p[0] + p[1]*p[1]; \ - GIM_INV_SQRT(len,len); \ - p[0] *= len; \ - p[1] *= len; \ - } else { \ - VEC_COPY_2 (p, v); \ - } \ -}\ - +#define NORM_XFORM_2X2(p, m, v) \ + { \ + GREAL len; \ + \ + /* do nothing if off-diagonals are zero and diagonals are \ + * equal */ \ + if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) \ + { \ + p[0] = m[1][1] * v[0] - m[1][0] * v[1]; \ + p[1] = -m[0][1] * v[0] + m[0][0] * v[1]; \ + \ + len = p[0] * p[0] + p[1] * p[1]; \ + GIM_INV_SQRT(len, len); \ + p[0] *= len; \ + p[1] *= len; \ + } \ + else \ + { \ + VEC_COPY_2(p, v); \ + } \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define OUTER_PRODUCT_2X2(m,v,t) \ -{ \ - m[0][0] = v[0] * t[0]; \ - m[0][1] = v[0] * t[1]; \ - \ - m[1][0] = v[1] * t[0]; \ - m[1][1] = v[1] * t[1]; \ -}\ - +#define OUTER_PRODUCT_2X2(m, v, t) \ + { \ + m[0][0] = v[0] * t[0]; \ + m[0][1] = v[0] * t[1]; \ + \ + m[1][0] = v[1] * t[0]; \ + m[1][1] = v[1] * t[1]; \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define OUTER_PRODUCT_3X3(m,v,t) \ -{ \ - m[0][0] = v[0] * t[0]; \ - m[0][1] = v[0] * t[1]; \ - m[0][2] = v[0] * t[2]; \ - \ - m[1][0] = v[1] * t[0]; \ - m[1][1] = v[1] * t[1]; \ - m[1][2] = v[1] * t[2]; \ - \ - m[2][0] = v[2] * t[0]; \ - m[2][1] = v[2] * t[1]; \ - m[2][2] = v[2] * t[2]; \ -}\ - +#define OUTER_PRODUCT_3X3(m, v, t) \ + { \ + m[0][0] = v[0] * t[0]; \ + m[0][1] = v[0] * t[1]; \ + m[0][2] = v[0] * t[2]; \ + \ + m[1][0] = v[1] * t[0]; \ + m[1][1] = v[1] * t[1]; \ + m[1][2] = v[1] * t[2]; \ + \ + m[2][0] = v[2] * t[0]; \ + m[2][1] = v[2] * t[1]; \ + m[2][2] = v[2] * t[2]; \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define OUTER_PRODUCT_4X4(m,v,t) \ -{ \ - m[0][0] = v[0] * t[0]; \ - m[0][1] = v[0] * t[1]; \ - m[0][2] = v[0] * t[2]; \ - m[0][3] = v[0] * t[3]; \ - \ - m[1][0] = v[1] * t[0]; \ - m[1][1] = v[1] * t[1]; \ - m[1][2] = v[1] * t[2]; \ - m[1][3] = v[1] * t[3]; \ - \ - m[2][0] = v[2] * t[0]; \ - m[2][1] = v[2] * t[1]; \ - m[2][2] = v[2] * t[2]; \ - m[2][3] = v[2] * t[3]; \ - \ - m[3][0] = v[3] * t[0]; \ - m[3][1] = v[3] * t[1]; \ - m[3][2] = v[3] * t[2]; \ - m[3][3] = v[3] * t[3]; \ -}\ - +#define OUTER_PRODUCT_4X4(m, v, t) \ + { \ + m[0][0] = v[0] * t[0]; \ + m[0][1] = v[0] * t[1]; \ + m[0][2] = v[0] * t[2]; \ + m[0][3] = v[0] * t[3]; \ + \ + m[1][0] = v[1] * t[0]; \ + m[1][1] = v[1] * t[1]; \ + m[1][2] = v[1] * t[2]; \ + m[1][3] = v[1] * t[3]; \ + \ + m[2][0] = v[2] * t[0]; \ + m[2][1] = v[2] * t[1]; \ + m[2][2] = v[2] * t[2]; \ + m[2][3] = v[2] * t[3]; \ + \ + m[3][0] = v[3] * t[0]; \ + m[3][1] = v[3] * t[1]; \ + m[3][2] = v[3] * t[2]; \ + m[3][3] = v[3] * t[3]; \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define ACCUM_OUTER_PRODUCT_2X2(m,v,t) \ -{ \ - m[0][0] += v[0] * t[0]; \ - m[0][1] += v[0] * t[1]; \ - \ - m[1][0] += v[1] * t[0]; \ - m[1][1] += v[1] * t[1]; \ -}\ - +#define ACCUM_OUTER_PRODUCT_2X2(m, v, t) \ + { \ + m[0][0] += v[0] * t[0]; \ + m[0][1] += v[0] * t[1]; \ + \ + m[1][0] += v[1] * t[0]; \ + m[1][1] += v[1] * t[1]; \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define ACCUM_OUTER_PRODUCT_3X3(m,v,t) \ -{ \ - m[0][0] += v[0] * t[0]; \ - m[0][1] += v[0] * t[1]; \ - m[0][2] += v[0] * t[2]; \ - \ - m[1][0] += v[1] * t[0]; \ - m[1][1] += v[1] * t[1]; \ - m[1][2] += v[1] * t[2]; \ - \ - m[2][0] += v[2] * t[0]; \ - m[2][1] += v[2] * t[1]; \ - m[2][2] += v[2] * t[2]; \ -}\ - +#define ACCUM_OUTER_PRODUCT_3X3(m, v, t) \ + { \ + m[0][0] += v[0] * t[0]; \ + m[0][1] += v[0] * t[1]; \ + m[0][2] += v[0] * t[2]; \ + \ + m[1][0] += v[1] * t[0]; \ + m[1][1] += v[1] * t[1]; \ + m[1][2] += v[1] * t[2]; \ + \ + m[2][0] += v[2] * t[0]; \ + m[2][1] += v[2] * t[1]; \ + m[2][2] += v[2] * t[2]; \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define ACCUM_OUTER_PRODUCT_4X4(m,v,t) \ -{ \ - m[0][0] += v[0] * t[0]; \ - m[0][1] += v[0] * t[1]; \ - m[0][2] += v[0] * t[2]; \ - m[0][3] += v[0] * t[3]; \ - \ - m[1][0] += v[1] * t[0]; \ - m[1][1] += v[1] * t[1]; \ - m[1][2] += v[1] * t[2]; \ - m[1][3] += v[1] * t[3]; \ - \ - m[2][0] += v[2] * t[0]; \ - m[2][1] += v[2] * t[1]; \ - m[2][2] += v[2] * t[2]; \ - m[2][3] += v[2] * t[3]; \ - \ - m[3][0] += v[3] * t[0]; \ - m[3][1] += v[3] * t[1]; \ - m[3][2] += v[3] * t[2]; \ - m[3][3] += v[3] * t[3]; \ -}\ - +#define ACCUM_OUTER_PRODUCT_4X4(m, v, t) \ + { \ + m[0][0] += v[0] * t[0]; \ + m[0][1] += v[0] * t[1]; \ + m[0][2] += v[0] * t[2]; \ + m[0][3] += v[0] * t[3]; \ + \ + m[1][0] += v[1] * t[0]; \ + m[1][1] += v[1] * t[1]; \ + m[1][2] += v[1] * t[2]; \ + m[1][3] += v[1] * t[3]; \ + \ + m[2][0] += v[2] * t[0]; \ + m[2][1] += v[2] * t[1]; \ + m[2][2] += v[2] * t[2]; \ + m[2][3] += v[2] * t[3]; \ + \ + m[3][0] += v[3] * t[0]; \ + m[3][1] += v[3] * t[1]; \ + m[3][2] += v[3] * t[2]; \ + m[3][3] += v[3] * t[3]; \ + } /** determinant of matrix * * Computes determinant of matrix m, returning d */ -#define DETERMINANT_2X2(d,m) \ -{ \ - d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \ -}\ - +#define DETERMINANT_2X2(d, m) \ + { \ + d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \ + } /** determinant of matrix * * Computes determinant of matrix m, returning d */ -#define DETERMINANT_3X3(d,m) \ -{ \ - d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]); \ - d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]); \ - d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]); \ -}\ - +#define DETERMINANT_3X3(d, m) \ + { \ + d = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); \ + d -= m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]); \ + d += m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \ + } /** i,j,th cofactor of a 4x4 matrix * */ -#define COFACTOR_4X4_IJ(fac,m,i,j) \ -{ \ - GUINT __ii[4], __jj[4], __k; \ - \ - for (__k=0; __k */ -#define INV_MAT_DOT_VEC_3X3(p,m,v) \ -{ \ - p[0] = MAT_DOT_COL(m,v,0); \ - p[1] = MAT_DOT_COL(m,v,1); \ - p[2] = MAT_DOT_COL(m,v,2); \ -}\ +#define INV_MAT_DOT_VEC_3X3(p, m, v) \ + { \ + p[0] = MAT_DOT_COL(m, v, 0); \ + p[1] = MAT_DOT_COL(m, v, 1); \ + p[2] = MAT_DOT_COL(m, v, 2); \ + } - - -#endif // GIM_VECTOR_H_INCLUDED +#endif // GIM_VECTOR_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_math.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_math.h index 939079e10..3c4f821a7 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_math.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_math.h @@ -34,8 +34,6 @@ email: projectileman@yahoo.com #include "LinearMath/btScalar.h" - - #define GREAL btScalar #define GREAL2 double #define GINT int @@ -45,8 +43,6 @@ email: projectileman@yahoo.com #define GINT64 long long #define GUINT64 unsigned long long - - #define G_PI 3.14159265358979f #define G_HALF_PI 1.5707963f //267948966 @@ -54,16 +50,14 @@ email: projectileman@yahoo.com //71795864 #define G_ROOT3 1.73205f #define G_ROOT2 1.41421f -#define G_UINT_INFINITY 0xffffffff //!< A very very high value +#define G_UINT_INFINITY 0xffffffff //!< A very very high value #define G_REAL_INFINITY FLT_MAX -#define G_SIGN_BITMASK 0x80000000 +#define G_SIGN_BITMASK 0x80000000 #define G_EPSILON SIMD_EPSILON - - enum GIM_SCALAR_TYPES { - G_STYPE_REAL =0, + G_STYPE_REAL = 0, G_STYPE_REAL2, G_STYPE_SHORT, G_STYPE_USHORT, @@ -73,85 +67,82 @@ enum GIM_SCALAR_TYPES G_STYPE_UINT64 }; - - -#define G_DEGTORAD(X) ((X)*3.1415926f/180.0f) -#define G_RADTODEG(X) ((X)*180.0f/3.1415926f) +#define G_DEGTORAD(X) ((X)*3.1415926f / 180.0f) +#define G_RADTODEG(X) ((X)*180.0f / 3.1415926f) //! Integer representation of a floating-point value. -#define GIM_IR(x) ((GUINT&)(x)) +#define GIM_IR(x) ((GUINT&)(x)) //! Signed integer representation of a floating-point value. -#define GIM_SIR(x) ((GINT&)(x)) +#define GIM_SIR(x) ((GINT&)(x)) //! Absolute integer representation of a floating-point value -#define GIM_AIR(x) (GIM_IR(x)&0x7fffffff) +#define GIM_AIR(x) (GIM_IR(x) & 0x7fffffff) //! Floating-point representation of an integer value. -#define GIM_FR(x) ((GREAL&)(x)) +#define GIM_FR(x) ((GREAL&)(x)) -#define GIM_MAX(a,b) (ab?b:a) +#define GIM_MAX(a, b) (a < b ? b : a) +#define GIM_MIN(a, b) (a > b ? b : a) -#define GIM_MAX3(a,b,c) GIM_MAX(a,GIM_MAX(b,c)) -#define GIM_MIN3(a,b,c) GIM_MIN(a,GIM_MIN(b,c)) +#define GIM_MAX3(a, b, c) GIM_MAX(a, GIM_MAX(b, c)) +#define GIM_MIN3(a, b, c) GIM_MIN(a, GIM_MIN(b, c)) -#define GIM_IS_ZERO(value) (value < G_EPSILON && value > -G_EPSILON) +#define GIM_IS_ZERO(value) (value < G_EPSILON && value > -G_EPSILON) #define GIM_IS_NEGATIVE(value) (value <= -G_EPSILON) #define GIM_IS_POSISITVE(value) (value >= G_EPSILON) -#define GIM_NEAR_EQUAL(v1,v2) GIM_IS_ZERO((v1-v2)) +#define GIM_NEAR_EQUAL(v1, v2) GIM_IS_ZERO((v1 - v2)) ///returns a clamped number -#define GIM_CLAMP(number,minval,maxval) (numbermaxval?maxval:number)) +#define GIM_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number)) -#define GIM_GREATER(x, y) btFabs(x) > (y) +#define GIM_GREATER(x, y) btFabs(x) > (y) ///Swap numbers -#define GIM_SWAP_NUMBERS(a,b){ \ - a = a+b; \ - b = a-b; \ - a = a-b; \ -}\ +#define GIM_SWAP_NUMBERS(a, b) \ + { \ + a = a + b; \ + b = a - b; \ + a = a - b; \ + } -#define GIM_INV_SQRT(va,isva)\ -{\ - if(va<=0.0000001f)\ - {\ - isva = G_REAL_INFINITY;\ - }\ - else\ - {\ - GREAL _x = va * 0.5f;\ - GUINT _y = 0x5f3759df - ( GIM_IR(va) >> 1);\ - isva = GIM_FR(_y);\ - isva = isva * ( 1.5f - ( _x * isva * isva ) );\ - }\ -}\ +#define GIM_INV_SQRT(va, isva) \ + { \ + if (va <= 0.0000001f) \ + { \ + isva = G_REAL_INFINITY; \ + } \ + else \ + { \ + GREAL _x = va * 0.5f; \ + GUINT _y = 0x5f3759df - (GIM_IR(va) >> 1); \ + isva = GIM_FR(_y); \ + isva = isva * (1.5f - (_x * isva * isva)); \ + } \ + } -#define GIM_SQRT(va,sva)\ -{\ - GIM_INV_SQRT(va,sva);\ - sva = 1.0f/sva;\ -}\ +#define GIM_SQRT(va, sva) \ + { \ + GIM_INV_SQRT(va, sva); \ + sva = 1.0f / sva; \ + } //! Computes 1.0f / sqrtf(x). Comes from Quake3. See http://www.magic-software.com/3DGEDInvSqrt.html inline GREAL gim_inv_sqrt(GREAL f) { - GREAL r; - GIM_INV_SQRT(f,r); - return r; + GREAL r; + GIM_INV_SQRT(f, r); + return r; } inline GREAL gim_sqrt(GREAL f) { - GREAL r; - GIM_SQRT(f,r); - return r; + GREAL r; + GIM_SQRT(f, r); + return r; } - - -#endif // GIM_MATH_H_INCLUDED +#endif // GIM_MATH_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_memory.cpp b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_memory.cpp index 1636eb786..9e29ab91d 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_memory.cpp +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_memory.cpp @@ -27,7 +27,6 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_memory.h" #include "stdlib.h" @@ -40,52 +39,49 @@ static gim_alloca_function *g_allocafn = 0; static gim_realloc_function *g_reallocfn = 0; static gim_free_function *g_freefn = 0; -void gim_set_alloc_handler (gim_alloc_function *fn) +void gim_set_alloc_handler(gim_alloc_function *fn) { - g_allocfn = fn; + g_allocfn = fn; } -void gim_set_alloca_handler (gim_alloca_function *fn) +void gim_set_alloca_handler(gim_alloca_function *fn) { - g_allocafn = fn; + g_allocafn = fn; } -void gim_set_realloc_handler (gim_realloc_function *fn) +void gim_set_realloc_handler(gim_realloc_function *fn) { - g_reallocfn = fn; + g_reallocfn = fn; } -void gim_set_free_handler (gim_free_function *fn) +void gim_set_free_handler(gim_free_function *fn) { - g_freefn = fn; + g_freefn = fn; } gim_alloc_function *gim_get_alloc_handler() { - return g_allocfn; + return g_allocfn; } gim_alloca_function *gim_get_alloca_handler() { - return g_allocafn; + return g_allocafn; } - -gim_realloc_function *gim_get_realloc_handler () +gim_realloc_function *gim_get_realloc_handler() { - return g_reallocfn; + return g_reallocfn; } - -gim_free_function *gim_get_free_handler () +gim_free_function *gim_get_free_handler() { - return g_freefn; + return g_freefn; } - -void * gim_alloc(size_t size) +void *gim_alloc(size_t size) { - void * ptr; + void *ptr; if (g_allocfn) { ptr = g_allocfn(size); @@ -93,27 +89,29 @@ void * gim_alloc(size_t size) else { #ifdef GIM_SIMD_MEMORY - ptr = btAlignedAlloc(size,16); + ptr = btAlignedAlloc(size, 16); #else ptr = malloc(size); #endif } - return ptr; + return ptr; } -void * gim_alloca(size_t size) +void *gim_alloca(size_t size) { - if (g_allocafn) return g_allocafn(size); else return gim_alloc(size); + if (g_allocafn) + return g_allocafn(size); + else + return gim_alloc(size); } - -void * gim_realloc(void *ptr, size_t oldsize, size_t newsize) +void *gim_realloc(void *ptr, size_t oldsize, size_t newsize) { - void * newptr = gim_alloc(newsize); - size_t copysize = oldsize #ifdef PREFETCH -#include // for prefetch -#define pfval 64 -#define pfval2 128 +#include // for prefetch +#define pfval 64 +#define pfval2 128 //! Prefetch 64 -#define pf(_x,_i) _mm_prefetch((void *)(_x + _i + pfval), 0) +#define pf(_x, _i) _mm_prefetch((void *)(_x + _i + pfval), 0) //! Prefetch 128 -#define pf2(_x,_i) _mm_prefetch((void *)(_x + _i + pfval2), 0) +#define pf2(_x, _i) _mm_prefetch((void *)(_x + _i + pfval2), 0) #else //! Prefetch 64 -#define pf(_x,_i) +#define pf(_x, _i) //! Prefetch 128 -#define pf2(_x,_i) +#define pf2(_x, _i) #endif - ///Functions for manip packed arrays of numbers -#define GIM_COPY_ARRAYS(dest_array,source_array,element_count)\ -{\ - for (GUINT _i_=0;_i_0) - { - *(c_dst_ptr++) = *(c_src_ptr++); - copysize--; - } - return; + char *c_src_ptr = (char *)src; + char *c_dst_ptr = (char *)dst; + while (copysize > 0) + { + *(c_dst_ptr++) = *(c_src_ptr++); + copysize--; + } + return; #else - memcpy(dst,src,copysize); + memcpy(dst, src, copysize); #endif } - - -template -inline void gim_swap_elements(T* _array,size_t _i,size_t _j) +template +inline void gim_swap_elements(T *_array, size_t _i, size_t _j) { T _e_tmp_ = _array[_i]; _array[_i] = _array[_j]; _array[_j] = _e_tmp_; } - -template -inline void gim_swap_elements_memcpy(T* _array,size_t _i,size_t _j) +template +inline void gim_swap_elements_memcpy(T *_array, size_t _i, size_t _j) { char _e_tmp_[sizeof(T)]; - gim_simd_memcpy(_e_tmp_,&_array[_i],sizeof(T)); - gim_simd_memcpy(&_array[_i],&_array[_j],sizeof(T)); - gim_simd_memcpy(&_array[_j],_e_tmp_,sizeof(T)); + gim_simd_memcpy(_e_tmp_, &_array[_i], sizeof(T)); + gim_simd_memcpy(&_array[_i], &_array[_j], sizeof(T)); + gim_simd_memcpy(&_array[_j], _e_tmp_, sizeof(T)); } template -inline void gim_swap_elements_ptr(char * _array,size_t _i,size_t _j) +inline void gim_swap_elements_ptr(char *_array, size_t _i, size_t _j) { char _e_tmp_[SIZE]; - _i*=SIZE; - _j*=SIZE; - gim_simd_memcpy(_e_tmp_,_array+_i,SIZE); - gim_simd_memcpy(_array+_i,_array+_j,SIZE); - gim_simd_memcpy(_array+_j,_e_tmp_,SIZE); + _i *= SIZE; + _j *= SIZE; + gim_simd_memcpy(_e_tmp_, _array + _i, SIZE); + gim_simd_memcpy(_array + _i, _array + _j, SIZE); + gim_simd_memcpy(_array + _j, _e_tmp_, SIZE); } -#endif // GIM_MEMORY_H_INCLUDED +#endif // GIM_MEMORY_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_pair.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_pair.h new file mode 100644 index 000000000..56c185a5d --- /dev/null +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_pair.h @@ -0,0 +1,28 @@ +#ifndef GIM_PAIR_H +#define GIM_PAIR_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; + } +}; + +#endif //GIM_PAIR_H + diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_radixsort.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_radixsort.h index c246ef125..ff7907adc 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_radixsort.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_radixsort.h @@ -40,24 +40,22 @@ email: projectileman@yahoo.com //! Prototype for comparators class less_comparator { - public: - - template - inline int operator() ( const T& a, const Z& b ) +public: + template + inline int operator()(const T& a, const Z& b) { - return ( ab?1:0)); + return (a < b ? -1 : (a > b ? 1 : 0)); } }; //! Prototype for comparators class integer_comparator { - public: - - template - inline int operator() ( const T& a, const T& b ) +public: + template + inline int operator()(const T& a, const T& b) { - return (int)(a-b); + return (int)(a - b); } }; @@ -65,20 +63,19 @@ class integer_comparator class uint_key_func { public: - template - inline GUINT operator()( const T& a) + template + inline GUINT operator()(const T& a) { return (GUINT)a; } }; - //!Prototype for copying elements class copy_elements_func { public: - template - inline void operator()(T& a,T& b) + template + inline void operator()(T& a, T& b) { a = b; } @@ -88,34 +85,33 @@ public: class memcopy_elements_func { public: - template - inline void operator()(T& a,T& b) + template + inline void operator()(T& a, T& b) { - gim_simd_memcpy(&a,&b,sizeof(T)); + gim_simd_memcpy(&a, &b, sizeof(T)); } }; - //! @{ struct GIM_RSORT_TOKEN { - GUINT m_key; - GUINT m_value; - GIM_RSORT_TOKEN() - { - } - GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken) - { - m_key = rtoken.m_key; - m_value = rtoken.m_value; - } + GUINT m_key; + GUINT m_value; + GIM_RSORT_TOKEN() + { + } + GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken) + { + m_key = rtoken.m_key; + m_value = rtoken.m_value; + } - inline bool operator <(const GIM_RSORT_TOKEN& other) const + inline bool operator<(const GIM_RSORT_TOKEN& other) const { return (m_key < other.m_key); } - inline bool operator >(const GIM_RSORT_TOKEN& other) const + inline bool operator>(const GIM_RSORT_TOKEN& other) const { return (m_key > other.m_key); } @@ -124,33 +120,28 @@ struct GIM_RSORT_TOKEN //! Prototype for comparators class GIM_RSORT_TOKEN_COMPARATOR { - public: - - inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b ) +public: + inline int operator()(const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b) { return (int)((a.m_key) - (b.m_key)); } }; - - #define kHist 2048 // ---- utils for accessing 11-bit quantities -#define D11_0(x) (x & 0x7FF) -#define D11_1(x) (x >> 11 & 0x7FF) -#define D11_2(x) (x >> 22 ) - - +#define D11_0(x) (x & 0x7FF) +#define D11_1(x) (x >> 11 & 0x7FF) +#define D11_2(x) (x >> 22) ///Radix sort for unsigned integer keys inline void gim_radix_sort_rtokens( - GIM_RSORT_TOKEN * array, - GIM_RSORT_TOKEN * sorted, GUINT element_count) + GIM_RSORT_TOKEN* array, + GIM_RSORT_TOKEN* sorted, GUINT element_count) { GUINT i; GUINT b0[kHist * 3]; - GUINT *b1 = b0 + kHist; - GUINT *b2 = b1 + kHist; + GUINT* b1 = b0 + kHist; + GUINT* b2 = b1 + kHist; for (i = 0; i < kHist * 3; ++i) { b0[i] = 0; @@ -159,10 +150,10 @@ inline void gim_radix_sort_rtokens( GUINT pos; for (i = 0; i < element_count; ++i) { - fi = array[i].m_key; - b0[D11_0(fi)] ++; - b1[D11_1(fi)] ++; - b2[D11_2(fi)] ++; + fi = array[i].m_key; + b0[D11_0(fi)]++; + b1[D11_1(fi)]++; + b2[D11_2(fi)]++; } { GUINT sum0 = 0, sum1 = 0, sum2 = 0; @@ -182,7 +173,7 @@ inline void gim_radix_sort_rtokens( } for (i = 0; i < element_count; ++i) { - fi = array[i].m_key; + fi = array[i].m_key; pos = D11_0(fi); pos = ++b0[pos]; sorted[pos].m_key = array[i].m_key; @@ -190,7 +181,7 @@ inline void gim_radix_sort_rtokens( } for (i = 0; i < element_count; ++i) { - fi = sorted[i].m_key; + fi = sorted[i].m_key; pos = D11_1(fi); pos = ++b1[pos]; array[pos].m_key = sorted[i].m_key; @@ -198,7 +189,7 @@ inline void gim_radix_sort_rtokens( } for (i = 0; i < element_count; ++i) { - fi = array[i].m_key; + fi = array[i].m_key; pos = D11_2(fi); pos = ++b2[pos]; sorted[pos].m_key = array[i].m_key; @@ -206,9 +197,6 @@ inline void gim_radix_sort_rtokens( } } - - - /// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN /*! *\param array Array of elements to sort @@ -216,21 +204,21 @@ inline void gim_radix_sort_rtokens( *\param element_count element count *\param uintkey_macro Functor which retrieves the integer representation of an array element */ -template +template void gim_radix_sort_array_tokens( - T* array , - GIM_RSORT_TOKEN * sorted_tokens, - GUINT element_count,GETKEY_CLASS uintkey_macro) + T* array, + GIM_RSORT_TOKEN* sorted_tokens, + GUINT element_count, GETKEY_CLASS uintkey_macro) { - GIM_RSORT_TOKEN * _unsorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count); - for (GUINT _i=0;_i +template void gim_radix_sort( - T * array, GUINT element_count, + T* array, GUINT element_count, GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro) { - GIM_RSORT_TOKEN * _sorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count); - gim_radix_sort_array_tokens(array,_sorted,element_count,get_uintkey_macro); - T * _original_array = (T *) gim_alloc(sizeof(T)*element_count); - gim_simd_memcpy(_original_array,array,sizeof(T)*element_count); - for (GUINT _i=0;_i -bool gim_binary_search_ex( - const T* _array, GUINT _start_i, - GUINT _end_i,GUINT & _result_index, - const KEYCLASS & _search_key, - COMP_CLASS _comp_macro) +template +bool gim_binary_search_ex( + const T* _array, GUINT _start_i, + GUINT _end_i, GUINT& _result_index, + const KEYCLASS& _search_key, + COMP_CLASS _comp_macro) { GUINT _k; int _comp_result; GUINT _i = _start_i; - GUINT _j = _end_i+1; + GUINT _j = _end_i + 1; while (_i < _j) { - _k = (_j+_i-1)/2; + _k = (_j + _i - 1) / 2; _comp_result = _comp_macro(_array[_k], _search_key); if (_comp_result == 0) { @@ -291,7 +279,7 @@ bool gim_binary_search_ex( } else if (_comp_result < 0) { - _i = _k+1; + _i = _k + 1; } else { @@ -302,8 +290,6 @@ bool gim_binary_search_ex( return false; } - - //! Failsafe Iterative binary search,Template version /*! If the element is not found, it returns the nearest upper element position, may be the further position after the last element. @@ -314,26 +300,26 @@ If the element is not found, it returns the nearest upper element position, may \param _result_index the index of the found element, or if not found then it will get the index of the closest bigger value \return true if found, else false */ -template +template bool gim_binary_search( - const T*_array,GUINT _start_i, - GUINT _end_i,const T & _search_key, - GUINT & _result_index) + const T* _array, GUINT _start_i, + GUINT _end_i, const T& _search_key, + GUINT& _result_index) { GUINT _i = _start_i; - GUINT _j = _end_i+1; + GUINT _j = _end_i + 1; GUINT _k; - while(_i < _j) + while (_i < _j) { - _k = (_j+_i-1)/2; - if(_array[_k]==_search_key) + _k = (_j + _i - 1) / 2; + if (_array[_k] == _search_key) { _result_index = _k; return true; } - else if (_array[_k]<_search_key) + else if (_array[_k] < _search_key) { - _i = _k+1; + _i = _k + 1; } else { @@ -344,27 +330,25 @@ bool gim_binary_search( return false; } - - ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ template -void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc) +void gim_down_heap(T* pArr, GUINT k, GUINT n, COMP_CLASS 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) + while (k <= n / 2) { - int child = 2*k; + int child = 2 * k; - if ((child < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0) + if ((child < (int)n) && CompareFunc(pArr[child - 1], pArr[child]) < 0) { child++; } /* pick larger child */ - if (CompareFunc(temp , pArr[child - 1])<0) + if (CompareFunc(temp, pArr[child - 1]) < 0) { /* move child up */ pArr[k - 1] = pArr[child - 1]; @@ -378,29 +362,25 @@ void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc) pArr[k - 1] = temp; } /*downHeap*/ - template -void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc) +void gim_heap_sort(T* pArr, GUINT element_count, COMP_CLASS CompareFunc) { /* sort a[0..N-1], N.B. 0 to N-1 */ GUINT k; GUINT n = element_count; - for (k = n/2; k > 0; k--) + for (k = n / 2; k > 0; k--) { gim_down_heap(pArr, k, n, CompareFunc); } /* a[1..N] is now a heap */ - while ( n>=2 ) + while (n >= 2) { - gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */ + gim_swap_elements(pArr, 0, n - 1); /* largest of a[0..n-1] */ --n; /* restore a[1..i-1] heap */ gim_down_heap(pArr, 1, n, CompareFunc); } } - - - -#endif // GIM_RADIXSORT_H_INCLUDED +#endif // GIM_RADIXSORT_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_tri_collision.cpp b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_tri_collision.cpp index f9727e1d5..8d83e95da 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_tri_collision.cpp +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_tri_collision.cpp @@ -33,15 +33,13 @@ email: projectileman@yahoo.com #include "gim_tri_collision.h" - #define TRI_LOCAL_EPSILON 0.000001f #define MIN_EDGE_EDGE_DIS 0.00001f - class GIM_TRIANGLE_CALCULATION_CACHE { public: - GREAL margin; + GREAL margin; btVector3 tu_vertices[3]; btVector3 tv_vertices[3]; btVector4 tu_plane; @@ -55,46 +53,47 @@ public: GREAL du0du2; GREAL dv[4]; GREAL dv0dv1; - GREAL dv0dv2; + GREAL dv0dv2; btVector3 temp_points[MAX_TRI_CLIPPING]; btVector3 temp_points1[MAX_TRI_CLIPPING]; btVector3 contact_points[MAX_TRI_CLIPPING]; - - //! if returns false, the faces are paralele SIMD_FORCE_INLINE bool compute_intervals( - const GREAL &D0, - const GREAL &D1, - const GREAL &D2, - const GREAL &D0D1, - const GREAL &D0D2, - GREAL & scale_edge0, - GREAL & scale_edge1, - GUINT &edge_index0, - GUINT &edge_index1) + const GREAL &D0, + const GREAL &D1, + const GREAL &D2, + const GREAL &D0D1, + const GREAL &D0D2, + GREAL &scale_edge0, + GREAL &scale_edge1, + GUINT &edge_index0, + GUINT &edge_index1) { - if(D0D1>0.0f) + if (D0D1 > 0.0f) { /* here we know that D0D2<=0.0 */ /* that is D0, D1 are on the same side, D2 on the other or on the plane */ - scale_edge0 = -D2/(D0-D2); - scale_edge1 = -D1/(D2-D1); - edge_index0 = 2;edge_index1 = 1; + scale_edge0 = -D2 / (D0 - D2); + scale_edge1 = -D1 / (D2 - D1); + edge_index0 = 2; + edge_index1 = 1; } - else if(D0D2>0.0f) + else if (D0D2 > 0.0f) { /* here we know that d0d1<=0.0 */ - scale_edge0 = -D0/(D1-D0); - scale_edge1 = -D1/(D2-D1); - edge_index0 = 0;edge_index1 = 1; + scale_edge0 = -D0 / (D1 - D0); + scale_edge1 = -D1 / (D2 - D1); + edge_index0 = 0; + edge_index1 = 1; } - else if(D1*D2>0.0f || D0!=0.0f) + else if (D1 * D2 > 0.0f || D0 != 0.0f) { /* here we know that d0d1<=0.0 or that D0!=0.0 */ - scale_edge0 = -D0/(D1-D0); - scale_edge1 = -D2/(D0-D2); - edge_index0 = 0 ;edge_index1 = 2; + scale_edge0 = -D0 / (D1 - D0); + scale_edge1 = -D2 / (D0 - D2); + edge_index0 = 0; + edge_index1 = 2; } else { @@ -103,46 +102,44 @@ public: return true; } - //! clip triangle /*! */ SIMD_FORCE_INLINE GUINT clip_triangle( - const btVector4 & tri_plane, - const btVector3 * tripoints, - const btVector3 * srcpoints, - btVector3 * clip_points) + const btVector4 &tri_plane, + const btVector3 *tripoints, + const btVector3 *srcpoints, + btVector3 *clip_points) { // edge 0 btVector4 edgeplane; - EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane); + EDGE_PLANE(tripoints[0], tripoints[1], tri_plane, edgeplane); GUINT clipped_count = PLANE_CLIP_TRIANGLE3D( - edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points); + edgeplane, srcpoints[0], srcpoints[1], srcpoints[2], temp_points); - if(clipped_count == 0) return 0; + if (clipped_count == 0) return 0; // edge 1 - EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane); + EDGE_PLANE(tripoints[1], tripoints[2], tri_plane, edgeplane); clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points,clipped_count,temp_points1); + edgeplane, temp_points, clipped_count, temp_points1); - if(clipped_count == 0) return 0; + if (clipped_count == 0) return 0; // edge 2 - EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane); + EDGE_PLANE(tripoints[2], tripoints[0], tri_plane, edgeplane); clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points1,clipped_count,clip_points); + edgeplane, temp_points1, clipped_count, clip_points); return clipped_count; - /*GUINT i0 = (tri_plane.closestAxis()+1)%3; GUINT i1 = (i0+1)%3; // edge 0 @@ -172,13 +169,13 @@ public: } SIMD_FORCE_INLINE void sort_isect( - GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1) + GREAL &isect0, GREAL &isect1, GUINT &e0, GUINT &e1, btVector3 &vec0, btVector3 &vec1) { - if(isect1=isect_v[1]) // face U casts face V + if (isect_u[1] >= isect_v[1]) // face U casts face V { return 1; } - else if(isect_v[0]<=isect_u[0]) // face V casts face U + else if (isect_v[0] <= isect_u[0]) // face V casts face U { return 2; } @@ -257,32 +253,31 @@ public: closest_point_v = vp_e0; // calc edges and separation - if(isect_u[1]+ MIN_EDGE_EDGE_DIS=isect_u[1]) // face V casts face U + if (isect_v[1] >= isect_u[1]) // face V casts face U { return 2; } - else if(isect_u[0]<=isect_v[0]) // face U casts face V + else if (isect_u[0] <= isect_v[0]) // face U casts face V { return 1; } @@ -291,41 +286,39 @@ public: closest_point_v = vp_e1; // calc edges and separation - if(isect_v[1]+MIN_EDGE_EDGE_DIS0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ? + if (du0du1 > 0.0f && du0du2 > 0.0f) // same sign on all of them + not equal 0 ? { - if(du[0]<0) //we need test behind the triangle plane + if (du[0] < 0) //we need test behind the triangle plane { - distances[0] = GIM_MAX3(du[0],du[1],du[2]); + distances[0] = GIM_MAX3(du[0], du[1], du[2]); distances[0] = -distances[0]; - if(distances[0]>margin) return false; //never intersect + if (distances[0] > margin) return false; //never intersect //reorder triangle v - VEC_SWAP(tv_vertices[0],tv_vertices[1]); - VEC_SCALE_4(tv_plane,-1.0f,tv_plane); + VEC_SWAP(tv_vertices[0], tv_vertices[1]); + VEC_SCALE_4(tv_plane, -1.0f, tv_plane); } else { - distances[0] = GIM_MIN3(du[0],du[1],du[2]); - if(distances[0]>margin) return false; //never intersect + distances[0] = GIM_MIN3(du[0], du[1], du[2]); + if (distances[0] > margin) return false; //never intersect } } else { //Look if we need to invert the triangle - distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid + distances[0] = (du[0] + du[1] + du[2]) / 3.0f; //centroid - if(distances[0]<0.0f) + if (distances[0] < 0.0f) { //reorder triangle v - VEC_SWAP(tv_vertices[0],tv_vertices[1]); - VEC_SCALE_4(tv_plane,-1.0f,tv_plane); + VEC_SWAP(tv_vertices[0], tv_vertices[1]); + VEC_SCALE_4(tv_plane, -1.0f, tv_plane); - distances[0] = GIM_MAX3(du[0],du[1],du[2]); + distances[0] = GIM_MAX3(du[0], du[1], du[2]); distances[0] = -distances[0]; } else { - distances[0] = GIM_MIN3(du[0],du[1],du[2]); + distances[0] = GIM_MIN3(du[0], du[1], du[2]); } } - // plane U vs V points - TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane); + TRIANGLE_PLANE(tu_vertices[0], tu_vertices[1], tu_vertices[2], tu_plane); - dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]); - dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]); - dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]); + dv[0] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[0]); + dv[1] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[1]); + dv[2] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[2]); dv0dv1 = dv[0] * dv[1]; dv0dv2 = dv[0] * dv[2]; - - if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ? + if (dv0dv1 > 0.0f && dv0dv2 > 0.0f) // same sign on all of them + not equal 0 ? { - if(dv[0]<0) //we need test behind the triangle plane + if (dv[0] < 0) //we need test behind the triangle plane { - distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]); distances[1] = -distances[1]; - if(distances[1]>margin) return false; //never intersect + if (distances[1] > margin) return false; //never intersect //reorder triangle u - VEC_SWAP(tu_vertices[0],tu_vertices[1]); - VEC_SCALE_4(tu_plane,-1.0f,tu_plane); + VEC_SWAP(tu_vertices[0], tu_vertices[1]); + VEC_SCALE_4(tu_plane, -1.0f, tu_plane); } else { - distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); - if(distances[1]>margin) return false; //never intersect + distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]); + if (distances[1] > margin) return false; //never intersect } } else { //Look if we need to invert the triangle - distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid + distances[1] = (dv[0] + dv[1] + dv[2]) / 3.0f; //centroid - if(distances[1]<0.0f) + if (distances[1] < 0.0f) { //reorder triangle v - VEC_SWAP(tu_vertices[0],tu_vertices[1]); - VEC_SCALE_4(tu_plane,-1.0f,tu_plane); + VEC_SWAP(tu_vertices[0], tu_vertices[1]); + VEC_SCALE_4(tu_plane, -1.0f, tu_plane); - distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]); distances[1] = -distances[1]; } else { - distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]); } } @@ -448,47 +437,44 @@ public: } else {*/ - bl = 0; - if(distances[0]margin) return false; + if (distances[2] > margin) return false; contacts.m_penetration_depth = -distances[2] + margin; contacts.m_points[0] = closest_point_v; contacts.m_point_count = 1; - VEC_COPY(contacts.m_separating_normal,edge_edge_dir); + VEC_COPY(contacts.m_separating_normal, edge_edge_dir); return true; } //clip face against other - GUINT point_count; //TODO - if(bl == 0) //clip U points against V + if (bl == 0) //clip U points against V { - point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points); - if(point_count == 0) return false; - contacts.merge_points(tv_plane,margin,contact_points,point_count); + point_count = clip_triangle(tv_plane, tv_vertices, tu_vertices, contact_points); + if (point_count == 0) return false; + contacts.merge_points(tv_plane, margin, contact_points, point_count); } - else //clip V points against U + else //clip V points against U { - point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points); - if(point_count == 0) return false; - contacts.merge_points(tu_plane,margin,contact_points,point_count); + point_count = clip_triangle(tu_plane, tu_vertices, tv_vertices, contact_points); + if (point_count == 0) return false; + contacts.merge_points(tu_plane, margin, contact_points, point_count); contacts.m_separating_normal *= -1.f; } - if(contacts.m_point_count == 0) return false; + if (contacts.m_point_count == 0) return false; return true; } - }; - /*class GIM_TRIANGLE_CALCULATION_CACHE { public: @@ -621,20 +607,13 @@ public: };*/ - - bool GIM_TRIANGLE::collide_triangle_hard_test( - const GIM_TRIANGLE & other, - GIM_TRIANGLE_CONTACT_DATA & contact_data) const + const GIM_TRIANGLE &other, + GIM_TRIANGLE_CONTACT_DATA &contact_data) const { - GIM_TRIANGLE_CALCULATION_CACHE calc_cache; + GIM_TRIANGLE_CALCULATION_CACHE calc_cache; return calc_cache.triangle_collision( - m_vertices[0],m_vertices[1],m_vertices[2],m_margin, - other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin, - contact_data); - + m_vertices[0], m_vertices[1], m_vertices[2], m_margin, + other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], other.m_margin, + contact_data); } - - - - diff --git a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_tri_collision.h b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_tri_collision.h index 267f806e7..e6d4bf547 100644 --- a/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_tri_collision.h +++ b/Engine/lib/bullet/src/BulletCollision/Gimpact/gim_tri_collision.h @@ -36,8 +36,6 @@ email: projectileman@yahoo.com #include "gim_box_collision.h" #include "gim_clip_polygon.h" - - #ifndef MAX_TRI_CLIPPING #define MAX_TRI_CLIPPING 16 #endif @@ -45,18 +43,18 @@ email: projectileman@yahoo.com //! Structure for collision struct GIM_TRIANGLE_CONTACT_DATA { - GREAL m_penetration_depth; - GUINT m_point_count; - btVector4 m_separating_normal; - btVector3 m_points[MAX_TRI_CLIPPING]; + GREAL m_penetration_depth; + GUINT m_point_count; + btVector4 m_separating_normal; + btVector3 m_points[MAX_TRI_CLIPPING]; - SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other) + SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA &other) { m_penetration_depth = other.m_penetration_depth; m_separating_normal = other.m_separating_normal; m_point_count = other.m_point_count; GUINT i = m_point_count; - while(i--) + while (i--) { m_points[i] = other.m_points[i]; } @@ -66,39 +64,36 @@ struct GIM_TRIANGLE_CONTACT_DATA { } - GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other) + GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA &other) { copy_from(other); } - - - - //! classify points that are closer - template - SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane, - GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func) - { - m_point_count = 0; - m_penetration_depth= -1000.0f; + //! classify points that are closer + template + SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE &plane, + GREAL margin, const btVector3 *points, GUINT point_count, DISTANCE_FUNC distance_func) + { + m_point_count = 0; + m_penetration_depth = -1000.0f; GUINT point_indices[MAX_TRI_CLIPPING]; GUINT _k; - for(_k=0;_k=0.0f) + if (_dist >= 0.0f) { - if(_dist>m_penetration_depth) + if (_dist > m_penetration_depth) { m_penetration_depth = _dist; point_indices[0] = _k; - m_point_count=1; + m_point_count = 1; } - else if((_dist+G_EPSILON)>=m_penetration_depth) + else if ((_dist + G_EPSILON) >= m_penetration_depth) { point_indices[m_point_count] = _k; m_point_count++; @@ -106,88 +101,87 @@ struct GIM_TRIANGLE_CONTACT_DATA } } - for( _k=0;_kG_EPSILON) + else if (sumuv - 1.0f > G_EPSILON) { return false; } @@ -294,50 +288,49 @@ if 0.0<= u+v <=1.0 then they are inside of triangle /*! Test if point is in triangle, with m_margin tolerance */ - SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const + SIMD_FORCE_INLINE bool is_point_inside(const btVector3 &point, const btVector3 &tri_normal) const { //Test with edge 0 btVector4 edge_plane; - this->get_edge_plane(0,tri_normal,edge_plane); - GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point); - if(dist-m_margin>0.0f) return false; // outside plane + this->get_edge_plane(0, tri_normal, edge_plane); + GREAL dist = DISTANCE_PLANE_POINT(edge_plane, point); + if (dist - m_margin > 0.0f) return false; // outside plane - this->get_edge_plane(1,tri_normal,edge_plane); - dist = DISTANCE_PLANE_POINT(edge_plane,point); - if(dist-m_margin>0.0f) return false; // outside plane + this->get_edge_plane(1, tri_normal, edge_plane); + dist = DISTANCE_PLANE_POINT(edge_plane, point); + if (dist - m_margin > 0.0f) return false; // outside plane - this->get_edge_plane(2,tri_normal,edge_plane); - dist = DISTANCE_PLANE_POINT(edge_plane,point); - if(dist-m_margin>0.0f) return false; // outside plane + this->get_edge_plane(2, tri_normal, edge_plane); + dist = DISTANCE_PLANE_POINT(edge_plane, point); + if (dist - m_margin > 0.0f) return false; // outside plane return true; } - //! Bidireccional ray collision SIMD_FORCE_INLINE bool ray_collision( - const btVector3 & vPoint, - const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal, - GREAL & tparam, GREAL tmax = G_REAL_INFINITY) + const btVector3 &vPoint, + const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal, + GREAL &tparam, GREAL tmax = G_REAL_INFINITY) { btVector4 faceplane; { btVector3 dif1 = m_vertices[1] - m_vertices[0]; btVector3 dif2 = m_vertices[2] - m_vertices[0]; - VEC_CROSS(faceplane,dif1,dif2); - faceplane[3] = m_vertices[0].dot(faceplane); + VEC_CROSS(faceplane, dif1, dif2); + faceplane[3] = m_vertices[0].dot(faceplane); } - GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax); - if(res == 0) return false; - if(! is_point_inside(pout,faceplane)) return false; + GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax); + if (res == 0) return false; + if (!is_point_inside(pout, faceplane)) return false; - if(res==2) //invert normal + if (res == 2) //invert normal { - triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]); + triangle_normal.setValue(-faceplane[0], -faceplane[1], -faceplane[2]); } else { - triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); + triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]); } VEC_NORMALIZE(triangle_normal); @@ -345,36 +338,31 @@ if 0.0<= u+v <=1.0 then they are inside of triangle return true; } - //! one direccion ray collision SIMD_FORCE_INLINE bool ray_collision_front_side( - const btVector3 & vPoint, - const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal, - GREAL & tparam, GREAL tmax = G_REAL_INFINITY) + const btVector3 &vPoint, + const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal, + GREAL &tparam, GREAL tmax = G_REAL_INFINITY) { btVector4 faceplane; { btVector3 dif1 = m_vertices[1] - m_vertices[0]; btVector3 dif2 = m_vertices[2] - m_vertices[0]; - VEC_CROSS(faceplane,dif1,dif2); - faceplane[3] = m_vertices[0].dot(faceplane); + VEC_CROSS(faceplane, dif1, dif2); + faceplane[3] = m_vertices[0].dot(faceplane); } - GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax); - if(res != 1) return false; + GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax); + if (res != 1) return false; - if(!is_point_inside(pout,faceplane)) return false; + if (!is_point_inside(pout, faceplane)) return false; - triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); + triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]); VEC_NORMALIZE(triangle_normal); return true; } - }; - - - -#endif // GIM_TRI_COLLISION_H_INCLUDED +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h index 9eb880b8d..c94391d81 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h @@ -16,163 +16,153 @@ subject to the following restrictions: #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 "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) + 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; + (void)v; - - 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; + // 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) +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() 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() < colDesc.m_gjkRelError2) + { + m_cachedSeparatingAxis = newCachedSeparatingAxis; + m_degenerateSimplex = 6; + checkSimplex = true; + break; + } + + btScalar previousSquaredDistance = squaredDistance; + squaredDistance = newCachedSeparatingAxis.length2(); #if 0 ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo if (squaredDistance>previousSquaredDistance) @@ -182,188 +172,183 @@ int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate 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 // + + //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; + + 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 +#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 940282f57..38df8d480 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btContinuousConvexCollision.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" @@ -24,59 +23,60 @@ subject to the following restrictions: #include "btPointCollector.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" - - -btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_simplexSolver(simplexSolver), -m_penetrationDepthSolver(penetrationDepthSolver), -m_convexA(convexA),m_convexB1(convexB),m_planeShape(0) +btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver) + : m_simplexSolver(simplexSolver), + m_penetrationDepthSolver(penetrationDepthSolver), + m_convexA(convexA), + m_convexB1(convexB), + m_planeShape(0) { } - -btContinuousConvexCollision::btContinuousConvexCollision( const btConvexShape* convexA,const btStaticPlaneShape* plane) -:m_simplexSolver(0), -m_penetrationDepthSolver(0), -m_convexA(convexA),m_convexB1(0),m_planeShape(plane) +btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btStaticPlaneShape* plane) + : m_simplexSolver(0), + m_penetrationDepthSolver(0), + m_convexA(convexA), + m_convexB1(0), + m_planeShape(plane) { } - /// This maximum should not be necessary. It allows for untested/degenerate cases in production code. /// You don't want your game ever to lock-up. #define MAX_ITERATIONS 64 -void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector) +void btContinuousConvexCollision::computeClosestPoints(const btTransform& transA, const btTransform& transB, btPointCollector& pointCollector) { if (m_convexB1) { m_simplexSolver->reset(); - btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver); + btGjkPairDetector gjk(m_convexA, m_convexB1, m_convexA->getShapeType(), m_convexB1->getShapeType(), m_convexA->getMargin(), m_convexB1->getMargin(), m_simplexSolver, m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; input.m_transformA = transA; input.m_transformB = transB; - gjk.getClosestPoints(input,pointCollector,0); - } else + gjk.getClosestPoints(input, pointCollector, 0); + } + else { //convex versus plane const btConvexShape* convexShape = m_convexA; const btStaticPlaneShape* planeShape = m_planeShape; - + const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); - + btTransform convexWorldTransform = transA; btTransform convexInPlaneTrans; - convexInPlaneTrans= transB.inverse() * convexWorldTransform; + convexInPlaneTrans = transB.inverse() * convexWorldTransform; btTransform planeInConvex; - planeInConvex= convexWorldTransform.inverse() * transB; - - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + planeInConvex = convexWorldTransform.inverse() * transB; + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected; btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal; @@ -87,41 +87,33 @@ void btContinuousConvexCollision::computeClosestPoints( const btTransform& trans } } -bool btContinuousConvexCollision::calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) +bool btContinuousConvexCollision::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) { - - /// compute linear and angular velocity for this interval, to interpolate - btVector3 linVelA,angVelA,linVelB,angVelB; - btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA); - btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB); - + btVector3 linVelA, angVelA, linVelB, angVelB; + btTransformUtil::calculateVelocity(fromA, toA, btScalar(1.), linVelA, angVelA); + btTransformUtil::calculateVelocity(fromB, toB, btScalar(1.), linVelB, angVelB); btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); - btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f; + btScalar boundingRadiusB = m_convexB1 ? m_convexB1->getAngularMotionDisc() : 0.f; btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; - btVector3 relLinVel = (linVelB-linVelA); + btVector3 relLinVel = (linVelB - linVelA); - btScalar relLinVelocLength = (linVelB-linVelA).length(); - - if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f) + btScalar relLinVelocLength = (linVelB - linVelA).length(); + + 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.)); + n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); bool hasResult = false; btVector3 c; @@ -131,15 +123,13 @@ bool btContinuousConvexCollision::calcTimeOfImpact( int numIter = 0; //first solution, using GJK - btScalar radius = 0.001f; -// result.drawCoordSystem(sphereTr); + // result.drawCoordSystem(sphereTr); - btPointCollector pointCollector1; + btPointCollector pointCollector1; { - - computeClosestPoints(fromA,fromB,pointCollector1); + computeClosestPoints(fromA, fromB, pointCollector1); hasResult = pointCollector1.m_hasResult; c = pointCollector1.m_pointInWorld; @@ -151,7 +141,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( dist = pointCollector1.m_distance + result.m_allowedPenetration; n = pointCollector1.m_normalOnBInWorld; btScalar projectedLinearVelocity = relLinVel.dot(n); - if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) + if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON) return false; //not close enough @@ -159,77 +149,69 @@ bool btContinuousConvexCollision::calcTimeOfImpact( { if (result.m_debugDrawer) { - result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1)); + result.m_debugDrawer->drawSphere(c, 0.2f, btVector3(1, 1, 1)); } btScalar dLambda = btScalar(0.); projectedLinearVelocity = relLinVel.dot(n); - //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) - return false; - - dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); - - - - lambda = lambda + dLambda; - - if (lambda > btScalar(1.)) + if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON) return false; - if (lambda < btScalar(0.)) - return false; + dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity); + lambda += dLambda; + + if (lambda > btScalar(1.) || 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; + btTransform interpolatedTransA, interpolatedTransB, relativeTrans; - btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA); - btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB); + btTransformUtil::integrateTransform(fromA, linVelA, angVelA, lambda, interpolatedTransA); + btTransformUtil::integrateTransform(fromB, linVelB, angVelB, lambda, interpolatedTransB); relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA); if (result.m_debugDrawer) { - result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0)); + result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(), 0.2f, btVector3(1, 0, 0)); } - result.DebugDraw( lambda ); + result.DebugDraw(lambda); - btPointCollector pointCollector; - computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector); + btPointCollector pointCollector; + computeClosestPoints(interpolatedTransA, interpolatedTransB, pointCollector); if (pointCollector.m_hasResult) { - dist = pointCollector.m_distance+result.m_allowedPenetration; - c = pointCollector.m_pointInWorld; + dist = pointCollector.m_distance + result.m_allowedPenetration; + c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; - } else + } + else { result.reportFailure(-1, numIter); return false; } numIter++; - if (numIter > maxIter) + if (numIter > MAX_ITERATIONS) { result.reportFailure(-2, numIter); return false; } } - + result.m_fraction = lambda; result.m_normal = n; result.m_hitPoint = c; @@ -237,6 +219,4 @@ bool btContinuousConvexCollision::calcTimeOfImpact( } return false; - } - diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h index bdc0572f7..67b2205c3 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_CONTINUOUS_COLLISION_CONVEX_CAST_H #define BT_CONTINUOUS_COLLISION_CONVEX_CAST_H @@ -25,35 +24,30 @@ 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 { btSimplexSolverInterface* m_simplexSolver; - btConvexPenetrationDepthSolver* m_penetrationDepthSolver; - const btConvexShape* m_convexA; + btConvexPenetrationDepthSolver* m_penetrationDepthSolver; + const btConvexShape* m_convexA; //second object is either a convex or a plane (code sharing) - const btConvexShape* m_convexB1; - const btStaticPlaneShape* m_planeShape; + const btConvexShape* m_convexB1; + const btStaticPlaneShape* m_planeShape; - void computeClosestPoints( const btTransform& transA, const btTransform& transB,struct btPointCollector& pointCollector); + void computeClosestPoints(const btTransform& transA, const btTransform& transB, struct btPointCollector& pointCollector); public: + btContinuousConvexCollision(const btConvexShape* shapeA, const btConvexShape* shapeB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver); - btContinuousConvexCollision (const btConvexShape* shapeA,const btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); - - btContinuousConvexCollision(const btConvexShape* shapeA,const btStaticPlaneShape* plane ); - - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result); - + btContinuousConvexCollision(const btConvexShape* shapeA, const btStaticPlaneShape* plane); + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); }; - -#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H - +#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index bfd79d03b..77b19be59 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_CONVEX_CAST_H #define BT_CONVEX_CAST_H @@ -23,51 +22,69 @@ subject to the following restrictions: class btMinkowskiSumShape; #include "LinearMath/btIDebugDraw.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define MAX_CONVEX_CAST_ITERATIONS 64 +#define MAX_CONVEX_CAST_EPSILON (SIMD_EPSILON * 10) +#else +#define MAX_CONVEX_CAST_ITERATIONS 32 +#define MAX_CONVEX_CAST_EPSILON btScalar(0.0001) +#endif +///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. +///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 +//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) + /// btConvexCast is an interface for Casting class btConvexCast { public: - - virtual ~btConvexCast(); ///RayResult stores the closest result /// alternatively, add a callback method to decide about closest/all results - struct CastResult + struct CastResult { //virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0; - - virtual void DebugDraw(btScalar fraction) {(void)fraction;} - virtual void drawCoordSystem(const btTransform& trans) {(void)trans;} - virtual void reportFailure(int errNo, int numIterations) {(void)errNo;(void)numIterations;} + + virtual void DebugDraw(btScalar fraction) { (void)fraction; } + virtual void drawCoordSystem(const btTransform& trans) { (void)trans; } + virtual void reportFailure(int errNo, int numIterations) + { + (void)errNo; + (void)numIterations; + } CastResult() - :m_fraction(btScalar(BT_LARGE_FLOAT)), - m_debugDrawer(0), - m_allowedPenetration(btScalar(0)) + : m_fraction(btScalar(BT_LARGE_FLOAT)), + m_debugDrawer(0), + m_allowedPenetration(btScalar(0)), + m_subSimplexCastMaxIterations(MAX_CONVEX_CAST_ITERATIONS), + m_subSimplexCastEpsilon(MAX_CONVEX_CAST_EPSILON) { } + virtual ~CastResult(){}; - virtual ~CastResult() {}; - - btTransform m_hitTransformA; - btTransform m_hitTransformB; - btVector3 m_normal; - btVector3 m_hitPoint; - btScalar m_fraction; //input and output + btTransform m_hitTransformA; + btTransform m_hitTransformB; + btVector3 m_normal; + btVector3 m_hitPoint; + btScalar m_fraction; //input and output btIDebugDraw* m_debugDrawer; - btScalar m_allowedPenetration; + btScalar m_allowedPenetration; + + int m_subSimplexCastMaxIterations; + btScalar m_subSimplexCastEpsilon; }; - /// cast a convex against another convex object - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) = 0; + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) = 0; }; -#endif //BT_CONVEX_CAST_H +#endif //BT_CONVEX_CAST_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 29620abff..65c9df934 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_CONVEX_PENETRATION_DEPTH_H #define BT_CONVEX_PENETRATION_DEPTH_H @@ -25,16 +24,12 @@ class btTransform; ///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. class btConvexPenetrationDepthSolver { -public: - - virtual ~btConvexPenetrationDepthSolver() {}; - virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw) = 0; - - +public: + virtual ~btConvexPenetrationDepthSolver(){}; + virtual bool calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* convexA, const btConvexShape* convexB, + const btTransform& transA, const btTransform& transB, + btVector3& v, btVector3& pa, btVector3& pb, + class btIDebugDraw* debugDraw) = 0; }; -#endif //BT_CONVEX_PENETRATION_DEPTH_H - +#endif //BT_CONVEX_PENETRATION_DEPTH_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index 0ea7b483c..d1bbb1a46 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H #define BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H @@ -27,64 +26,60 @@ subject to the following restrictions: /// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB struct btDiscreteCollisionDetectorInterface { - struct Result { - - virtual ~Result(){} + virtual ~Result() {} ///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner - virtual void setShapeIdentifiersA(int partId0,int index0)=0; - virtual void setShapeIdentifiersB(int partId1,int index1)=0; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0; + virtual void setShapeIdentifiersA(int partId0, int index0) = 0; + virtual void setShapeIdentifiersB(int partId1, int index1) = 0; + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) = 0; }; struct ClosestPointInput { ClosestPointInput() - :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)) + : m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)) { } btTransform m_transformA; btTransform m_transformB; - btScalar m_maximumDistanceSquared; + btScalar m_maximumDistanceSquared; }; - virtual ~btDiscreteCollisionDetectorInterface() {}; + virtual ~btDiscreteCollisionDetectorInterface(){}; // // give either closest points (distance > 0) or penetration (distance) // the normal always points from B towards A // - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false) = 0; - + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false) = 0; }; struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result { - btVector3 m_normalOnSurfaceB; - btVector3 m_closestPointInB; - btScalar m_distance; //negative means penetration ! + btVector3 m_normalOnSurfaceB; + btVector3 m_closestPointInB; + btScalar m_distance; //negative means penetration ! - protected: - btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) - { - } - - public: - virtual ~btStorageResult() {}; +protected: + btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) + { + } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) +public: + virtual ~btStorageResult(){}; + + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) + { + if (depth < m_distance) { - if (depth < m_distance) - { - m_normalOnSurfaceB = normalOnBInWorld; - m_closestPointInB = pointInWorld; - m_distance = depth; - } + m_normalOnSurfaceB = normalOnBInWorld; + m_closestPointInB = pointInWorld; + m_distance = depth; } + } }; -#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H - +#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h index 0b49b0ecc..c9fd84beb 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GJK_COLLISION_DESCRIPTION_H #define GJK_COLLISION_DESCRIPTION_H @@ -21,21 +20,20 @@ subject to the following restrictions: struct btGjkCollisionDescription { - btVector3 m_firstDir; - int m_maxGjkIterations; - btScalar m_maximumDistanceSquared; - btScalar m_gjkRelError2; - btGjkCollisionDescription() - :m_firstDir(0,1,0), - m_maxGjkIterations(1000), - m_maximumDistanceSquared(1e30f), - m_gjkRelError2(1.0e-6) - { - } - virtual ~btGjkCollisionDescription() - { - } + btVector3 m_firstDir; + int m_maxGjkIterations; + btScalar m_maximumDistanceSquared; + btScalar m_gjkRelError2; + btGjkCollisionDescription() + : m_firstDir(0, 1, 0), + m_maxGjkIterations(1000), + m_maximumDistanceSquared(1e30f), + m_gjkRelError2(1.0e-6) + { + } + virtual ~btGjkCollisionDescription() + { + } }; -#endif //GJK_COLLISION_DESCRIPTION_H - +#endif //GJK_COLLISION_DESCRIPTION_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp index bef697a0a..9d61e75da 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btGjkConvexCast.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" #include "btGjkPairDetector.h" @@ -27,41 +25,39 @@ subject to the following restrictions: #define MAX_ITERATIONS 32 #endif -btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) -:m_simplexSolver(simplexSolver), -m_convexA(convexA), -m_convexB(convexB) +btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver) + : m_simplexSolver(simplexSolver), + m_convexA(convexA), + m_convexB(convexB) { } -bool btGjkConvexCast::calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) +bool btGjkConvexCast::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) { - - m_simplexSolver->reset(); /// compute linear velocity for this interval, to interpolate //assume no rotation/angular velocity, assert here? - btVector3 linVelA,linVelB; - linVelA = toA.getOrigin()-fromA.getOrigin(); - linVelB = toB.getOrigin()-fromB.getOrigin(); + btVector3 linVelA, linVelB; + linVelA = toA.getOrigin() - fromA.getOrigin(); + linVelB = toB.getOrigin() - fromB.getOrigin(); btScalar radius = btScalar(0.001); btScalar lambda = btScalar(0.); - btVector3 v(1,0,0); + btVector3 v(1, 0, 0); int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); bool hasResult = false; btVector3 c; - btVector3 r = (linVelA-linVelB); + btVector3 r = (linVelA - linVelB); btScalar lastLambda = lambda; //btScalar epsilon = btScalar(0.001); @@ -69,17 +65,14 @@ bool btGjkConvexCast::calcTimeOfImpact( int numIter = 0; //first solution, using GJK - btTransform identityTrans; identityTrans.setIdentity(); + // result.drawCoordSystem(sphereTr); -// result.drawCoordSystem(sphereTr); + btPointCollector pointCollector; - btPointCollector pointCollector; - - - btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver); + btGjkPairDetector gjk(m_convexA, m_convexB, m_simplexSolver, 0); //m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; //we don't use margins during CCD @@ -87,7 +80,7 @@ bool btGjkConvexCast::calcTimeOfImpact( input.m_transformA = fromA; input.m_transformB = fromB; - gjk.getClosestPoints(input,pointCollector,0); + gjk.getClosestPoints(input, pointCollector, 0); hasResult = pointCollector.m_hasResult; c = pointCollector.m_pointInWorld; @@ -98,20 +91,18 @@ bool btGjkConvexCast::calcTimeOfImpact( dist = pointCollector.m_distance; n = pointCollector.m_normalOnBInWorld; - - //not close enough while (dist > radius) { numIter++; if (numIter > maxIter) { - return false; //todo: report a failure + return false; //todo: report a failure } btScalar dLambda = btScalar(0.); btScalar projectedLinearVelocity = r.dot(n); - + dLambda = dist / (projectedLinearVelocity); lambda = lambda - dLambda; @@ -132,35 +123,35 @@ bool btGjkConvexCast::calcTimeOfImpact( lastLambda = lambda; //interpolate to next lambda - result.DebugDraw( lambda ); - input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); - input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); - - gjk.getClosestPoints(input,pointCollector,0); + result.DebugDraw(lambda); + input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(), toA.getOrigin(), lambda); + input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda); + + gjk.getClosestPoints(input, pointCollector, 0); if (pointCollector.m_hasResult) { if (pointCollector.m_distance < btScalar(0.)) { result.m_fraction = lastLambda; n = pointCollector.m_normalOnBInWorld; - result.m_normal=n; + result.m_normal = n; result.m_hitPoint = pointCollector.m_pointInWorld; return true; } - c = pointCollector.m_pointInWorld; + c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; dist = pointCollector.m_distance; - } else + } + else { //?? return false; } - } //is n normalized? //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (n.dot(r)>=-result.m_allowedPenetration) + if (n.dot(r) >= -result.m_allowedPenetration) return false; result.m_fraction = lambda; @@ -170,7 +161,4 @@ bool btGjkConvexCast::calcTimeOfImpact( } return false; - - } - diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h index 6a42ee63b..ef5979173 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_GJK_CONVEX_CAST_H #define BT_GJK_CONVEX_CAST_H @@ -29,22 +27,20 @@ class btMinkowskiSumShape; ///GjkConvexCast performs a raycast on a convex object using support mapping. class btGjkConvexCast : public btConvexCast { - btSimplexSolverInterface* m_simplexSolver; - const btConvexShape* m_convexA; - const btConvexShape* m_convexB; + btSimplexSolverInterface* m_simplexSolver; + const btConvexShape* m_convexA; + const btConvexShape* m_convexB; public: - - btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver); + btGjkConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver); /// cast a convex against another convex object - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result); - + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); }; -#endif //BT_GJK_CONVEX_CAST_H +#endif //BT_GJK_CONVEX_CAST_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index eefb974bb..45d181713 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -26,1010 +26,1065 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #include "BulletCollision/CollisionShapes/btSphereShape.h" #include "btGjkEpa2.h" -#if defined(DEBUG) || defined (_DEBUG) -#include //for debug printf +#if defined(DEBUG) || defined(_DEBUG) +#include //for debug printf #ifdef __SPU__ #include #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #endif namespace gjkepa2_impl { +// Config - // Config - - /* GJK */ -#define GJK_MAX_ITERATIONS 128 +/* GJK */ +#define GJK_MAX_ITERATIONS 128 #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) +#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_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) -#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 128 -#define EPA_MAX_ITERATIONS 255 +/* EPA */ +#define EPA_MAX_VERTICES 128 +#define EPA_MAX_ITERATIONS 255 #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) +#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) +#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) +#define EPA_FALLBACK (10 * EPA_ACCURACY) +#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2) +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; - // Shorthands - typedef unsigned int U; - typedef unsigned char U1; +// MinkowskiDiff +struct MinkowskiDiff +{ + const btConvexShape* m_shapes[2]; + btMatrix3x3 m_toshape1; + btTransform m_toshape0; +#ifdef __SPU__ + bool m_enableMargin; +#else + btVector3 (btConvexShape::*Ls)(const btVector3&) const; +#endif //__SPU__ - // MinkowskiDiff - struct MinkowskiDiff + MinkowskiDiff() { - const btConvexShape* m_shapes[2]; - btMatrix3x3 m_toshape1; - btTransform m_toshape0; + } #ifdef __SPU__ - bool m_enableMargin; + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + if (m_enableMargin) + { + return m_shapes[0]->localGetSupportVertexNonVirtual(d); + } + else + { + return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); + } + } + inline btVector3 Support1(const btVector3& d) const + { + if (m_enableMargin) + { + return m_toshape0 * (m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1 * d)); + } + else + { + return m_toshape0 * (m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1 * d)); + } + } #else - btVector3 (btConvexShape::*Ls)(const btVector3&) const; -#endif//__SPU__ - + void EnableMargin(bool enable) + { + if (enable) + Ls = &btConvexShape::localGetSupportVertexNonVirtual; + else + Ls = &btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; + } + inline btVector3 Support0(const btVector3& d) const + { + return (((m_shapes[0])->*(Ls))(d)); + } + inline btVector3 Support1(const btVector3& d) const + { + return (m_toshape0 * ((m_shapes[1])->*(Ls))(m_toshape1 * d)); + } +#endif //__SPU__ - MinkowskiDiff() - { + 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)); + } +}; - } -#ifdef __SPU__ - void EnableMargin(bool enable) - { - m_enableMargin = enable; - } - inline btVector3 Support0(const btVector3& d) const - { - if (m_enableMargin) - { - return m_shapes[0]->localGetSupportVertexNonVirtual(d); - } else - { - return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); - } - } - inline btVector3 Support1(const btVector3& d) const - { - if (m_enableMargin) - { - return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d)); - } else - { - return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d)); - } - } -#else - void EnableMargin(bool enable) - { - if(enable) - Ls=&btConvexShape::localGetSupportVertexNonVirtual; - else - Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; - } - inline btVector3 Support0(const btVector3& d) const - { - return(((m_shapes[0])->*(Ls))(d)); - } - inline btVector3 Support1(const btVector3& d) const - { - return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); - } -#endif //__SPU__ +typedef MinkowskiDiff tShape; - 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)); - } +// GJK +struct GJK +{ + /* Types */ + struct sSV + { + btVector3 d, w; }; - - typedef MinkowskiDiff tShape; - - - // GJK - struct GJK + struct sSimplex { - /* Types */ - struct sSV + sSV* c[4]; + btScalar p[4]; + U rank; + }; + struct eStatus + { + enum _ { - btVector3 d,w; - }; - struct sSimplex - { - sSV* c[4]; - btScalar p[4]; - U rank; - }; - struct eStatus { enum _ { Valid, Inside, - Failed };}; - /* Fields */ - tShape m_shape; - btVector3 m_ray; - btScalar m_distance; - sSimplex m_simplices[2]; - sSV m_store[4]; - sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; - eStatus::_ m_status; - /* Methods */ - GJK() - { - Initialize(); - } - void Initialize() - { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; - } - eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) - { - U iterations=0; - btScalar sqdist=0; - btScalar alpha=0; - btVector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eStatus::Valid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar rl=m_ray.length(); - if(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=eStatus::Inside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)rank) - { - case 1: - { - for(U i=0;i<3;++i) - { - 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< 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 { - btVector3 n; - btScalar d; - sSV* c[3]; - sFace* f[3]; - sFace* l[2]; - U1 e[3]; - U1 pass; - }; - struct sList + const U next = 1 - m_current; + sSimplex& cs = m_simplices[m_current]; + sSimplex& ns = m_simplices[next]; + /* Check zero */ + const btScalar rl = m_ray.length(); + if (rl < GJK_MIN_DISTANCE) + { /* Touching or inside */ + m_status = eStatus::Inside; + break; + } + /* Append new vertice in -'v' direction */ + appendvertice(cs, -m_ray); + const btVector3& w = cs.c[cs.rank - 1]->w; + bool found = false; + for (U i = 0; i < 4; ++i) + { + if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS) + { + found = true; + break; + } + } + if (found) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + { /* Update lastw */ + lastw[clastw = (clastw + 1) & 3] = w; + } + /* Check for termination */ + const btScalar omega = btDot(m_ray, w) / rl; + alpha = btMax(omega, alpha); + if (((rl - alpha) - (GJK_ACCURACY * rl)) <= 0) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + btScalar weights[4]; + U mask = 0; + switch (cs.rank) + { + case 2: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + weights, mask); + break; + case 3: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights, mask); + break; + case 4: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights, mask); + break; + } + if (sqdist >= 0) + { /* Valid */ + ns.rank = 0; + m_ray = btVector3(0, 0, 0); + m_current = next; + for (U i = 0, ni = cs.rank; i < ni; ++i) + { + if (mask & (1 << i)) + { + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w * weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if (mask == 15) m_status = eStatus::Inside; + } + else + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus::Failed; + } while (m_status == eStatus::Valid); + m_simplex = &m_simplices[m_current]; + switch (m_status) { - sFace* root; - U count; - sList() : root(0),count(0) {} - }; - struct sHorizon + case eStatus::Valid: + m_distance = m_ray.length(); + break; + case eStatus::Inside: + m_distance = 0; + break; + default: + { + } + } + return (m_status); + } + bool EncloseOrigin() + { + switch (m_simplex->rank) + { + case 1: + { + for (U i = 0; i < 3; ++i) + { + btVector3 axis = btVector3(0, 0, 0); + axis[i] = 1; + appendvertice(*m_simplex, axis); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -axis); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w; + for (U i = 0; i < 3; ++i) + { + btVector3 axis = btVector3(0, 0, 0); + axis[i] = 1; + const btVector3 p = btCross(d, axis); + if (p.length2() > 0) + { + appendvertice(*m_simplex, p); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -p); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n = btCross(m_simplex->c[1]->w - m_simplex->c[0]->w, + m_simplex->c[2]->w - m_simplex->c[0]->w); + if (n.length2() > 0) + { + appendvertice(*m_simplex, n); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -n); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if (btFabs(det(m_simplex->c[0]->w - m_simplex->c[3]->w, + m_simplex->c[1]->w - m_simplex->c[3]->w, + m_simplex->c[2]->w - m_simplex->c[3]->w)) > 0) + return (true); + } + break; + } + return (false); + } + /* Internals */ + void getsupport(const btVector3& d, sSV& sv) const + { + sv.d = d / d.length(); + sv.w = m_shape.Support(sv.d); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++] = simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex, const btVector3& v) + { + simplex.p[simplex.rank] = 0; + simplex.c[simplex.rank] = m_free[--m_nfree]; + getsupport(v, *simplex.c[simplex.rank++]); + } + static btScalar det(const btVector3& a, const btVector3& b, const btVector3& c) + { + return (a.y() * b.z() * c.x() + a.z() * b.x() * c.y() - + a.x() * b.z() * c.y() - a.y() * b.x() * c.z() + + a.x() * b.y() * c.z() - a.z() * b.y() * c.x()); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + btScalar* w, U& m) + { + const btVector3 d = b - a; + const btScalar l = d.length2(); + if (l > GJK_SIMPLEX2_EPS) + { + const btScalar t(l > 0 ? -btDot(a, d) / l : 0); + if (t >= 1) + { + w[0] = 0; + w[1] = 1; + m = 2; + return (b.length2()); + } + else if (t <= 0) + { + w[0] = 1; + w[1] = 0; + m = 1; + return (a.length2()); + } + else + { + w[0] = 1 - (w[1] = t); + m = 3; + return ((a + d * t).length2()); + } + } + return (-1); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w, U& m) + { + static const U imd3[] = {1, 2, 0}; + const btVector3* vt[] = {&a, &b, &c}; + const btVector3 dl[] = {a - b, b - c, c - a}; + const btVector3 n = btCross(dl[0], dl[1]); + const btScalar l = n.length2(); + if (l > GJK_SIMPLEX3_EPS) + { + btScalar mindist = -1; + btScalar subw[2] = {0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + if (btDot(*vt[i], btCross(dl[i], n)) > 0) + { + const U j = imd3[i]; + const btScalar subd(projectorigin(*vt[i], *vt[j], subw, subm)); + if ((mindist < 0) || (subd < mindist)) + { + mindist = subd; + m = static_cast(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + } + } + } + if (mindist < 0) + { + const btScalar d = btDot(a, n); + const btScalar s = btSqrt(l); + const btVector3 p = n * (d / l); + mindist = p.length2(); + m = 7; + w[0] = (btCross(dl[1], b - p)).length() / s; + w[1] = (btCross(dl[2], c - p)).length() / s; + w[2] = 1 - (w[0] + w[1]); + } + return (mindist); + } + return (-1); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& d, + btScalar* w, U& m) + { + static const U imd3[] = {1, 2, 0}; + const btVector3* vt[] = {&a, &b, &c, &d}; + const btVector3 dl[] = {a - d, b - d, c - d}; + const btScalar vl = det(dl[0], dl[1], dl[2]); + const bool ng = (vl * btDot(a, btCross(b - c, a - b))) <= 0; + if (ng && (btFabs(vl) > GJK_SIMPLEX4_EPS)) + { + btScalar mindist = -1; + btScalar subw[3] = {0.f, 0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + const U j = imd3[i]; + const btScalar s = vl * btDot(d, btCross(dl[i], dl[j])); + if (s > 0) + { + const btScalar subd = projectorigin(*vt[i], *vt[j], d, subw, subm); + if ((mindist < 0) || (subd < mindist)) + { + mindist = subd; + m = static_cast((subm & 1 ? 1 << i : 0) + + (subm & 2 ? 1 << j : 0) + + (subm & 4 ? 8 : 0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + w[3] = subw[2]; + } + } + } + if (mindist < 0) + { + mindist = 0; + m = 15; + w[0] = det(c, b, d) / vl; + w[1] = det(a, c, d) / vl; + w[2] = det(b, a, d) / vl; + w[3] = 1 - (w[0] + w[1] + w[2]); + } + return (mindist); + } + return (-1); + } +}; + +// EPA +struct EPA +{ + /* Types */ + typedef GJK::sSV sSV; + struct sFace + { + btVector3 n; + btScalar d; + sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + U1 e[3]; + U1 pass; + }; + struct sList + { + sFace* root; + U count; + sList() : root(0), count(0) {} + }; + struct sHorizon + { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(0), ff(0), nf(0) {} + }; + struct eStatus + { + enum _ { - sFace* cf; - sFace* ff; - U nf; - sHorizon() : cf(0),ff(0),nf(0) {} - }; - struct eStatus { enum _ { Valid, Touching, Degenerated, NonConvex, - InvalidHull, + InvalidHull, OutOfFaces, OutOfVertices, AccuraryReached, FallBack, - Failed };}; - /* Fields */ - eStatus::_ m_status; - GJK::sSimplex m_result; - btVector3 m_normal; - btScalar m_depth; - sSV m_sv_store[EPA_MAX_VERTICES]; - sFace m_fc_store[EPA_MAX_FACES]; - U m_nextsv; - sList m_hull; - sList m_stock; - /* Methods */ - EPA() - { - Initialize(); - } - - - static inline void bind(sFace* fa,U ea,sFace* fb,U eb) - { - fa->e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } - static inline void append(sList& list,sFace* face) - { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; - } - static inline void remove(sList& list,sFace* face) - { - if(face->l[1]) face->l[1]->l[0]=face->l[0]; - if(face->l[0]) face->l[0]->l[1]=face->l[1]; - if(face==list.root) list.root=face->l[1]; - --list.count; - } - - - void Initialize() - { - m_status = eStatus::Failed; - m_normal = btVector3(0,0,0); - m_depth = 0; - m_nextsv = 0; - for(U i=0;i1)&&gjk.EncloseOrigin()) - { - - /* Clean up */ - while(m_hull.root) - { - sFace* f = m_hull.root; - remove(m_hull,f); - append(m_stock,f); - } - m_status = eStatus::Valid; - m_nextsv = 0; - /* Orient simplex */ - if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, - simplex.c[1]->w-simplex.c[3]->w, - simplex.c[2]->w-simplex.c[3]->w)<0) - { - btSwap(simplex.c[0],simplex.c[1]); - btSwap(simplex.p[0],simplex.p[1]); - } - /* Build initial hull */ - sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), - newface(simplex.c[1],simplex.c[0],simplex.c[3],true), - newface(simplex.c[2],simplex.c[1],simplex.c[3],true), - newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; - if(m_hull.count==4) - { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U iterations=0; - bind(tetra[0],0,tetra[1],0); - bind(tetra[0],1,tetra[2],0); - bind(tetra[0],2,tetra[3],0); - bind(tetra[1],1,tetra[3],2); - bind(tetra[1],2,tetra[2],1); - bind(tetra[2],2,tetra[3],1); - m_status=eStatus::Valid; - for(;iterationspass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const btScalar wdist=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=eStatus::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } - } - const btVector3 projection=outer.n*outer.d; - m_normal = outer.n; - m_depth = outer.d; - m_result.rank = 3; - m_result.c[0] = outer.c[0]; - m_result.c[1] = outer.c[1]; - m_result.c[2] = outer.c[2]; - m_result.p[0] = btCross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = btCross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = btCross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; - m_result.p[0] /= sum; - m_result.p[1] /= sum; - m_result.p[2] /= sum; - return(m_status); - } - } - /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const btScalar nl=m_normal.length(); - if(nl>0) - m_normal = m_normal/nl; - else - m_normal = btVector3(1,0,0); - m_depth = 0; - m_result.rank=1; - m_result.c[0]=simplex.c[0]; - m_result.p[0]=1; - return(m_status); - } - bool getedgedist(sFace* face, sSV* a, 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(sSV* a,sSV* b,sSV* c,bool forced) - { - if(m_stock.root) - { - sFace* face=m_stock.root; - remove(m_stock,face); - append(m_hull,face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = btCross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; - - 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=eStatus::NonConvex; - } - else - m_status=eStatus::Degenerated; - - remove(m_hull, face); - append(m_stock, face); - return 0; - - } - m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces; - return 0; - } - sFace* findbest() - { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const btScalar sqd=f->d*f->d; - if(sqdpass!=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); - } - + Failed + }; }; - - // - static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - btGjkEpaSolver2::sResults& results, - tShape& shape, - bool withmargins) + /* Fields */ + eStatus::_ m_status; + GJK::sSimplex m_result; + btVector3 m_normal; + btScalar m_depth; + sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + U m_nextsv; + sList m_hull; + sList m_stock; + /* Methods */ + EPA() { - /* Results */ - results.witnesses[0] = - results.witnesses[1] = btVector3(0,0,0); - results.status = btGjkEpaSolver2::sResults::Separated; - /* Shape */ - shape.m_shapes[0] = shape0; - shape.m_shapes[1] = shape1; - shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); - shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); - shape.EnableMargin(withmargins); + Initialize(); } + static inline void bind(sFace* fa, U ea, sFace* fb, U eb) + { + fa->e[ea] = (U1)eb; + fa->f[ea] = fb; + fb->e[eb] = (U1)ea; + fb->f[eb] = fa; + } + static inline void append(sList& list, sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if (list.root) list.root->l[0] = face; + list.root = face; + ++list.count; + } + static inline void remove(sList& list, sFace* face) + { + if (face->l[1]) face->l[1]->l[0] = face->l[0]; + if (face->l[0]) face->l[0]->l[1] = face->l[1]; + if (face == list.root) list.root = face->l[1]; + --list.count; + } + + void Initialize() + { + m_status = eStatus::Failed; + m_normal = btVector3(0, 0, 0); + m_depth = 0; + m_nextsv = 0; + for (U i = 0; i < EPA_MAX_FACES; ++i) + { + append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]); + } + } + eStatus::_ Evaluate(GJK& gjk, const btVector3& guess) + { + GJK::sSimplex& simplex = *gjk.m_simplex; + if ((simplex.rank > 1) && gjk.EncloseOrigin()) + { + /* Clean up */ + while (m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull, f); + append(m_stock, f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if (gjk.det(simplex.c[0]->w - simplex.c[3]->w, + simplex.c[1]->w - simplex.c[3]->w, + simplex.c[2]->w - simplex.c[3]->w) < 0) + { + btSwap(simplex.c[0], simplex.c[1]); + btSwap(simplex.p[0], simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[] = {newface(simplex.c[0], simplex.c[1], simplex.c[2], true), + newface(simplex.c[1], simplex.c[0], simplex.c[3], true), + newface(simplex.c[2], simplex.c[1], simplex.c[3], true), + newface(simplex.c[0], simplex.c[2], simplex.c[3], true)}; + if (m_hull.count == 4) + { + sFace* best = findbest(); + sFace outer = *best; + U pass = 0; + U iterations = 0; + bind(tetra[0], 0, tetra[1], 0); + bind(tetra[0], 1, tetra[2], 0); + bind(tetra[0], 2, tetra[3], 0); + bind(tetra[1], 1, tetra[3], 2); + bind(tetra[1], 2, tetra[2], 1); + bind(tetra[2], 2, tetra[3], 1); + m_status = eStatus::Valid; + for (; iterations < EPA_MAX_ITERATIONS; ++iterations) + { + if (m_nextsv < EPA_MAX_VERTICES) + { + sHorizon horizon; + sSV* w = &m_sv_store[m_nextsv++]; + bool valid = true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n, *w); + const btScalar wdist = btDot(best->n, w->w) - best->d; + if (wdist > EPA_ACCURACY) + { + for (U j = 0; (j < 3) && valid; ++j) + { + valid &= expand(pass, w, + best->f[j], best->e[j], + horizon); + } + if (valid && (horizon.nf >= 3)) + { + bind(horizon.cf, 1, horizon.ff, 2); + remove(m_hull, best); + append(m_stock, best); + best = findbest(); + outer = *best; + } + else + { + m_status = eStatus::InvalidHull; + break; + } + } + else + { + m_status = eStatus::AccuraryReached; + break; + } + } + else + { + m_status = eStatus::OutOfVertices; + break; + } + } + const btVector3 projection = outer.n * outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = btCross(outer.c[1]->w - projection, + outer.c[2]->w - projection) + .length(); + m_result.p[1] = btCross(outer.c[2]->w - projection, + outer.c[0]->w - projection) + .length(); + m_result.p[2] = btCross(outer.c[0]->w - projection, + outer.c[1]->w - projection) + .length(); + const btScalar sum = m_result.p[0] + m_result.p[1] + m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return (m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const btScalar nl = m_normal.length(); + if (nl > 0) + m_normal = m_normal / nl; + else + m_normal = btVector3(1, 0, 0); + m_depth = 0; + m_result.rank = 1; + m_result.c[0] = simplex.c[0]; + m_result.p[0] = 1; + return (m_status); + } + bool getedgedist(sFace* face, sSV* a, 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(sSV* a, sSV* b, sSV* c, bool forced) + { + if (m_stock.root) + { + sFace* face = m_stock.root; + remove(m_stock, face); + append(m_hull, face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = btCross(b->w - a->w, c->w - a->w); + const btScalar l = face->n.length(); + const bool v = l > EPA_ACCURACY; + + 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 = eStatus::NonConvex; + } + else + m_status = eStatus::Degenerated; + + remove(m_hull, face); + append(m_stock, face); + return 0; + } + m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces; + return 0; + } + sFace* findbest() + { + sFace* minf = m_hull.root; + btScalar mind = minf->d * minf->d; + for (sFace* f = minf->l[1]; f; f = f->l[1]) + { + const btScalar sqd = f->d * f->d; + if (sqd < mind) + { + minf = f; + mind = sqd; + } + } + return (minf); + } + bool expand(U pass, sSV* w, sFace* f, U e, sHorizon& horizon) + { + static const U i1m3[] = {1, 2, 0}; + static const U i2m3[] = {2, 0, 1}; + if (f->pass != pass) + { + const U e1 = i1m3[e]; + if ((btDot(f->n, w->w) - f->d) < -EPA_PLANE_EPS) + { + sFace* nf = newface(f->c[e1], f->c[e], w, false); + if (nf) + { + bind(nf, 0, f, e); + if (horizon.cf) + bind(horizon.cf, 1, nf, 2); + else + horizon.ff = nf; + horizon.cf = nf; + ++horizon.nf; + return (true); + } + } + else + { + const U e2 = i2m3[e]; + f->pass = (U1)pass; + if (expand(pass, w, f->f[e1], f->e[e1], horizon) && + expand(pass, w, f->f[e2], f->e[e2], horizon)) + { + remove(m_hull, f); + append(m_stock, f); + return (true); + } + } + } + return (false); + } +}; + +// +static void Initialize(const btConvexShape* shape0, const btTransform& wtrs0, + const btConvexShape* shape1, const btTransform& wtrs1, + btGjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) +{ + /* Results */ + results.witnesses[0] = + results.witnesses[1] = btVector3(0, 0, 0); + results.status = btGjkEpaSolver2::sResults::Separated; + /* Shape */ + shape.m_shapes[0] = shape0; + shape.m_shapes[1] = shape1; + shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); + shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); + shape.EnableMargin(withmargins); } +} // namespace gjkepa2_impl + // // Api // -using namespace gjkepa2_impl; +using namespace gjkepa2_impl; // -int btGjkEpaSolver2::StackSizeRequirement() +int btGjkEpaSolver2::StackSizeRequirement() { - return(sizeof(GJK)+sizeof(EPA)); + return (sizeof(GJK) + sizeof(EPA)); } // -bool btGjkEpaSolver2::Distance( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) +bool btGjkEpaSolver2::Distance(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) { - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); - if(gjk_status==GJK::eStatus::Valid) + tShape shape; + Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, false); + GJK gjk; + GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, guess); + if (gjk_status == GJK::eStatus::Valid) { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;irank;++i) + btVector3 w0 = btVector3(0, 0, 0); + btVector3 w1 = btVector3(0, 0, 0); + for (U i = 0; i < gjk.m_simplex->rank; ++i) { - const btScalar p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + const btScalar p = gjk.m_simplex->p[i]; + w0 += shape.Support(gjk.m_simplex->c[i]->d, 0) * p; + w1 += shape.Support(-gjk.m_simplex->c[i]->d, 1) * p; } - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*w1; - results.normal = w0-w1; - results.distance = results.normal.length(); - results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; - return(true); + results.witnesses[0] = wtrs0 * w0; + results.witnesses[1] = wtrs0 * w1; + results.normal = w0 - w1; + results.distance = results.normal.length(); + results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1; + return (true); } else { - results.status = gjk_status==GJK::eStatus::Inside? - sResults::Penetrating : - sResults::GJK_Failed ; - return(false); + results.status = gjk_status == GJK::eStatus::Inside ? sResults::Penetrating : sResults::GJK_Failed; + return (false); } } // -bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results, - bool usemargins) +bool btGjkEpaSolver2::Penetration(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + bool usemargins) { - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); - switch(gjk_status) + tShape shape; + Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, usemargins); + GJK gjk; + GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, -guess); + switch (gjk_status) { - case GJK::eStatus::Inside: + case GJK::eStatus::Inside: { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) + EPA epa; + EPA::eStatus::_ epa_status = epa.Evaluate(gjk, -guess); + if (epa_status != EPA::eStatus::Failed) { - btVector3 w0=btVector3(0,0,0); - for(U i=0;id,0)*epa.m_result.p[i]; + w0 += shape.Support(epa.m_result.c[i]->d, 0) * epa.m_result.p[i]; } - results.status = sResults::Penetrating; - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); - results.normal = -epa.m_normal; - results.distance = -epa.m_depth; - return(true); - } else results.status=sResults::EPA_Failed; + results.status = sResults::Penetrating; + results.witnesses[0] = wtrs0 * w0; + results.witnesses[1] = wtrs0 * (w0 - epa.m_normal * epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return (true); + } + else + results.status = sResults::EPA_Failed; } break; - case GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; + case GJK::eStatus::Failed: + results.status = sResults::GJK_Failed; + break; default: - { - } + { + } } - return(false); + return (false); } #ifndef __SPU__ // -btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, - btScalar margin, - const btConvexShape* shape0, - const btTransform& wtrs0, - sResults& results) +btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, + btScalar margin, + const btConvexShape* shape0, + const btTransform& wtrs0, + sResults& results) { - tShape shape; - btSphereShape shape1(margin); - btTransform wtrs1(btQuaternion(0,0,0,1),position); - Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); - if(gjk_status==GJK::eStatus::Valid) + tShape shape; + btSphereShape shape1(margin); + btTransform wtrs1(btQuaternion(0, 0, 0, 1), position); + Initialize(shape0, wtrs0, &shape1, wtrs1, results, shape, false); + GJK gjk; + GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, btVector3(1, 1, 1)); + if (gjk_status == GJK::eStatus::Valid) { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;irank;++i) + btVector3 w0 = btVector3(0, 0, 0); + btVector3 w1 = btVector3(0, 0, 0); + for (U i = 0; i < gjk.m_simplex->rank; ++i) { - const btScalar p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + const btScalar p = gjk.m_simplex->p[i]; + w0 += shape.Support(gjk.m_simplex->c[i]->d, 0) * p; + w1 += shape.Support(-gjk.m_simplex->c[i]->d, 1) * p; } - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*w1; - const btVector3 delta= results.witnesses[1]- - results.witnesses[0]; - const btScalar margin= shape0->getMarginNonVirtual()+ - shape1.getMarginNonVirtual(); - const btScalar length= delta.length(); - results.normal = delta/length; - results.witnesses[0] += results.normal*margin; - return(length-margin); + results.witnesses[0] = wtrs0 * w0; + results.witnesses[1] = wtrs0 * w1; + const btVector3 delta = results.witnesses[1] - + results.witnesses[0]; + const btScalar margin = shape0->getMarginNonVirtual() + + shape1.getMarginNonVirtual(); + const btScalar length = delta.length(); + results.normal = delta / length; + results.witnesses[0] += results.normal * margin; + return (length - margin); } else { - if(gjk_status==GJK::eStatus::Inside) + if (gjk_status == GJK::eStatus::Inside) { - if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + if (Penetration(shape0, wtrs0, &shape1, wtrs1, gjk.m_ray, results)) { - const btVector3 delta= results.witnesses[0]- - results.witnesses[1]; - const btScalar length= delta.length(); + const btVector3 delta = results.witnesses[0] - + results.witnesses[1]; + const btScalar length = delta.length(); if (length >= SIMD_EPSILON) - results.normal = delta/length; - return(-length); + results.normal = delta / length; + return (-length); } - } + } } - return(SIMD_INFINITY); + return (SIMD_INFINITY); } // -bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) +bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) { - if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) - return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); + if (!Distance(shape0, wtrs0, shape1, wtrs1, guess, results)) + return (Penetration(shape0, wtrs0, shape1, wtrs1, guess, results, false)); else - return(true); + return (true); } -#endif //__SPU__ +#endif //__SPU__ -/* Symbols cleanup */ +/* Symbols cleanup */ #undef GJK_MAX_ITERATIONS #undef GJK_ACCURACY diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h index ac501d5ec..893daea3f 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -28,48 +28,46 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #include "BulletCollision/CollisionShapes/btConvexShape.h" ///btGjkEpaSolver contributed under zlib by Nathanael Presson -struct btGjkEpaSolver2 +struct btGjkEpaSolver2 { -struct sResults + struct sResults { - enum eStatus + 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; + 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; }; -static int StackSizeRequirement(); + static int StackSizeRequirement(); -static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - const btVector3& guess, - sResults& results); + static bool Distance(const btConvexShape* shape0, const btTransform& wtrs0, + const btConvexShape* shape1, const btTransform& wtrs1, + const btVector3& guess, + sResults& results); -static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, + static bool Penetration(const btConvexShape* shape0, const btTransform& wtrs0, + const btConvexShape* shape1, const btTransform& wtrs1, const btVector3& guess, sResults& results, - bool usemargins=true); + bool usemargins = true); #ifndef __SPU__ -static btScalar SignedDistance( const btVector3& position, - btScalar 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 btVector3& guess, - sResults& results); -#endif //__SPU__ + static btScalar SignedDistance(const btVector3& position, + btScalar 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 btVector3& guess, + sResults& results); +#endif //__SPU__ }; -#endif //BT_GJK_EPA2_H - +#endif //BT_GJK_EPA2_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h index ce1f24bc5..6fedbbb3e 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h @@ -29,915 +29,946 @@ Improvements and refactoring by Erwin Coumans, 2008-2014 #include "LinearMath/btTransform.h" #include "btGjkCollisionDescription.h" - - -struct btGjkEpaSolver3 +struct btGjkEpaSolver3 { -struct sResults + struct sResults { - enum eStatus + 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; + 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 +#if defined(DEBUG) || defined(_DEBUG) +#include //for debug printf #ifdef __SPU__ #include #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #endif +// Config - - // 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 */ +#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) - // 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()); - - } - +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 (rl < GJK_MIN_DISTANCE) + { /* Touching or inside */ + m_status = eGjkInside; + break; + } + /* Append new vertice in -'v' direction */ + appendvertice(cs, -m_ray); + const btVector3& w = cs.c[cs.rank - 1]->w; + bool found = false; + for (U i = 0; i < 4; ++i) + { + if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS) + { + found = true; + break; + } + } + if (found) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + { /* Update lastw */ + lastw[clastw = (clastw + 1) & 3] = w; + } + /* Check for termination */ + const btScalar omega = btDot(m_ray, w) / rl; + alpha = btMax(omega, alpha); + if (((rl - alpha) - (GJK_ACCURARY * rl)) <= 0) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + btScalar weights[4]; + U mask = 0; + switch (cs.rank) + { + case 2: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + weights, mask); + break; + case 3: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights, mask); + break; + case 4: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights, mask); + break; + } + if (sqdist >= 0) + { /* Valid */ + ns.rank = 0; + m_ray = btVector3(0, 0, 0); + m_current = next; + for (U i = 0, ni = cs.rank; i < ni; ++i) + { + if (mask & (1 << i)) + { + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w * weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if (mask == 15) m_status = eGjkInside; + } + else + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eGjkFailed; + } while (m_status == eGjkValid); + m_simplex = &m_simplices[m_current]; + switch (m_status) + { + case eGjkValid: + m_distance = m_ray.length(); + break; + case eGjkInside: + m_distance = 0; + break; + default: + { + } + } + return (m_status); + } + bool EncloseOrigin() + { + switch (m_simplex->rank) + { + case 1: + { + for (U i = 0; i < 3; ++i) + { + btVector3 axis = btVector3(0, 0, 0); + axis[i] = 1; + appendvertice(*m_simplex, axis); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -axis); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w; + for (U i = 0; i < 3; ++i) + { + btVector3 axis = btVector3(0, 0, 0); + axis[i] = 1; + const btVector3 p = btCross(d, axis); + if (p.length2() > 0) + { + appendvertice(*m_simplex, p); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -p); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n = btCross(m_simplex->c[1]->w - m_simplex->c[0]->w, + m_simplex->c[2]->w - m_simplex->c[0]->w); + if (n.length2() > 0) + { + appendvertice(*m_simplex, n); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -n); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if (btFabs(det(m_simplex->c[0]->w - m_simplex->c[3]->w, + m_simplex->c[1]->w - m_simplex->c[3]->w, + m_simplex->c[2]->w - m_simplex->c[3]->w)) > 0) + return (true); + } + break; + } + return (false); + } + /* Internals */ + void getsupport(const btVector3& d, sSV& sv) const + { + sv.d = d / d.length(); + sv.w = m_shape.Support(sv.d); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++] = simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex, const btVector3& v) + { + simplex.p[simplex.rank] = 0; + simplex.c[simplex.rank] = m_free[--m_nfree]; + getsupport(v, *simplex.c[simplex.rank++]); + } + static btScalar det(const btVector3& a, const btVector3& b, const btVector3& c) + { + return (a.y() * b.z() * c.x() + a.z() * b.x() * c.y() - + a.x() * b.z() * c.y() - a.y() * b.x() * c.z() + + a.x() * b.y() * c.z() - a.z() * b.y() * c.x()); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + btScalar* w, U& m) + { + const btVector3 d = b - a; + const btScalar l = d.length2(); + if (l > GJK_SIMPLEX2_EPS) + { + const btScalar t(l > 0 ? -btDot(a, d) / l : 0); + if (t >= 1) + { + w[0] = 0; + w[1] = 1; + m = 2; + return (b.length2()); + } + else if (t <= 0) + { + w[0] = 1; + w[1] = 0; + m = 1; + return (a.length2()); + } + else + { + w[0] = 1 - (w[1] = t); + m = 3; + return ((a + d * t).length2()); + } + } + return (-1); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w, U& m) + { + static const U imd3[] = {1, 2, 0}; + const btVector3* vt[] = {&a, &b, &c}; + const btVector3 dl[] = {a - b, b - c, c - a}; + const btVector3 n = btCross(dl[0], dl[1]); + const btScalar l = n.length2(); + if (l > GJK_SIMPLEX3_EPS) + { + btScalar mindist = -1; + btScalar subw[2] = {0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + if (btDot(*vt[i], btCross(dl[i], n)) > 0) + { + const U j = imd3[i]; + const btScalar subd(projectorigin(*vt[i], *vt[j], subw, subm)); + if ((mindist < 0) || (subd < mindist)) + { + mindist = subd; + m = static_cast(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + } + } + } + if (mindist < 0) + { + const btScalar d = btDot(a, n); + const btScalar s = btSqrt(l); + const btVector3 p = n * (d / l); + mindist = p.length2(); + m = 7; + w[0] = (btCross(dl[1], b - p)).length() / s; + w[1] = (btCross(dl[2], c - p)).length() / s; + w[2] = 1 - (w[0] + w[1]); + } + return (mindist); + } + return (-1); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& d, + btScalar* w, U& m) + { + static const U imd3[] = {1, 2, 0}; + const btVector3* vt[] = {&a, &b, &c, &d}; + const btVector3 dl[] = {a - d, b - d, c - d}; + const btScalar vl = det(dl[0], dl[1], dl[2]); + const bool ng = (vl * btDot(a, btCross(b - c, a - b))) <= 0; + if (ng && (btFabs(vl) > GJK_SIMPLEX4_EPS)) + { + btScalar mindist = -1; + btScalar subw[3] = {0.f, 0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + const U j = imd3[i]; + const btScalar s = vl * btDot(d, btCross(dl[i], dl[j])); + if (s > 0) + { + const btScalar subd = projectorigin(*vt[i], *vt[j], d, subw, subm); + if ((mindist < 0) || (subd < mindist)) + { + mindist = subd; + m = static_cast((subm & 1 ? 1 << i : 0) + + (subm & 2 ? 1 << j : 0) + + (subm & 4 ? 8 : 0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + w[3] = subw[2]; + } + } + } + if (mindist < 0) + { + mindist = 0; + m = 15; + w[0] = det(c, b, d) / vl; + w[1] = det(a, c, d) / vl; + w[2] = det(b, a, d) / vl; + w[3] = 1 - (w[0] + w[1] + w[2]); + } + return (mindist); + } + return (-1); + } +}; + +enum eEpaStatus +{ + eEpaValid, + eEpaTouching, + eEpaDegenerated, + eEpaNonConvex, + eEpaInvalidHull, + eEpaOutOfFaces, + eEpaOutOfVertices, + eEpaAccuraryReached, + eEpaFallBack, + eEpaFailed +}; + +// EPA +template +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 < EPA_MAX_FACES; ++i) + { + append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]); + } + } + eEpaStatus Evaluate(GJK& 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 < EPA_MAX_ITERATIONS; ++iterations) + { + if (m_nextsv < EPA_MAX_VERTICES) + { + sHorizon horizon; + typename GJK::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 < mind) + { + minf = f; + mind = sqd; + } + } + return (minf); + } + bool expand(U pass, typename GJK::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) +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); - } + 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; i < gjk.m_simplex->rank; ++i) + { + const btScalar p = gjk.m_simplex->p[i]; + w0 += shape.Support(gjk.m_simplex->c[i]->d, 0) * p; + w1 += shape.Support(-gjk.m_simplex->c[i]->d, 1) * p; + } + results.witnesses[0] = 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) +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); + 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; i < epa.m_result.rank; ++i) + { + w0 += shape.Support(epa.m_result.c[i]->d, 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 @@ -990,28 +1021,28 @@ int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanc #endif template -int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, - const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo) +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; + 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 */ +/* Symbols cleanup */ #undef GJK_MAX_ITERATIONS #undef GJK_ACCURARY @@ -1029,7 +1060,4 @@ int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, #undef EPA_PLANE_EPS #undef EPA_INSIDE_EPS - - -#endif //BT_GJK_EPA3_H - +#endif //BT_GJK_EPA3_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 572ec36f5..07629229a 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -18,49 +18,64 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "btGjkEpaPenetrationDepthSolver.h" - #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* pConvexA, const btConvexShape* pConvexB, - const btTransform& transformA, const btTransform& transformB, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw) +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()).safeNormalize(), + btVector3(transformA.getOrigin() - transformB.getOrigin()).safeNormalize(), + 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/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index 1ed6340af..92d6df172 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -23,21 +23,18 @@ subject to the following restrictions: ///calculate the penetration depth between two convex shapes. class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver { - public : +public: + btGjkEpaPenetrationDepthSolver() + { + } - btGjkEpaPenetrationDepthSolver() - { - } - - bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* pConvexA, const btConvexShape* pConvexB, - const btTransform& transformA, const btTransform& transformB, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw); - - private : + bool calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* pConvexA, const btConvexShape* pConvexB, + const btTransform& transformA, const btTransform& transformB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, + class btIDebugDraw* debugDraw); +private: }; -#endif // BT_GJP_EPA_PENETRATION_DEPTH_H - +#endif // BT_GJP_EPA_PENETRATION_DEPTH_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 257b026d9..4339b2ea7 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -18,86 +18,685 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" - - -#if defined(DEBUG) || defined (_DEBUG) +#if defined(DEBUG) || defined(_DEBUG) //#define TEST_NON_VIRTUAL 1 -#include //for debug printf +#include //for debug printf #ifdef __SPU__ #include #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #endif //must be above the machine epsilon -#ifdef BT_USE_DOUBLE_PRECISION - #define REL_ERROR2 btScalar(1.0e-12) - btScalar gGjkEpaPenetrationTolerance = 1e-7; +#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; +#define REL_ERROR2 btScalar(1.0e-6) +btScalar gGjkEpaPenetrationTolerance = 0.001; #endif -//temp globals, to improve GJK/EPA/penetration calculations -int gNumDeepPenetrationChecks = 0; -int gNumGjkChecks = 0; - -btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), -m_penetrationDepthSolver(penetrationDepthSolver), -m_simplexSolver(simplexSolver), -m_minkowskiA(objectA), -m_minkowskiB(objectB), -m_shapeTypeA(objectA->getShapeType()), -m_shapeTypeB(objectB->getShapeType()), -m_marginA(objectA->getMargin()), -m_marginB(objectB->getMargin()), -m_ignoreMargin(false), -m_lastUsedMethod(-1), -m_catchDegeneracies(1), -m_fixContactNormalDirection(1) +btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver) + : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)), + m_penetrationDepthSolver(penetrationDepthSolver), + m_simplexSolver(simplexSolver), + m_minkowskiA(objectA), + m_minkowskiB(objectB), + m_shapeTypeA(objectA->getShapeType()), + m_shapeTypeB(objectB->getShapeType()), + m_marginA(objectA->getMargin()), + m_marginB(objectB->getMargin()), + m_ignoreMargin(false), + m_lastUsedMethod(-1), + m_catchDegeneracies(1), + m_fixContactNormalDirection(1) { } -btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), -m_penetrationDepthSolver(penetrationDepthSolver), -m_simplexSolver(simplexSolver), -m_minkowskiA(objectA), -m_minkowskiB(objectB), -m_shapeTypeA(shapeTypeA), -m_shapeTypeB(shapeTypeB), -m_marginA(marginA), -m_marginB(marginB), -m_ignoreMargin(false), -m_lastUsedMethod(-1), -m_catchDegeneracies(1), -m_fixContactNormalDirection(1) +btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver) + : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)), + m_penetrationDepthSolver(penetrationDepthSolver), + m_simplexSolver(simplexSolver), + m_minkowskiA(objectA), + m_minkowskiB(objectB), + m_shapeTypeA(shapeTypeA), + m_shapeTypeB(shapeTypeB), + m_marginA(marginA), + m_marginB(marginB), + m_ignoreMargin(false), + m_lastUsedMethod(-1), + m_catchDegeneracies(1), + m_fixContactNormalDirection(1) { } -void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +void btGjkPairDetector::getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults) { (void)swapResults; - getClosestPointsNonVirtual(input,output,debugDraw); + getClosestPointsNonVirtual(input, output, debugDraw); +} + +static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb) +{ + btVector3 seperatingAxisInA = (dir)*localTransA.getBasis(); + btVector3 seperatingAxisInB = (-dir) * localTransB.getBasis(); + + btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + + btVector3 pInA = pInANoMargin; + btVector3 qInB = qInBNoMargin; + + supAworld = localTransA(pInA); + supBworld = localTransB(qInB); + + if (check2d) + { + supAworld[2] = 0.f; + supBworld[2] = 0.f; + } + + aMinb = supAworld - supBworld; +} + +struct btSupportVector +{ + btVector3 v; //!< Support point in minkowski sum + btVector3 v1; //!< Support point in obj1 + btVector3 v2; //!< Support point in obj2 +}; + +struct btSimplex +{ + btSupportVector ps[4]; + int last; //!< index of last added point +}; + +static btVector3 ccd_vec3_origin(0, 0, 0); + +inline void btSimplexInit(btSimplex *s) +{ + s->last = -1; +} + +inline int btSimplexSize(const btSimplex *s) +{ + return s->last + 1; +} + +inline const btSupportVector *btSimplexPoint(const btSimplex *s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} +inline void btSupportCopy(btSupportVector *d, const btSupportVector *s) +{ + *d = *s; +} + +inline void btVec3Copy(btVector3 *v, const btVector3 *w) +{ + *v = *w; +} + +inline void ccdVec3Add(btVector3 *v, const btVector3 *w) +{ + v->m_floats[0] += w->m_floats[0]; + v->m_floats[1] += w->m_floats[1]; + v->m_floats[2] += w->m_floats[2]; +} + +inline void ccdVec3Sub(btVector3 *v, const btVector3 *w) +{ + *v -= *w; +} +inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) +{ + *d = (*v) - (*w); +} +inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b) +{ + btScalar dot; + dot = a->dot(*b); + + return dot; +} + +inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b) +{ + btVector3 ab; + btVec3Sub2(&ab, a, b); + return btVec3Dot(&ab, &ab); +} + +inline void btVec3Scale(btVector3 *d, btScalar k) +{ + d->m_floats[0] *= k; + d->m_floats[1] *= k; + d->m_floats[2] *= k; +} + +inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b) +{ + d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]); + d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]); + d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]); +} + +inline void btTripleCross(const btVector3 *a, const btVector3 *b, + const btVector3 *c, btVector3 *d) +{ + btVector3 e; + btVec3Cross(&e, a, b); + btVec3Cross(d, &e, c); +} + +inline int ccdEq(btScalar _a, btScalar _b) +{ + btScalar ab; + btScalar a, b; + + ab = btFabs(_a - _b); + if (btFabs(ab) < SIMD_EPSILON) + return 1; + + a = btFabs(_a); + b = btFabs(_b); + if (b > a) + { + return ab < SIMD_EPSILON * b; + } + else + { + return ab < SIMD_EPSILON * a; + } +} + +btScalar ccdVec3X(const btVector3 *v) +{ + return v->x(); +} + +btScalar ccdVec3Y(const btVector3 *v) +{ + return v->y(); +} + +btScalar ccdVec3Z(const btVector3 *v) +{ + return v->z(); +} +inline int btVec3Eq(const btVector3 *a, const btVector3 *b) +{ + return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) && ccdEq(ccdVec3Z(a), ccdVec3Z(b)); +} + +inline void btSimplexAdd(btSimplex *s, const btSupportVector *v) +{ + // here is no check on boundaries in sake of speed + ++s->last; + btSupportCopy(s->ps + s->last, v); +} + +inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a) +{ + btSupportCopy(s->ps + pos, a); +} + +inline void btSimplexSetSize(btSimplex *s, int size) +{ + s->last = size - 1; +} + +inline const btSupportVector *ccdSimplexLast(const btSimplex *s) +{ + return btSimplexPoint(s, s->last); +} + +inline int ccdSign(btScalar val) +{ + if (btFuzzyZero(val)) + { + return 0; + } + else if (val < btScalar(0)) + { + return -1; + } + return 1; +} + +inline btScalar btVec3PointSegmentDist2(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. + + btScalar dist, t; + btVector3 d, a; + + // direction of segment + btVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + btVec3Sub2(&a, x0, P); + + t = -btScalar(1.) * btVec3Dot(&a, &d); + t /= btVec3Dot(&d, &d); + + if (t < btScalar(0) || btFuzzyZero(t)) + { + dist = ccdVec3Dist2(x0, P); + if (witness) + btVec3Copy(witness, x0); + } + else if (t > btScalar(1) || ccdEq(t, btScalar(1))) + { + dist = ccdVec3Dist2(b, P); + if (witness) + btVec3Copy(witness, b); + } + else + { + if (witness) + { + btVec3Copy(witness, &d); + btVec3Scale(witness, t); + ccdVec3Add(witness, x0); + dist = ccdVec3Dist2(witness, P); + } + else + { + // recycling variables + btVec3Scale(&d, t); + ccdVec3Add(&d, &a); + dist = btVec3Dot(&d, &d); + } + } + + return dist; +} + +btScalar btVec3PointTriDist2(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; + double u, v, w, p, q, r; + double s, t, dist, dist2; + btVector3 witness2; + + btVec3Sub2(&d1, B, x0); + btVec3Sub2(&d2, C, x0); + btVec3Sub2(&a, x0, P); + + u = btVec3Dot(&a, &a); + v = btVec3Dot(&d1, &d1); + w = btVec3Dot(&d2, &d2); + p = btVec3Dot(&a, &d1); + q = btVec3Dot(&a, &d2); + r = btVec3Dot(&d1, &d2); + + s = (q * r - w * p) / (w * v - r * r); + t = (-s * r - q) / w; + + if ((btFuzzyZero(s) || s > btScalar(0)) && (ccdEq(s, btScalar(1)) || s < btScalar(1)) && (btFuzzyZero(t) || t > btScalar(0)) && (ccdEq(t, btScalar(1)) || t < btScalar(1)) && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1))) + { + if (witness) + { + btVec3Scale(&d1, s); + btVec3Scale(&d2, t); + btVec3Copy(witness, x0); + ccdVec3Add(witness, &d1); + ccdVec3Add(witness, &d2); + + dist = ccdVec3Dist2(witness, P); + } + else + { + dist = s * s * v; + dist += t * t * w; + dist += btScalar(2.) * s * t * r; + dist += btScalar(2.) * s * p; + dist += btScalar(2.) * t * q; + dist += u; + } + } + else + { + dist = btVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist) + { + dist = dist2; + if (witness) + btVec3Copy(witness, &witness2); + } + + dist2 = btVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist) + { + dist = dist2; + if (witness) + btVec3Copy(witness, &witness2); + } + } + + return dist; +} + +static int btDoSimplex2(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B; + btVector3 AB, AO, tmp; + btScalar dot; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other point + B = btSimplexPoint(simplex, 0); + // compute AB oriented segment + btVec3Sub2(&AB, &B->v, &A->v); + // compute AO vector + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + + // dot product AB . AO + dot = btVec3Dot(&AB, &AO); + + // check if origin doesn't lie on AB segment + btVec3Cross(&tmp, &AB, &AO); + if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0)) + { + return 1; + } + + // check if origin is in area where AB segment is + if (btFuzzyZero(dot) || dot < btScalar(0)) + { + // origin is in outside are of A + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + else + { + // origin is in area where AB segment is + + // keep simplex untouched and set direction to + // AB x AO x AB + btTripleCross(&AB, &AO, &AB, dir); + } + + return 0; +} + +static int btDoSimplex3(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B, *C; + btVector3 AO, AB, AC, ABC, tmp; + btScalar dot, dist; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other points + B = btSimplexPoint(simplex, 1); + C = btSimplexPoint(simplex, 0); + + // check touching contact + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0); + if (btFuzzyZero(dist)) + { + return 1; + } + + // check if triangle is really triangle (has area > 0) + // if not simplex can't be expanded and thus no itersection is found + if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v)) + { + return -1; + } + + // compute AO vector + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + + // compute AB and AC segments and ABC vector (perpendircular to triangle) + btVec3Sub2(&AB, &B->v, &A->v); + btVec3Sub2(&AC, &C->v, &A->v); + btVec3Cross(&ABC, &AB, &AC); + + btVec3Cross(&tmp, &ABC, &AC); + dot = btVec3Dot(&tmp, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + dot = btVec3Dot(&AC, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + // C is already in place + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AC, &AO, &AC, dir); + } + else + { + dot = btVec3Dot(&AB, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AB, &AO, &AB, dir); + } + else + { + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + } + } + else + { + btVec3Cross(&tmp, &AB, &ABC); + dot = btVec3Dot(&tmp, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + dot = btVec3Dot(&AB, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AB, &AO, &AB, dir); + } + else + { + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + } + else + { + dot = btVec3Dot(&ABC, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + btVec3Copy(dir, &ABC); + } + else + { + btSupportVector tmp; + btSupportCopy(&tmp, C); + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, &tmp); + + btVec3Copy(dir, &ABC); + btVec3Scale(dir, -btScalar(1)); + } + } + } + + return 0; +} + +static int btDoSimplex4(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B, *C, *D; + btVector3 AO, AB, AC, AD, ABC, ACD, ADB; + int B_on_ACD, C_on_ADB, D_on_ABC; + int AB_O, AC_O, AD_O; + btScalar dist; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other points + B = btSimplexPoint(simplex, 2); + C = btSimplexPoint(simplex, 1); + D = btSimplexPoint(simplex, 0); + + // check if tetrahedron is really tetrahedron (has volume > 0) + // if it is not simplex can't be expanded and thus no intersection is + // found + dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + { + return -1; + } + + // check if origin lies on some of tetrahedron's face - if so objects + // intersect + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + + // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + btVec3Sub2(&AB, &B->v, &A->v); + btVec3Sub2(&AC, &C->v, &A->v); + btVec3Sub2(&AD, &D->v, &A->v); + btVec3Cross(&ABC, &AB, &AC); + btVec3Cross(&ACD, &AC, &AD); + btVec3Cross(&ADB, &AD, &AB); + + // side (positive or negative) of B, C, D relative to planes ACD, ADB + // and ABC respectively + B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB)); + C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC)); + D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD)); + + // whether origin is on same side of ACD, ADB, ABC as B, C, D + // respectively + AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD; + AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB; + AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC; + + if (AB_O && AC_O && AD_O) + { + // origin is in tetrahedron + return 1; + // rearrange simplex to triangle and call btDoSimplex3() + } + else if (!AB_O) + { + // B is farthest from the origin among all of the tetrahedron's + // points, so remove it from the list and go on with the triangle + // case + + // D and C are in place + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + else if (!AC_O) + { + // C is farthest + btSimplexSet(simplex, 1, D); + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + else + { // (!AD_O) + btSimplexSet(simplex, 0, C); + btSimplexSet(simplex, 1, B); + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + + return btDoSimplex3(simplex, dir); +} + +static int btDoSimplex(btSimplex *simplex, btVector3 *dir) +{ + if (btSimplexSize(simplex) == 2) + { + // simplex contains segment only one segment + return btDoSimplex2(simplex, dir); + } + else if (btSimplexSize(simplex) == 3) + { + // simplex contains triangle + return btDoSimplex3(simplex, dir); + } + else + { // btSimplexSize(simplex) == 4 + // tetrahedron - this is the only shape which can encapsule origin + // so btDoSimplex4() also contains test on it + return btDoSimplex4(simplex, dir); + } } #ifdef __SPU__ -void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw) #else -void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw) +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw) #endif { m_cachedSeparatingDistance = 0.f; - btScalar distance=btScalar(0.); - btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar distance = btScalar(0.); + btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.)); - btVector3 pointOnA,pointOnB; - btTransform localTransA = input.m_transformA; + btVector3 pointOnA, pointOnB; + btTransform localTransA = input.m_transformA; btTransform localTransB = input.m_transformB; - btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); + btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; @@ -106,7 +705,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu btScalar marginA = m_marginA; btScalar marginB = m_marginB; - gNumGjkChecks++; //for CCD we don't use margins if (m_ignoreMargin) @@ -116,8 +714,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu } m_curIter = 0; - int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? - m_cachedSeparatingAxis.setValue(0,1,0); + int gGjkMaxIter = 1000; //this is to catch invalid input, perhaps check for #NaN? + m_cachedSeparatingAxis.setValue(0, 1, 0); bool isValid = false; bool checkSimplex = false; @@ -125,191 +723,291 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu m_degenerateSimplex = 0; m_lastUsedMethod = -1; + int status = -2; + btVector3 orgNormalInB(0, 0, 0); + btScalar margin = marginA + marginB; + //we add a separate implementation to check if the convex shapes intersect + //See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang + //Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver + //and remove this temporary code from libCCD + //this fixes issue https://github.com/bulletphysics/bullet3/issues/1703 + //note, for large differences in shapes, use double precision build! { btScalar squaredDistance = BT_LARGE_FLOAT; btScalar delta = btScalar(0.); - - btScalar margin = marginA + marginB; - - + + btSimplex simplex1; + btSimplex *simplex = &simplex1; + btSimplexInit(simplex); + + btVector3 dir(1, 0, 0); + + { + btVector3 lastSupV; + btVector3 supAworld; + btVector3 supBworld; + btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV); + + btSupportVector last; + last.v = lastSupV; + last.v1 = supAworld; + last.v2 = supBworld; + + btSimplexAdd(simplex, &last); + + dir = -lastSupV; + + // start iterations + for (int iterations = 0; iterations < gGjkMaxIter; iterations++) + { + // obtain support point + btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV); + + // check if farthest point in Minkowski difference in direction dir + // isn't somewhere before origin (the test on negative dot product) + // - because if it is, objects are not intersecting at all. + btScalar delta = lastSupV.dot(dir); + if (delta < 0) + { + //no intersection, besides margin + status = -1; + break; + } + + // add last support vector to simplex + last.v = lastSupV; + last.v1 = supAworld; + last.v2 = supBworld; + + btSimplexAdd(simplex, &last); + + // if btDoSimplex returns 1 if objects intersect, -1 if objects don't + // intersect and 0 if algorithm should continue + + btVector3 newDir; + int do_simplex_res = btDoSimplex(simplex, &dir); + + if (do_simplex_res == 1) + { + status = 0; // intersection found + break; + } + else if (do_simplex_res == -1) + { + // intersection not found + status = -1; + break; + } + + if (btFuzzyZero(btVec3Dot(&dir, &dir))) + { + // intersection not found + status = -1; + } + + if (dir.length2() < SIMD_EPSILON) + { + //no intersection, besides margin + status = -1; + break; + } + + if (dir.fuzzyZero()) + { + // intersection not found + status = -1; + break; + } + } + } m_simplexSolver->reset(); - - for ( ; ; ) - //while (true) + if (status == 0) { + //status = 0; + //printf("Intersect!\n"); + } - btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); - - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - - - if (check2d) + if (status == -1) + { + //printf("not intersect\n"); + } + //printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]); + if (1) + { + for (;;) + //while (true) { - pWorld[2] = 0.f; - qWorld[2] = 0.f; - } + btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis(); + btVector3 seperatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis(); - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) - { - m_degenerateSimplex = 10; - checkSimplex=true; - //checkPenetration = false; - break; - } + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); - //exit 0: the new point is already in the simplex, or we didn't come any closer - if (m_simplexSolver->inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * REL_ERROR2; - - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) + if (check2d) { - m_degenerateSimplex = 2; - } else - { - m_degenerateSimplex = 11; + pWorld[2] = 0.f; + qWorld[2] = 0.f; } - checkSimplex = true; - break; - } - //add current vertex to simplex - m_simplexSolver->addVertex(w, pWorld, qWorld); - btVector3 newCachedSeparatingAxis; + btVector3 w = pWorld - qWorld; + delta = m_cachedSeparatingAxis.dot(w); - //calculate the closest point to the origin (update vector v) - if (!m_simplexSolver->closest(newCachedSeparatingAxis)) - { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } + // potential exit, they don't overlap + if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) + { + m_degenerateSimplex = 10; + checkSimplex = true; + //checkPenetration = false; + break; + } - if(newCachedSeparatingAxis.length2()inSimplex(w)) + { + m_degenerateSimplex = 1; + checkSimplex = true; + break; + } + // are we getting any closer ? + btScalar f0 = squaredDistance - delta; + btScalar f1 = squaredDistance * REL_ERROR2; - btScalar previousSquaredDistance = squaredDistance; - squaredDistance = newCachedSeparatingAxis.length2(); + if (f0 <= f1) + { + if (f0 <= btScalar(0.)) + { + m_degenerateSimplex = 2; + } + else + { + m_degenerateSimplex = 11; + } + checkSimplex = true; + break; + } + + //add current vertex to simplex + m_simplexSolver->addVertex(w, pWorld, qWorld); + btVector3 newCachedSeparatingAxis; + + //calculate the closest point to the origin (update vector v) + if (!m_simplexSolver->closest(newCachedSeparatingAxis)) + { + m_degenerateSimplex = 3; + checkSimplex = true; + break; + } + + if (newCachedSeparatingAxis.length2() < REL_ERROR2) + { + m_cachedSeparatingAxis = newCachedSeparatingAxis; + m_degenerateSimplex = 6; + checkSimplex = true; + break; + } + + btScalar previousSquaredDistance = squaredDistance; + squaredDistance = newCachedSeparatingAxis.length2(); #if 0 -///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo - if (squaredDistance>previousSquaredDistance) - { - m_degenerateSimplex = 7; - squaredDistance = previousSquaredDistance; - checkSimplex = false; - break; - } -#endif // - + ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo + if (squaredDistance > previousSquaredDistance) + { + m_degenerateSimplex = 7; + squaredDistance = previousSquaredDistance; + checkSimplex = false; + break; + } +#endif // - //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); + //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; + //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, shapeTypeA=%i,shapeTypeB=%i\n", + m_cachedSeparatingAxis.getX(), + m_cachedSeparatingAxis.getY(), + m_cachedSeparatingAxis.getZ(), + squaredDistance, + m_minkowskiA->getShapeType(), + m_minkowskiB->getShapeType()); + +#endif + break; + } + + bool check = (!m_simplexSolver->fullSimplex()); + //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); + + if (!check) + { + //do we need this backup_closest here ? + // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + m_degenerateSimplex = 13; + break; + } } - 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, shapeTypeA=%i,shapeTypeB=%i\n", - m_cachedSeparatingAxis.getX(), - m_cachedSeparatingAxis.getY(), - m_cachedSeparatingAxis.getZ(), - squaredDistance, - m_minkowskiA->getShapeType(), - m_minkowskiB->getShapeType()); - - #endif - break; - - } - - - bool check = (!m_simplexSolver->fullSimplex()); - //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); - - if (!check) + if (checkSimplex) { - //do we need this backup_closest here ? -// m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - m_degenerateSimplex = 13; - break; + m_simplexSolver->compute_points(pointOnA, pointOnB); + normalInB = m_cachedSeparatingAxis; + + btScalar lenSqr = m_cachedSeparatingAxis.length2(); + + //valid normal + if (lenSqr < REL_ERROR2) + { + 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; + orgNormalInB = normalInB; + + m_lastUsedMethod = 1; + } + else + { + m_lastUsedMethod = 2; + } } } - if (checkSimplex) - { - m_simplexSolver->compute_points(pointOnA, pointOnB); - normalInB = m_cachedSeparatingAxis; - - btScalar lenSqr =m_cachedSeparatingAxis.length2(); - - //valid normal - if (lenSqr < REL_ERROR2) - { - m_degenerateSimplex = 5; - } - if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - normalInB *= rlen; //normalize - - btScalar s = btSqrt(squaredDistance); - - btAssert(s > btScalar(0.0)); - pointOnA -= m_cachedSeparatingAxis * (marginA / s); - pointOnB += m_cachedSeparatingAxis * (marginB / s); - distance = ((btScalar(1.)/rlen) - margin); - isValid = true; - - m_lastUsedMethod = 1; - } else - { - m_lastUsedMethod = 2; - } - } - - bool catchDegeneratePenetrationCase = - (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance)); + bool catchDegeneratePenetrationCase = + (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance + margin) < gGjkEpaPenetrationTolerance)); //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) + if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0)) { //penetration case @@ -317,151 +1015,169 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu if (m_penetrationDepthSolver) { // Penetration depth case. - btVector3 tmpPointOnA,tmpPointOnB; - - gNumDeepPenetrationChecks++; + btVector3 tmpPointOnA, tmpPointOnB; + m_cachedSeparatingAxis.setZero(); - bool isValid2 = m_penetrationDepthSolver->calcPenDepth( - *m_simplexSolver, - m_minkowskiA,m_minkowskiB, - localTransA,localTransB, + bool isValid2 = m_penetrationDepthSolver->calcPenDepth( + *m_simplexSolver, + m_minkowskiA, m_minkowskiB, + localTransA, localTransB, m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, - debugDraw - ); + debugDraw); - - if (isValid2) + if (m_cachedSeparatingAxis.length2()) { - btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - btScalar lenSqr = tmpNormalInB.length2(); - if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) + if (isValid2) { - tmpNormalInB = m_cachedSeparatingAxis; - lenSqr = m_cachedSeparatingAxis.length2(); - } - - if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB /= btSqrt(lenSqr); - btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); - m_lastUsedMethod = 3; - //only replace valid penetrations when the result is deeper (check) - if (!isValid || (distance2 < distance)) + btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; + btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON)) { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - normalInB = tmpNormalInB; - - isValid = true; - - } else - { - m_lastUsedMethod = 8; + tmpNormalInB = m_cachedSeparatingAxis; + lenSqr = m_cachedSeparatingAxis.length2(); + } + + if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON)) + { + tmpNormalInB /= btSqrt(lenSqr); + btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length(); + m_lastUsedMethod = 3; + //only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + isValid = true; + } + else + { + m_lastUsedMethod = 8; + } + } + else + { + m_lastUsedMethod = 9; } - } else - { - m_lastUsedMethod = 9; } - } else + 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(); + ///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 - isValid = true; - m_lastUsedMethod = 6; - } else + if (m_cachedSeparatingAxis.length2() > btScalar(0.)) { - m_lastUsedMethod = 5; + 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; + } } } } - + else + { + //printf("EPA didn't return a valid value\n"); + } } - } } - - - if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) + if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared))) { - m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; - + if (1) { - ///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 + ///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; + btScalar d2 = 0.f; { - btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis(); - + btVector3 seperatingAxisInA = (-orgNormalInB) * localTransA.getBasis(); + btVector3 seperatingAxisInB = orgNormalInB * localTransB.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); + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d2 = orgNormalInB.dot(w) - margin; + } + + btScalar d1 = 0; + { + btVector3 seperatingAxisInA = (normalInB)*localTransA.getBasis(); + btVector3 seperatingAxisInB = -normalInB * localTransB.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) - margin; } btScalar d0 = 0.f; { - btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis(); - + 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); + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d0 = normalInB.dot(w) - margin; } - if (d1>d0) + + if (d1 > d0) { m_lastUsedMethod = 10; - normalInB*=-1; - } + normalInB *= -1; + } + if (orgNormalInB.length2()) + { + if (d2 > d0 && d2 > d1 && d2 > distance) + { + normalInB = orgNormalInB; + distance = d2; + } + } } + output.addContactPoint( normalInB, - pointOnB+positionOffset, + pointOnB + positionOffset, distance); - } - - + else + { + //printf("invalid gjk query\n"); + } } - - - - - diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index feeae6862..659b63551 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -13,9 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - - #ifndef BT_GJK_PAIR_DETECTOR_H #define BT_GJK_PAIR_DETECTOR_H @@ -29,39 +26,34 @@ class btConvexPenetrationDepthSolver; /// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface class btGjkPairDetector : public btDiscreteCollisionDetectorInterface { - - - btVector3 m_cachedSeparatingAxis; - btConvexPenetrationDepthSolver* m_penetrationDepthSolver; + btVector3 m_cachedSeparatingAxis; + btConvexPenetrationDepthSolver* m_penetrationDepthSolver; btSimplexSolverInterface* m_simplexSolver; const btConvexShape* m_minkowskiA; const btConvexShape* m_minkowskiB; - int m_shapeTypeA; + int m_shapeTypeA; int m_shapeTypeB; - btScalar m_marginA; - btScalar m_marginB; + btScalar m_marginA; + btScalar m_marginB; - bool m_ignoreMargin; - btScalar m_cachedSeparatingDistance; - + bool m_ignoreMargin; + btScalar m_cachedSeparatingDistance; public: - //some debugging to fix degeneracy problems - int m_lastUsedMethod; - int m_curIter; - int m_degenerateSimplex; - int m_catchDegeneracies; - int m_fixContactNormalDirection; + int m_lastUsedMethod; + int m_curIter; + int m_degenerateSimplex; + int m_catchDegeneracies; + int m_fixContactNormalDirection; - btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); - btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); - virtual ~btGjkPairDetector() {}; + btGjkPairDetector(const btConvexShape* objectA, const btConvexShape* objectB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver); + btGjkPairDetector(const btConvexShape* objectA, const btConvexShape* objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver); + virtual ~btGjkPairDetector(){}; - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false); - void getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); - + void getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw); void setMinkowskiA(const btConvexShape* minkA) { @@ -81,23 +73,21 @@ public: { return m_cachedSeparatingAxis; } - btScalar getCachedSeparatingDistance() const + btScalar getCachedSeparatingDistance() const { return m_cachedSeparatingDistance; } - void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) + void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) { m_penetrationDepthSolver = penetrationDepthSolver; } ///don't use setIgnoreMargin, it's for Bullet's internal use - void setIgnoreMargin(bool ignoreMargin) + void setIgnoreMargin(bool ignoreMargin) { m_ignoreMargin = ignoreMargin; } - - }; -#endif //BT_GJK_PAIR_DETECTOR_H +#endif //BT_GJK_PAIR_DETECTOR_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 571ad2c5f..6d21f6620 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -20,161 +20,152 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" #ifdef PFX_USE_FREE_VECTORMATH - #include "physics_effects/base_level/solver/pfx_constraint_row.h" +#include "physics_effects/base_level/solver/pfx_constraint_row.h" typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow; #else - // Don't change following order of parameters - ATTRIBUTE_ALIGNED16(struct) btConstraintRow { - btScalar m_normal[3]; - btScalar m_rhs; - btScalar m_jacDiagInv; - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_accumImpulse; - }; - typedef btConstraintRow PfxConstraintRow; -#endif //PFX_USE_FREE_VECTORMATH +// Don't change following order of parameters +ATTRIBUTE_ALIGNED16(struct) +btConstraintRow +{ + btScalar m_normal[3]; + btScalar m_rhs; + btScalar m_jacDiagInv; + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_accumImpulse; +}; +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_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. class btManifoldPoint +{ +public: + btManifoldPoint() + : m_userPersistentData(0), + m_contactPointFlags(0), + m_appliedImpulse(0.f), + m_appliedImpulseLateral1(0.f), + m_appliedImpulseLateral2(0.f), + m_contactMotion1(0.f), + m_contactMotion2(0.f), + m_contactCFM(0.f), + m_contactERP(0.f), + m_frictionCFM(0.f), + m_lifeTime(0) { - public: - btManifoldPoint() - :m_userPersistentData(0), - m_contactPointFlags(0), - m_appliedImpulse(0.f), - m_appliedImpulseLateral1(0.f), - m_appliedImpulseLateral2(0.f), - m_contactMotion1(0.f), - m_contactMotion2(0.f), - m_contactCFM(0.f), - m_contactERP(0.f), - m_frictionCFM(0.f), - m_lifeTime(0) - { - } + } - btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB, - const btVector3 &normal, - btScalar distance ) : - m_localPointA( pointA ), - m_localPointB( pointB ), - m_normalWorldOnB( normal ), - m_distance1( distance ), - m_combinedFriction(btScalar(0.)), - m_combinedRollingFriction(btScalar(0.)), - m_combinedSpinningFriction(btScalar(0.)), - m_combinedRestitution(btScalar(0.)), - m_userPersistentData(0), - m_contactPointFlags(0), - m_appliedImpulse(0.f), - m_appliedImpulseLateral1(0.f), - m_appliedImpulseLateral2(0.f), - m_contactMotion1(0.f), - m_contactMotion2(0.f), - m_contactCFM(0.f), - m_contactERP(0.f), - m_frictionCFM(0.f), - m_lifeTime(0) - { - - } + btManifoldPoint(const btVector3& pointA, const btVector3& pointB, + const btVector3& normal, + btScalar distance) : m_localPointA(pointA), + m_localPointB(pointB), + m_normalWorldOnB(normal), + m_distance1(distance), + m_combinedFriction(btScalar(0.)), + m_combinedRollingFriction(btScalar(0.)), + m_combinedSpinningFriction(btScalar(0.)), + m_combinedRestitution(btScalar(0.)), + m_userPersistentData(0), + m_contactPointFlags(0), + m_appliedImpulse(0.f), + m_appliedImpulseLateral1(0.f), + m_appliedImpulseLateral2(0.f), + m_contactMotion1(0.f), + m_contactMotion2(0.f), + m_contactCFM(0.f), + m_contactERP(0.f), + m_frictionCFM(0.f), + m_lifeTime(0) + { + } - + btVector3 m_localPointA; + btVector3 m_localPointB; + btVector3 m_positionWorldOnB; + ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity + btVector3 m_positionWorldOnA; + btVector3 m_normalWorldOnB; - btVector3 m_localPointA; - btVector3 m_localPointB; - btVector3 m_positionWorldOnB; - ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity - btVector3 m_positionWorldOnA; - btVector3 m_normalWorldOnB; - - btScalar m_distance1; - btScalar m_combinedFriction; - 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; + btScalar m_distance1; + btScalar m_combinedFriction; + 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; - int m_partId1; - int m_index0; - int m_index1; - - mutable void* m_userPersistentData; - //bool m_lateralFrictionInitialized; - int m_contactPointFlags; - - btScalar m_appliedImpulse; - btScalar m_appliedImpulseLateral1; - btScalar m_appliedImpulseLateral2; - btScalar m_contactMotion1; - btScalar m_contactMotion2; - - union - { - btScalar m_contactCFM; - btScalar m_combinedContactStiffness1; - }; - - union - { - btScalar m_contactERP; - btScalar m_combinedContactDamping1; - }; + //BP mod, store contact triangles. + int m_partId0; + int m_partId1; + int m_index0; + int m_index1; - btScalar m_frictionCFM; + mutable void* m_userPersistentData; + //bool m_lateralFrictionInitialized; + int m_contactPointFlags; - int m_lifeTime;//lifetime of the contactpoint in frames - - btVector3 m_lateralFrictionDir1; - btVector3 m_lateralFrictionDir2; - - - - - btScalar getDistance() const - { - return m_distance1; - } - int getLifeTime() const - { - return m_lifeTime; - } - - const btVector3& getPositionWorldOnA() const { - return m_positionWorldOnA; -// return m_positionWorldOnB + m_normalWorldOnB * m_distance1; - } - - const btVector3& getPositionWorldOnB() const - { - return m_positionWorldOnB; - } - - void setDistance(btScalar dist) - { - m_distance1 = dist; - } - - ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver - btScalar getAppliedImpulse() const - { - return m_appliedImpulse; - } - - + btScalar m_appliedImpulse; + btScalar m_appliedImpulseLateral1; + btScalar m_appliedImpulseLateral2; + btScalar m_contactMotion1; + btScalar m_contactMotion2; + union { + btScalar m_contactCFM; + btScalar m_combinedContactStiffness1; }; -#endif //BT_MANIFOLD_CONTACT_POINT_H + union { + btScalar m_contactERP; + btScalar m_combinedContactDamping1; + }; + + btScalar m_frictionCFM; + + int m_lifeTime; //lifetime of the contactpoint in frames + + btVector3 m_lateralFrictionDir1; + btVector3 m_lateralFrictionDir2; + + btScalar getDistance() const + { + return m_distance1; + } + int getLifeTime() const + { + return m_lifeTime; + } + + const btVector3& getPositionWorldOnA() const + { + return m_positionWorldOnA; + // return m_positionWorldOnB + m_normalWorldOnB * m_distance1; + } + + const btVector3& getPositionWorldOnB() const + { + return m_positionWorldOnB; + } + + void setDistance(btScalar dist) + { + m_distance1 = dist; + } + + ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver + btScalar getAppliedImpulse() const + { + return m_appliedImpulse; + } +}; + +#endif //BT_MANIFOLD_CONTACT_POINT_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index fa45f4903..a77449812 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -21,42 +21,38 @@ subject to the following restrictions: #define NUM_UNITSPHERE_POINTS 42 - bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw - ) + const btConvexShape* convexA, const btConvexShape* convexB, + const btTransform& transA, const btTransform& transB, + btVector3& v, btVector3& pa, btVector3& pb, + class btIDebugDraw* debugDraw) { - (void)v; - - bool check2d= convexA->isConvex2d() && convexB->isConvex2d(); + + bool check2d = convexA->isConvex2d() && convexB->isConvex2d(); struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result { - - btIntermediateResult():m_hasResult(false) + btIntermediateResult() : m_hasResult(false) { } - + btVector3 m_normalOnBInWorld; btVector3 m_pointInWorld; btScalar m_depth; - bool m_hasResult; + bool m_hasResult; - virtual void setShapeIdentifiersA(int partId0,int index0) + virtual void setShapeIdentifiersA(int partId0, int index0) { (void)partId0; (void)index0; } - virtual void setShapeIdentifiersB(int partId1,int index1) + virtual void setShapeIdentifiersB(int partId1, int index1) { (void)partId1; (void)index1; } - void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { m_normalOnBInWorld = normalOnBInWorld; m_pointInWorld = pointInWorld; @@ -68,39 +64,39 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s //just take fixed number of orientation, and sample the penetration depth in that direction btScalar minProj = btScalar(BT_LARGE_FLOAT); btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.)); - btVector3 minA,minB; - btVector3 seperatingAxisInA,seperatingAxisInB; - btVector3 pInA,qInB,pWorld,qWorld,w; + btVector3 minA, minB; + btVector3 seperatingAxisInA, seperatingAxisInB; + btVector3 pInA, qInB, pWorld, qWorld, w; #ifndef __SPU__ #define USE_BATCHED_SUPPORT 1 #endif #ifdef USE_BATCHED_SUPPORT - btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; + btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; + btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; + btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int i; int numSampleDirections = NUM_UNITSPHERE_POINTS; - for (i=0;igetNumPreferredPenetrationDirections(); if (numPDA) { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - norm = transA.getBasis() * norm; + convexA->getPreferredPenetrationDirection(i, norm); + norm = transA.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); @@ -113,11 +109,11 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numPDB = convexB->getNumPreferredPenetrationDirections(); if (numPDB) { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - norm = transB.getBasis() * norm; + convexB->getPreferredPenetrationDirection(i, norm); + norm = transB.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); @@ -126,29 +122,25 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s } } + convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections); + convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections); - - - convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); - convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); - - for (i=0;i0.01) + if (norm.length2() > 0.01) { - seperatingAxisInA = seperatingAxisInABatch[i]; seperatingAxisInB = seperatingAxisInBBatch[i]; pInA = supportVerticesABatch[i]; qInB = supportVerticesBBatch[i]; - pWorld = transA(pInA); + pWorld = transA(pInA); qWorld = transB(qInB); if (check2d) { @@ -156,7 +148,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s qWorld[2] = 0.f; } - w = qWorld - pWorld; + w = qWorld - pWorld; btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) @@ -167,7 +159,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minB = qWorld; } } - } + } #else int numSampleDirections = NUM_UNITSPHERE_POINTS; @@ -177,11 +169,11 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numPDA = convexA->getNumPreferredPenetrationDirections(); if (numPDA) { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - norm = transA.getBasis() * norm; + convexA->getPreferredPenetrationDirection(i, norm); + norm = transA.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } @@ -192,28 +184,28 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numPDB = convexB->getNumPreferredPenetrationDirections(); if (numPDB) { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - norm = transB.getBasis() * norm; + convexB->getPreferredPenetrationDirection(i, norm); + norm = transB.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } } -#endif // __SPU__ +#endif // __SPU__ - for (int i=0;ilocalGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - pWorld = transA(pInA); + pWorld = transA(pInA); qWorld = transB(qInB); - w = qWorld - pWorld; + w = qWorld - pWorld; btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) @@ -224,48 +216,39 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minB = qWorld; } } -#endif //USE_BATCHED_SUPPORT +#endif //USE_BATCHED_SUPPORT //add the margins - minA += minNorm*convexA->getMarginNonVirtual(); - minB -= minNorm*convexB->getMarginNonVirtual(); + minA += minNorm * convexA->getMarginNonVirtual(); + minB -= minNorm * convexB->getMarginNonVirtual(); //no penetration if (minProj < btScalar(0.)) return false; - btScalar extraSeparation = 0.5f;///scale dependent - minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); - - - - + btScalar extraSeparation = 0.5f; ///scale dependent + minProj += extraSeparation + (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); //#define DEBUG_DRAW 1 #ifdef DEBUG_DRAW if (debugDraw) { - btVector3 color(0,1,0); - debugDraw->drawLine(minA,minB,color); - color = btVector3 (1,1,1); - btVector3 vec = minB-minA; + btVector3 color(0, 1, 0); + debugDraw->drawLine(minA, minB, color); + color = btVector3(1, 1, 1); + btVector3 vec = minB - minA; btScalar prj2 = minNorm.dot(vec); - debugDraw->drawLine(minA,minA+(minNorm*minProj),color); - + debugDraw->drawLine(minA, minA + (minNorm * minProj), color); } -#endif //DEBUG_DRAW +#endif //DEBUG_DRAW - - - btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); + btGjkPairDetector gjkdet(convexA, convexB, &simplexSolver, 0); btScalar offsetDist = minProj; btVector3 offset = minNorm * offsetDist; - - btGjkPairDetector::ClosestPointInput input; - + btVector3 newOrg = transA.getOrigin() + offset; btTransform displacedTrans = transA; @@ -273,89 +256,81 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_transformA = displacedTrans; input.m_transformB = transB; - input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; - + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); //minProj; + btIntermediateResult res; gjkdet.setCachedSeperatingAxis(-minNorm); - gjkdet.getClosestPoints(input,res,debugDraw); + gjkdet.getClosestPoints(input, res, debugDraw); btScalar correctedMinNorm = minProj - res.m_depth; - //the penetration depth is over-estimated, relax it - btScalar penetration_relaxation= btScalar(1.); - minNorm*=penetration_relaxation; - + btScalar penetration_relaxation = btScalar(1.); + minNorm *= penetration_relaxation; if (res.m_hasResult) { - pa = res.m_pointInWorld - minNorm * correctedMinNorm; pb = res.m_pointInWorld; v = minNorm; - + #ifdef DEBUG_DRAW if (debugDraw) { - btVector3 color(1,0,0); - debugDraw->drawLine(pa,pb,color); + btVector3 color(1, 0, 0); + debugDraw->drawLine(pa, pb, color); } -#endif//DEBUG_DRAW - - +#endif //DEBUG_DRAW } return res.m_hasResult; } -btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections() +btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections() { - static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = - { - btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), - btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), - btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), - btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), - btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), - btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), - btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), - btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), - btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), - btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), - btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), - btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), - btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), - btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), - btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), - btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), - btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), - btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), - btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), - btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), - btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), - btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), - btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), - btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), - btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), - btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), - btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), - btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), - btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), - btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) - }; + static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] = + { + btVector3(btScalar(0.000000), btScalar(-0.000000), btScalar(-1.000000)), + btVector3(btScalar(0.723608), btScalar(-0.525725), btScalar(-0.447219)), + btVector3(btScalar(-0.276388), btScalar(-0.850649), btScalar(-0.447219)), + btVector3(btScalar(-0.894426), btScalar(-0.000000), btScalar(-0.447216)), + btVector3(btScalar(-0.276388), btScalar(0.850649), btScalar(-0.447220)), + btVector3(btScalar(0.723608), btScalar(0.525725), btScalar(-0.447219)), + btVector3(btScalar(0.276388), btScalar(-0.850649), btScalar(0.447220)), + btVector3(btScalar(-0.723608), btScalar(-0.525725), btScalar(0.447219)), + btVector3(btScalar(-0.723608), btScalar(0.525725), btScalar(0.447219)), + btVector3(btScalar(0.276388), btScalar(0.850649), btScalar(0.447219)), + btVector3(btScalar(0.894426), btScalar(0.000000), btScalar(0.447216)), + btVector3(btScalar(-0.000000), btScalar(0.000000), btScalar(1.000000)), + btVector3(btScalar(0.425323), btScalar(-0.309011), btScalar(-0.850654)), + btVector3(btScalar(-0.162456), btScalar(-0.499995), btScalar(-0.850654)), + btVector3(btScalar(0.262869), btScalar(-0.809012), btScalar(-0.525738)), + btVector3(btScalar(0.425323), btScalar(0.309011), btScalar(-0.850654)), + btVector3(btScalar(0.850648), btScalar(-0.000000), btScalar(-0.525736)), + btVector3(btScalar(-0.525730), btScalar(-0.000000), btScalar(-0.850652)), + btVector3(btScalar(-0.688190), btScalar(-0.499997), btScalar(-0.525736)), + btVector3(btScalar(-0.162456), btScalar(0.499995), btScalar(-0.850654)), + btVector3(btScalar(-0.688190), btScalar(0.499997), btScalar(-0.525736)), + btVector3(btScalar(0.262869), btScalar(0.809012), btScalar(-0.525738)), + btVector3(btScalar(0.951058), btScalar(0.309013), btScalar(0.000000)), + btVector3(btScalar(0.951058), btScalar(-0.309013), btScalar(0.000000)), + btVector3(btScalar(0.587786), btScalar(-0.809017), btScalar(0.000000)), + btVector3(btScalar(0.000000), btScalar(-1.000000), btScalar(0.000000)), + btVector3(btScalar(-0.587786), btScalar(-0.809017), btScalar(0.000000)), + btVector3(btScalar(-0.951058), btScalar(-0.309013), btScalar(-0.000000)), + btVector3(btScalar(-0.951058), btScalar(0.309013), btScalar(-0.000000)), + btVector3(btScalar(-0.587786), btScalar(0.809017), btScalar(-0.000000)), + btVector3(btScalar(-0.000000), btScalar(1.000000), btScalar(-0.000000)), + btVector3(btScalar(0.587786), btScalar(0.809017), btScalar(-0.000000)), + btVector3(btScalar(0.688190), btScalar(-0.499997), btScalar(0.525736)), + btVector3(btScalar(-0.262869), btScalar(-0.809012), btScalar(0.525738)), + btVector3(btScalar(-0.850648), btScalar(0.000000), btScalar(0.525736)), + btVector3(btScalar(-0.262869), btScalar(0.809012), btScalar(0.525738)), + btVector3(btScalar(0.688190), btScalar(0.499997), btScalar(0.525736)), + btVector3(btScalar(0.525730), btScalar(0.000000), btScalar(0.850652)), + btVector3(btScalar(0.162456), btScalar(-0.499995), btScalar(0.850654)), + btVector3(btScalar(-0.425323), btScalar(-0.309011), btScalar(0.850654)), + btVector3(btScalar(-0.425323), btScalar(0.309011), btScalar(0.850654)), + btVector3(btScalar(0.162456), btScalar(0.499995), btScalar(0.850654))}; return sPenetrationDirections; } - - diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h index fd533b4fc..8e3e39325 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -23,18 +23,14 @@ subject to the following restrictions: class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver { protected: - - static btVector3* getPenetrationDirections(); + static btVector3* getPenetrationDirections(); public: - - virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw - ); + virtual bool calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* convexA, const btConvexShape* convexB, + const btTransform& transA, const btTransform& transB, + btVector3& v, btVector3& pa, btVector3& pb, + class btIDebugDraw* debugDraw); }; -#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H - +#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h index a22a0bae6..94d0f284f 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h @@ -28,32 +28,31 @@ //#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() - { - } + 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; + btVector3 m_pointOnA; + btVector3 m_pointOnB; + btVector3 m_normalBtoA; + btScalar m_distance; }; #ifdef __cplusplus @@ -67,131 +66,112 @@ struct btMprDistanceInfo #define BT_MPR_TOLERANCE 1E-6f #define BT_MPR_MAX_ITERATIONS 1000 -struct _btMprSupport_t +struct _btMprSupport_t { - btVector3 v; //!< Support point in minkowski sum - btVector3 v1; //!< Support point in obj1 - btVector3 v2; //!< Support point in obj2 + 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 +struct _btMprSimplex_t { - btMprSupport_t ps[4]; - int last; //!< index of last added point + 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) +inline btMprSupport_t *btMprSimplexPointW(btMprSimplex_t *s, int idx) { - return &s->ps[idx]; + return &s->ps[idx]; } inline void btMprSimplexSetSize(btMprSimplex_t *s, int size) { - s->last = size - 1; + s->last = size - 1; } #ifdef DEBUG_MPR -inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index) +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()); + 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 - - - +#endif //DEBUG_MPR inline int btMprSimplexSize(const btMprSimplex_t *s) { - return s->last + 1; + return s->last + 1; } - -inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx) +inline const btMprSupport_t *btMprSimplexPoint(const btMprSimplex_t *s, int idx) { - // here is no check on boundaries - return &s->ps[idx]; + // here is no check on boundaries + return &s->ps[idx]; } inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s) { - *d = *s; + *d = *s; } inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a) { - btMprSupportCopy(s->ps + pos, a); + btMprSupportCopy(s->ps + pos, a); } - inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2) { - btMprSupport_t supp; + btMprSupport_t supp; - btMprSupportCopy(&supp, &s->ps[pos1]); - btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]); - btMprSupportCopy(&s->ps[pos2], &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; + return BT_MPR_FABS(val) < FLT_EPSILON; } - - inline int btMprEq(float _a, float _b) { - float ab; - float a, b; + float ab; + float a, b; - ab = BT_MPR_FABS(_a - _b); - if (BT_MPR_FABS(ab) < FLT_EPSILON) - return 1; + 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; - } + 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) +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()); + 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) +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; + 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); + v->setValue(x, y, z); } inline void btMprVec3Add(btVector3 *v, const btVector3 *w) @@ -201,41 +181,38 @@ inline void btMprVec3Add(btVector3 *v, const btVector3 *w) inline void btMprVec3Copy(btVector3 *v, const btVector3 *w) { - *v = *w; + *v = *w; } inline void btMprVec3Scale(btVector3 *d, float k) { - *d *= k; + *d *= k; } inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b) { - float dot; + float dot; - dot = btDot(*a,*b); - return dot; + dot = btDot(*a, *b); + return dot; } - inline float btMprVec3Len2(const btVector3 *v) { - return btMprVec3Dot(v, v); + return btMprVec3Dot(v, v); } inline void btMprVec3Normalize(btVector3 *d) { - float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d)); - btMprVec3Scale(d, k); + 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); - + *d = btCross(*a, *b); } - inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) { *d = *v - *w; @@ -243,89 +220,97 @@ inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir) { - btVector3 v2v1, v3v1; + 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); + 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) + const btVector3 *dir) { - float dot; - dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v); - return btMprIsZero(dot) || dot > 0.f; + 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) + const btMprSupport_t *v4, + const btVector3 *dir) { - float dv1, dv2, dv3, dv4; - float dot1, dot2, dot3; + float dv1, dv2, dv3, dv4; + float dot1, dot2, dot3; - // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} + // 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); + 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 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; - dot1 = BT_MPR_FMIN(dot1, dot2); - dot1 = BT_MPR_FMIN(dot1, dot3); + dot1 = BT_MPR_FMIN(dot1, dot2); + dot1 = BT_MPR_FMIN(dot1, dot3); - return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE; + return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE; } inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, - const btMprSupport_t *v4, - const btVector3 *dir) + const btMprSupport_t *v4, + const btVector3 *dir) { - float dot; - dot = btMprVec3Dot(&v4->v, dir); - return btMprIsZero(dot) || dot > 0.f; + float dot; + dot = btMprVec3Dot(&v4->v, dir); + return btMprIsZero(dot) || dot > 0.f; } inline void btExpandPortal(btMprSimplex_t *portal, - const btMprSupport_t *v4) + const btMprSupport_t *v4) { - float dot; - btVector3 v4v0; + 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); - } - } + 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) +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 seperatingAxisInA = dir * a.getWorldTransform().getBasis(); + btVector3 seperatingAxisInB = -dir * b.getWorldTransform().getBasis(); btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA); btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB); @@ -335,574 +320,565 @@ inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b, supp->v = supp->v1 - supp->v2; } - template -static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal) +static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b, + const btMprCollisionDescription &colDesc, + btMprSimplex_t *portal) { - btVector3 dir, va, vb; - float dot; - int cont; - - + 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); - + // 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; + 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); - } + 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); - // 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); - 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); - // test if origin isn't outside of v1 - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir); - + if (btMprIsZero(dot) || dot < 0.f) + return -1; - 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)); - // 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; - } - } + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir); + if (btMprIsZero(dot) || dot < 0.f) + return -1; - 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); - 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); - // 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); + } - // 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)); - 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; + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir); + if (btMprIsZero(dot) || dot < 0.f) + return -1; - cont = 0; + 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; - } + // 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) + { + // 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); - } - } + 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; + return 0; } template -static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal) +static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, + btMprSimplex_t *portal) { - btVector3 dir; - btMprSupport_t v4; + btVector3 dir; + btMprSupport_t v4; - for (int i=0;iv, + &btMprSimplexPoint(portal, 2)->v); + b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); - // use barycentric coordinates of tetrahedron to find origin - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, - &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, 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, 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); + 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){ + 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); + 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(&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); + 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); + btMprVec3Copy(pos, &p1); + btMprVec3Add(pos, &p2); + btMprVec3Scale(pos, 0.5); #else - btMprVec3Copy(pos, &p2); -#endif//MPR_AVERAGE_CONTACT_POSITIONS + 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); + btVector3 ab; + btMprVec3Sub2(&ab, a, b); + return btMprVec3Len2(&ab); } inline float _btMprVec3PointSegmentDist2(const btVector3 *P, - const btVector3 *x0, - const btVector3 *b, - btVector3 *witness) + 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. + // 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; + float dist, t; + btVector3 d, a; - // direction of segment - btMprVec3Sub2(&d, b, x0); + // direction of segment + btMprVec3Sub2(&d, b, x0); - // precompute vector from P to x0 - btMprVec3Sub2(&a, x0, P); + // precompute vector from P to x0 + btMprVec3Sub2(&a, x0, P); - t = -1.f * btMprVec3Dot(&a, &d); - t /= btMprVec3Len2(&d); + 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); - } - } + 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; + return dist; } - - inline float btMprVec3PointTriDist2(const btVector3 *P, - const btVector3 *x0, const btVector3 *B, - const btVector3 *C, - btVector3 *witness) + 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. + // 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; + 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); + 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); + 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 = -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 ((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); - 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); - 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, 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); + } + } - dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2); - if (dist2 < dist){ - dist = dist2; - if (witness) - btMprVec3Copy(witness, &witness2); - } - } - - return dist; + return dist; } template -static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal, - float *depth, btVector3 *pdir, btVector3 *pos) +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 dir; + btMprSupport_t v4; + unsigned long iterations; - btVector3 zero = btVector3(0,0,0); - btVector3* origin = &zero; + 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) + // reached tolerance -> 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); - + *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); + // barycentric coordinates: + btFindPos(portal, pos); - return; - } + return; + } - btExpandPortal(portal, &v4); + btExpandPortal(portal, &v4); - iterations++; - } + iterations++; + } } -static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos) +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; - + // 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); + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); + btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Scale(pos, 0.5); #else - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); #endif } static void btFindPenetrSegment(btMprSimplex_t *portal, - float *depth, btVector3 *dir, btVector3 *pos) + float *depth, btVector3 *dir, btVector3 *pos) { - - // Origin lies on v0-v1 segment. - // Depth is distance to v1, direction also and position must be - // computed + // 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); + 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(pos, &btMprSimplexPoint(portal, 1)->v2); +#endif //MPR_AVERAGE_CONTACT_POSITIONS - btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v); - *depth = BT_MPR_SQRT(btMprVec3Len2(dir)); - btMprVec3Normalize(dir); - - + 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) +inline int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, + const btMprCollisionDescription &colDesc, + float *depthOut, btVector3 *dirOut, btVector3 *posOut) { - - btMprSimplex_t portal; + btMprSimplex_t portal; + // Phase 1: Portal discovery + int result = btDiscoverPortal(a, b, colDesc, &portal); - // Phase 1: Portal discovery - int result = btDiscoverPortal(a,b,colDesc, &portal); - - //sepAxis[pairIndex] = *pdir;//or -dir? switch (result) { - case 0: + case 0: { // Phase 2: Portal refinement - - result = btRefinePortal(a,b,colDesc, &portal); + + result = btRefinePortal(a, b, colDesc, &portal); if (result < 0) return -1; // Phase 3. Penetration info - btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut); - - + btFindPenetr(a, b, colDesc, &portal, depthOut, dirOut, posOut); + break; } - case 1: + case 1: { - // Touching contact on portal's v1. + // Touching contact on portal's v1. btFindPenetrTouch(&portal, depthOut, dirOut, posOut); - result=0; + result = 0; break; } - case 2: + case 2: { - - btFindPenetrSegment( &portal, depthOut, dirOut, posOut); - result=0; + btFindPenetrSegment(&portal, depthOut, dirOut, posOut); + result = 0; break; } - default: + default: { //if (res < 0) //{ - // Origin isn't inside portal - no collision. - result = -1; + // 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) +template +inline int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo) { - btVector3 dir,pos; + btVector3 dir, pos; float depth; - int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos); - if (res==0) + 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; + distInfo->m_pointOnA = pos - distInfo->m_distance * dir; return 0; } return -1; } - - -#endif //BT_MPR_PENETRATION_H +#endif //BT_MPR_PENETRATION_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 23aaece22..f1422cad4 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -13,86 +13,86 @@ subject to the following restrictions: 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; +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; - +bool gContactCalcArea3Points = true; btPersistentManifold::btPersistentManifold() -:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), -m_body0(0), -m_body1(0), -m_cachedPoints (0), -m_index1a(0) + : 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() +void btPersistentManifold::DebugPersistency() { int i; - printf("DebugPersistency : numPoints %d\n",m_cachedPoints); - for (i=0;i1) + if (occurance > 1) printf("error in clearUserCache\n"); } } - btAssert(occurance<=0); -#endif //DEBUG_PERSISTENCY + 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) +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]; + btVector3 a[3], b[3]; a[0] = p0 - p1; a[1] = p0 - p2; a[2] = p0 - p3; @@ -105,100 +105,102 @@ static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,c btVector3 tmp1 = a[1].cross(b[1]); btVector3 tmp2 = a[2].cross(b[2]); - return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2()); + return btMax(btMax(tmp0.length2(), tmp1.length2()), tmp2.length2()); } -int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) +int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) { - //calculate 4 possible cases areas, and take biggest area - //also need to keep 'deepest' - - int maxPenetrationIndex = -1; + //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++) + btScalar maxPenetration = pt.getDistance(); + for (int i = 0; i < 4; i++) + { + if (m_pointCache[i].getDistance() < maxPenetration) { - if (m_pointCache[i].getDistance() < maxPenetration) - { - maxPenetrationIndex = i; - maxPenetration = m_pointCache[i].getDistance(); - } + maxPenetrationIndex = i; + maxPenetration = m_pointCache[i].getDistance(); } -#endif //KEEP_DEEPEST_POINT - - btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.)); + } +#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 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 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 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 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 != 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 != 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 != 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); + 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); + btVector4 maxvec(res0, res1, res2, res3); int biggestarea = maxvec.closestAxis4(); return biggestarea; - } - int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const { - btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); + btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); int size = getNumContacts(); int nearestPoint = -1; - for( int i = 0; i < size; i++ ) + for (int i = 0; i < size; i++) { - const btManifoldPoint &mp = m_pointCache[i]; + const btManifoldPoint& mp = m_pointCache[i]; - btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA; + btVector3 diffA = mp.m_localPointA - newPoint.m_localPointA; const btScalar distToManiPoint = diffA.dot(diffA); - if( distToManiPoint < shortestDist ) + if (distToManiPoint < shortestDist) { shortestDist = distToManiPoint; nearestPoint = i; @@ -213,7 +215,7 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool { btAssert(validContactDistance(newPoint)); } - + int insertIndex = getNumContacts(); if (insertIndex == MANIFOLD_CACHE_SIZE) { @@ -224,85 +226,222 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool insertIndex = 0; #endif clearUserCache(m_pointCache[insertIndex]); - - } else + } + else { m_cachedPoints++; - - } - if (insertIndex<0) - insertIndex=0; + if (insertIndex < 0) + insertIndex = 0; - btAssert(m_pointCache[insertIndex].m_userPersistentData==0); + btAssert(m_pointCache[insertIndex].m_userPersistentData == 0); m_pointCache[insertIndex] = newPoint; return insertIndex; } -btScalar btPersistentManifold::getContactBreakingThreshold() const +btScalar btPersistentManifold::getContactBreakingThreshold() const { return m_contactBreakingThreshold; } - - -void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB) +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 + 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--) + 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); + 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 + /// then btScalar distance2d; - btVector3 projectedDifference,projectedPoint; - for (i=getNumContacts()-1;i>=0;i--) + btVector3 projectedDifference, projectedPoint; + for (i = getNumContacts() - 1; i >= 0; i--) { - - btManifoldPoint &manifoldPoint = m_pointCache[i]; + btManifoldPoint& manifoldPoint = m_pointCache[i]; //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) if (!validContactDistance(manifoldPoint)) { removeContactPoint(i); - } else + } + else { - //todo: friction anchor may require the contact to be around a bit longer + //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() ) + if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold()) { removeContactPoint(i); - } else + } + else { //contact point processed callback if (gContactProcessedCallback) - (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1); + (*gContactProcessedCallback)(manifoldPoint, (void*)m_body0, (void*)m_body1); } } } #ifdef DEBUG_PERSISTENCY DebugPersistency(); -#endif // +#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/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index f872c8e1c..8a9134c95 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BT_PERSISTENT_MANIFOLD_H #define BT_PERSISTENT_MANIFOLD_H - #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "btManifoldPoint.h" @@ -24,6 +23,8 @@ class btCollisionObject; #include "LinearMath/btAlignedAllocator.h" struct btCollisionResult; +struct btCollisionObjectDoubleData; +struct btCollisionObjectFloatData; ///maximum contact breaking and merging threshold extern btScalar gContactBreakingThreshold; @@ -32,14 +33,14 @@ extern btScalar gContactBreakingThreshold; 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; +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 +#endif //SWIG //the enum starts at 1024 to avoid type conflicts with btTypedConstraint enum btContactManifoldTypes @@ -58,70 +59,74 @@ enum btContactManifoldTypes ///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. - //ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject -ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject +ATTRIBUTE_ALIGNED16(class) +btPersistentManifold : public btTypedObject { - btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; /// this two body pointers can point to the physics rigidbody class. const btCollisionObject* m_body0; const btCollisionObject* m_body1; - int m_cachedPoints; + int m_cachedPoints; - btScalar m_contactBreakingThreshold; - btScalar m_contactProcessingThreshold; + btScalar m_contactBreakingThreshold; + btScalar m_contactProcessingThreshold; - /// sort cached points so most isolated points come first - int sortCachedPoints(const btManifoldPoint& pt); + int sortCachedPoints(const btManifoldPoint& pt); - int findContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt); + int findContactPoint(const btManifoldPoint* unUsed, int numUnused, const btManifoldPoint& pt); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - int m_companionIdA; - int m_companionIdB; + int m_companionIdA; + int m_companionIdB; int m_index1a; btPersistentManifold(); - btPersistentManifold(const btCollisionObject* body0,const btCollisionObject* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold) + btPersistentManifold(const btCollisionObject* body0, const btCollisionObject* body1, int, btScalar contactBreakingThreshold, btScalar contactProcessingThreshold) : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), - m_body0(body0),m_body1(body1),m_cachedPoints(0), - m_contactBreakingThreshold(contactBreakingThreshold), - m_contactProcessingThreshold(contactProcessingThreshold) + m_body0(body0), + m_body1(body1), + m_cachedPoints(0), + m_contactBreakingThreshold(contactBreakingThreshold), + m_contactProcessingThreshold(contactProcessingThreshold), + m_companionIdA(0), + m_companionIdB(0), + m_index1a(0) { } - SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0;} - SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1;} + SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0; } + SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1; } - void setBodies(const btCollisionObject* body0,const btCollisionObject* body1) + void setBodies(const btCollisionObject* body0, const btCollisionObject* body1) { m_body0 = body0; m_body1 = body1; } - void clearUserCache(btManifoldPoint& pt); + void clearUserCache(btManifoldPoint & pt); #ifdef DEBUG_PERSISTENCY - void DebugPersistency(); -#endif // - - SIMD_FORCE_INLINE int getNumContacts() const { return m_cachedPoints;} + void DebugPersistency(); +#endif // + + SIMD_FORCE_INLINE int getNumContacts() const + { + return m_cachedPoints; + } /// the setNumContacts API is usually not used, except when you gather/fill all contacts manually void setNumContacts(int cachedPoints) { m_cachedPoints = cachedPoints; } - SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const { btAssert(index < m_cachedPoints); @@ -135,39 +140,36 @@ public: } ///@todo: get this margin from the current physics / collision environment - btScalar getContactBreakingThreshold() const; + btScalar getContactBreakingThreshold() const; - btScalar getContactProcessingThreshold() const + btScalar getContactProcessingThreshold() const { return m_contactProcessingThreshold; } - + void setContactBreakingThreshold(btScalar contactBreakingThreshold) { m_contactBreakingThreshold = contactBreakingThreshold; } - void setContactProcessingThreshold(btScalar contactProcessingThreshold) + void setContactProcessingThreshold(btScalar contactProcessingThreshold) { m_contactProcessingThreshold = contactProcessingThreshold; } - - - int getCacheEntry(const btManifoldPoint& newPoint) const; - int addManifoldPoint( const btManifoldPoint& newPoint, bool isPredictive=false); + int addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive = false); - void removeContactPoint (int index) + void removeContactPoint(int index) { clearUserCache(m_pointCache[index]); int lastUsedIndex = getNumContacts() - 1; -// m_pointCache[index] = m_pointCache[lastUsedIndex]; - if(index != lastUsedIndex) + // m_pointCache[index] = m_pointCache[lastUsedIndex]; + if (index != lastUsedIndex) { - m_pointCache[index] = m_pointCache[lastUsedIndex]; + m_pointCache[index] = m_pointCache[lastUsedIndex]; //get rid of duplicated userPersistentData pointer m_pointCache[lastUsedIndex].m_userPersistentData = 0; m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f; @@ -177,7 +179,7 @@ public: m_pointCache[lastUsedIndex].m_lifeTime = 0; } - btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0); + btAssert(m_pointCache[lastUsedIndex].m_userPersistentData == 0); m_cachedPoints--; if (gContactEndedCallback && m_cachedPoints == 0) @@ -238,13 +240,12 @@ public: return pt.m_distance1 <= getContactBreakingThreshold(); } /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin - void refreshContactPoints( const btTransform& trA,const btTransform& trB); + void refreshContactPoints(const btTransform& trA, const btTransform& trB); - - SIMD_FORCE_INLINE void clearManifold() + SIMD_FORCE_INLINE void clearManifold() { int i; - for (i=0;i //for FLT_MAX +#include //for FLT_MAX -int gExpectedNbTests=0; +int gExpectedNbTests = 0; int gActualNbTests = 0; bool gUseInternalObject = true; // Clips a face to the back of a plane -void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS) +void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS) { - int ve; btScalar ds, de; int numVerts = pVtxIn.size(); if (numVerts < 2) return; - btVector3 firstVertex=pVtxIn[pVtxIn.size()-1]; + btVector3 firstVertex = pVtxIn[pVtxIn.size() - 1]; btVector3 endVertex = pVtxIn[0]; - - ds = planeNormalWS.dot(firstVertex)+planeEqWS; + + ds = planeNormalWS.dot(firstVertex) + planeEqWS; for (ve = 0; ve < numVerts; ve++) { - endVertex=pVtxIn[ve]; + endVertex = pVtxIn[ve]; - de = planeNormalWS.dot(endVertex)+planeEqWS; + de = planeNormalWS.dot(endVertex) + planeEqWS; - if (ds<0) + if (ds < 0) { - if (de<0) + if (de < 0) { // Start < 0, end < 0, so output endVertex ppVtxOut.push_back(endVertex); @@ -59,15 +56,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex else { // Start < 0, end >= 0, so output intersection - ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de)))); } } else { - if (de<0) + if (de < 0) { // Start >= 0, end < 0 so output intersection and end - ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de)))); ppVtxOut.push_back(endVertex); } } @@ -76,47 +73,44 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex } } - -static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB) +static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB) { - btScalar Min0,Max0; - btScalar Min1,Max1; - btVector3 witnesPtMinA,witnesPtMaxA; - btVector3 witnesPtMinB,witnesPtMaxB; + btScalar Min0, Max0; + btScalar Min1, Max1; + btVector3 witnesPtMinA, witnesPtMaxA; + btVector3 witnesPtMinB, witnesPtMaxB; - hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA); - hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB); + hullA.project(transA, sep_axis, Min0, Max0, witnesPtMinA, witnesPtMaxA); + hullB.project(transB, sep_axis, Min1, Max1, witnesPtMinB, witnesPtMaxB); - if(Max0=0.0f); + btAssert(d0 >= 0.0f); btScalar d1 = Max1 - Min0; - btAssert(d1>=0.0f); - if (d0= 0.0f); + if (d0 < d1) { depth = d0; witnessPointA = witnesPtMaxA; witnessPointB = witnesPtMinB; - - } else + } + else { depth = d1; witnessPointA = witnesPtMinA; witnessPointB = witnesPtMaxB; } - + return true; } - - -static int gActualSATPairTests=0; +static int gActualSATPairTests = 0; inline bool IsAlmostZero(const btVector3& v) { - if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6) return false; + if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false; return true; } @@ -125,9 +119,9 @@ inline bool IsAlmostZero(const btVector3& v) inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3]) { // This version is ~11.000 cycles (4%) faster overall in one of the tests. -// IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK); -// IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK); -// IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK); + // IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK); + // IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK); + // IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK); p[0] = sv[0] < 0.0f ? -extents[0] : extents[0]; p[1] = sv[1] < 0.0f ? -extents[1] : extents[1]; p[2] = sv[2] < 0.0f ? -extents[2] : extents[2]; @@ -140,90 +134,94 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans const btVector3& r1 = rot[1]; const btVector3& r2 = rot[2]; - const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z(); - const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z(); - const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z(); + const btScalar x = r0.x() * in.x() + r1.x() * in.y() + r2.x() * in.z(); + const btScalar y = r0.y() * in.x() + r1.y() * in.y() + r2.y() * in.z(); + const btScalar z = r0.z() * in.x() + r1.z() * in.y() + r2.z() * in.z(); out.setValue(x, y, z); } - bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin) +bool TestInternalObjects(const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin) { const btScalar dp = delta_c.dot(axis); btVector3 localAxis0; - InverseTransformPoint3x3(localAxis0, axis,trans0); + InverseTransformPoint3x3(localAxis0, axis, trans0); btVector3 localAxis1; - InverseTransformPoint3x3(localAxis1, axis,trans1); + InverseTransformPoint3x3(localAxis1, axis, trans1); btScalar p0[3]; BoxSupport(convex0.m_extents, localAxis0, p0); btScalar p1[3]; BoxSupport(convex1.m_extents, localAxis1, p1); - const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z(); - const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z(); + const btScalar Radius0 = p0[0] * localAxis0.x() + p0[1] * localAxis0.y() + p0[2] * localAxis0.z(); + const btScalar Radius1 = p1[0] * localAxis1.x() + p1[1] * localAxis1.y() + p1[2] * localAxis1.z(); - const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius; - const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius; + const btScalar MinRadius = Radius0 > convex0.m_radius ? Radius0 : convex0.m_radius; + const btScalar MaxRadius = Radius1 > convex1.m_radius ? Radius1 : convex1.m_radius; const btScalar MinMaxRadius = MaxRadius + MinRadius; const btScalar d0 = MinMaxRadius + dp; const btScalar d1 = MinMaxRadius - dp; - const btScalar depth = d0dmin) + const btScalar depth = d0 < d1 ? d0 : d1; + if (depth > dmin) return false; return true; } -#endif //TEST_INTERNAL_OBJECTS +#endif //TEST_INTERNAL_OBJECTS - - - SIMD_FORCE_INLINE void btSegmentsClosestPoints( +SIMD_FORCE_INLINE void btSegmentsClosestPoints( btVector3& ptsVector, btVector3& offsetA, btVector3& offsetB, btScalar& tA, btScalar& tB, const btVector3& translation, const btVector3& dirA, btScalar hlenA, - const btVector3& dirB, btScalar hlenB ) + const btVector3& dirB, btScalar hlenB) { // compute the parameters of the closest points on each line segment - btScalar dirA_dot_dirB = btDot(dirA,dirB); - btScalar dirA_dot_trans = btDot(dirA,translation); - btScalar dirB_dot_trans = btDot(dirB,translation); + btScalar dirA_dot_dirB = btDot(dirA, dirB); + btScalar dirA_dot_trans = btDot(dirA, translation); + btScalar dirB_dot_trans = btDot(dirB, translation); btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - if ( denom == 0.0f ) { + if (denom == 0.0f) + { tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) + } + else + { + tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom; + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } tB = tA * dirA_dot_dirB - dirB_dot_trans; - if ( tB < -hlenB ) { + if (tB < -hlenB) + { tB = -hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; - } else if ( tB > hlenB ) { + } + else if (tB > hlenB) + { tB = hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } @@ -235,44 +233,42 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans ptsVector = translation - offsetA + offsetB; } - - -bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut) +bool btPolyhedralContactClipping::findSeparatingAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut) { gActualSATPairTests++; -//#ifdef TEST_INTERNAL_OBJECTS + //#ifdef TEST_INTERNAL_OBJECTS const btVector3 c0 = transA * hullA.m_localCenter; const btVector3 c1 = transB * hullB.m_localCenter; const btVector3 DeltaC2 = c0 - c1; -//#endif + //#endif btScalar dmin = FLT_MAX; - int curPlaneTests=0; + int curPlaneTests = 0; int numFacesA = hullA.m_faces.size(); // Test normals from hullA - for(int i=0;i=0&&edgeB>=0) + if (edgeA >= 0 && edgeB >= 0) { -// printf("edge-edge\n"); + // printf("edge-edge\n"); //add an edge-edge contact btVector3 ptsVector; @@ -375,57 +368,55 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& btScalar tA; btScalar tB; - btVector3 translation = witnessPointB-witnessPointA; + btVector3 translation = witnessPointB - witnessPointA; btVector3 dirA = worldEdgeA; btVector3 dirB = worldEdgeB; - + btScalar hlenB = 1e30f; btScalar hlenA = 1e30f; - btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB, - translation, - dirA, hlenA, - dirB,hlenB); + btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, + translation, + dirA, hlenA, + dirB, hlenB); btScalar nlSqrt = ptsVector.length2(); - if (nlSqrt>SIMD_EPSILON) + if (nlSqrt > SIMD_EPSILON) { btScalar nl = btSqrt(nlSqrt); - ptsVector *= 1.f/nl; - if (ptsVector.dot(DeltaC2)<0.f) + ptsVector *= 1.f / nl; + if (ptsVector.dot(DeltaC2) < 0.f) { - ptsVector*=-1.f; + ptsVector *= -1.f; } btVector3 ptOnB = witnessPointB + offsetB; btScalar distance = nl; - resultOut.addContactPoint(ptsVector, ptOnB,-distance); + resultOut.addContactPoint(ptsVector, ptOnB, -distance); } - } - - if((DeltaC2.dot(sep))<0.0f) + if ((DeltaC2.dot(sep)) < 0.0f) sep = -sep; return true; } -void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, 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) { worldVertsB2.resize(0); btVertexArray* pVtxIn = &worldVertsB1; btVertexArray* pVtxOut = &worldVertsB2; pVtxOut->reserve(pVtxIn->size()); - int closestFaceA=-1; + int closestFaceA = -1; { btScalar dmin = FLT_MAX; - for(int face=0;faceresize(0); } - - -//#define ONLY_REPORT_DEEPEST_POINT + //#define ONLY_REPORT_DEEPEST_POINT btVector3 point; - // only keep points that are behind the witness face { - btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]); + btVector3 localPlaneNormal(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); btScalar localPlaneEq = polyA.m_plane[3]; - btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; - btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); - for (int i=0;isize();i++) + btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal; + btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin()); + for (int i = 0; i < pVtxIn->size(); i++) { btVector3 vtx = pVtxIn->at(i); - btScalar depth = planeNormalWS.dot(vtx)+planeEqWS; - if (depth <=minDist) + btScalar depth = planeNormalWS.dot(vtx) + planeEqWS; + if (depth <= minDist) { -// printf("clamped: depth=%f to minDist=%f\n",depth,minDist); + // printf("clamped: depth=%f to minDist=%f\n",depth,minDist); depth = minDist; } - if (depth <=maxDist) + if (depth <= maxDist) { btVector3 point = pVtxIn->at(i); #ifdef ONLY_REPORT_DEEPEST_POINT @@ -507,40 +495,32 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin { printf("error in btPolyhedralContactClipping depth = %f\n", depth); printf("likely wrong separatingNormal passed in\n"); - } -#endif - resultOut.addContactPoint(separatingNormal,point,depth); + } +#endif + resultOut.addContactPoint(separatingNormal, point, depth); #endif } } } #ifdef ONLY_REPORT_DEEPEST_POINT - if (curMaxDist=0) - clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, worldVertsB2,minDist, maxDist,resultOut); + { + const btFace& polyB = hullB.m_faces[closestFaceB]; + const int numVertices = polyB.m_indices.size(); + for (int e0 = 0; e0 < numVertices; e0++) + { + const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]]; + worldVertsB1.push_back(transB * b); + } + } + if (closestFaceB >= 0) + clipFaceAgainstHull(separatingNormal, hullA, transA, worldVertsB1, worldVertsB2, minDist, maxDist, resultOut); } diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h index 30e3db687..328f6424b 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h @@ -13,14 +13,11 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans - #ifndef BT_POLYHEDRAL_CONTACT_CLIPPING_H #define BT_POLYHEDRAL_CONTACT_CLIPPING_H - #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btTransform.h" #include "btDiscreteCollisionDetectorInterface.h" @@ -32,18 +29,14 @@ typedef btAlignedObjectArray btVertexArray; // Clips a face to the back of a plane struct btPolyhedralContactClipping { + 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 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 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); + static bool findSeparatingAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut); ///the clipFace method is used internally - static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS); - + static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS); }; -#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H - +#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index 786efd182..3d11e5bce 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -23,39 +23,38 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "btRaycastCallback.h" -btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags) - : - m_from(from), - m_to(to), - //@BP Mod - m_flags(flags), - m_hitFraction(btScalar(1.)) +btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from, const btVector3& to, unsigned int flags) + : m_from(from), + m_to(to), + //@BP Mod + m_flags(flags), + m_hitFraction(btScalar(1.)) { - } - - -void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) +void btTriangleRaycastCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { - const btVector3 &vert0=triangle[0]; - const btVector3 &vert1=triangle[1]; - const btVector3 &vert2=triangle[2]; + const btVector3& vert0 = triangle[0]; + const btVector3& vert1 = triangle[1]; + const btVector3& vert2 = triangle[2]; - btVector3 v10; v10 = vert1 - vert0 ; - btVector3 v20; v20 = vert2 - vert0 ; + btVector3 v10; + v10 = vert1 - vert0; + btVector3 v20; + v20 = vert2 - vert0; + + btVector3 triangleNormal; + triangleNormal = v10.cross(v20); - btVector3 triangleNormal; triangleNormal = v10.cross( v20 ); - const btScalar dist = vert0.dot(triangleNormal); - btScalar dist_a = triangleNormal.dot(m_from) ; - dist_a-= dist; + btScalar dist_a = triangleNormal.dot(m_from); + dist_a -= dist; btScalar dist_b = triangleNormal.dot(m_to); dist_b -= dist; - if ( dist_a * dist_b >= btScalar(0.0) ) + if (dist_a * dist_b >= btScalar(0.0)) { - return ; // same sign + return; // same sign } if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a <= btScalar(0.0))) @@ -64,52 +63,52 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, return; } - - const btScalar proj_length=dist_a-dist_b; - const btScalar distance = (dist_a)/(proj_length); + const btScalar proj_length = dist_a - dist_b; + const btScalar distance = (dist_a) / (proj_length); // Now we have the intersection point on the plane, we'll see if it's inside the triangle // Add an epsilon as a tolerance for the raycast, // in case the ray hits exacly on the edge of the triangle. // It must be scaled for the triangle size. - - if(distance < m_hitFraction) + + if (distance < m_hitFraction) { - - - btScalar edge_tolerance =triangleNormal.length2(); + btScalar edge_tolerance = triangleNormal.length2(); edge_tolerance *= btScalar(-0.0001); - btVector3 point; point.setInterpolate3( m_from, m_to, distance); + btVector3 point; + point.setInterpolate3(m_from, m_to, distance); { - btVector3 v0p; v0p = vert0 - point; - btVector3 v1p; v1p = vert1 - point; - btVector3 cp0; cp0 = v0p.cross( v1p ); + btVector3 v0p; + v0p = vert0 - point; + btVector3 v1p; + v1p = vert1 - point; + btVector3 cp0; + cp0 = v0p.cross(v1p); - if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) + if ((btScalar)(cp0.dot(triangleNormal)) >= edge_tolerance) { - - - btVector3 v2p; v2p = vert2 - point; + btVector3 v2p; + v2p = vert2 - point; btVector3 cp1; - cp1 = v1p.cross( v2p); - if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) + cp1 = v1p.cross(v2p); + if ((btScalar)(cp1.dot(triangleNormal)) >= edge_tolerance) { btVector3 cp2; cp2 = v2p.cross(v0p); - - if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) - { - //@BP Mod - // Triangle normal isn't normalized - triangleNormal.normalize(); - //@BP Mod - Allow for unflipped normal when raycasting against backfaces + if ((btScalar)(cp2.dot(triangleNormal)) >= edge_tolerance) + { + //@BP Mod + // Triangle normal isn't normalized + triangleNormal.normalize(); + + //@BP Mod - Allow for unflipped normal when raycasting against backfaces if (((m_flags & kF_KeepUnflippedNormal) == 0) && (dist_a <= btScalar(0.0))) { - m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex); + m_hitFraction = reportHit(-triangleNormal, distance, partId, triangleIndex); } else { - m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex); + m_hitFraction = reportHit(triangleNormal, distance, partId, triangleIndex); } } } @@ -118,8 +117,7 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, } } - -btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin) +btTriangleConvexcastCallback::btTriangleConvexcastCallback(const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin) { m_convexShape = convexShape; m_convexShapeFrom = convexShapeFrom; @@ -130,14 +128,13 @@ btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* m_allowedPenetration = 0.f; } -void -btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex) +void btTriangleConvexcastCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { - btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]); - triangleShape.setMargin(m_triangleCollisionMargin); + btTriangleShape triangleShape(triangle[0], triangle[1], triangle[2]); + triangleShape.setMargin(m_triangleCollisionMargin); - btVoronoiSimplexSolver simplexSolver; - btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; + btVoronoiSimplexSolver simplexSolver; + btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; //#define USE_SUBSIMPLEX_CONVEX_CAST 1 //if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below @@ -145,21 +142,21 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver); #else //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver); - btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver); -#endif //#USE_SUBSIMPLEX_CONVEX_CAST - + btContinuousConvexCollision convexCaster(m_convexShape, &triangleShape, &simplexSolver, &gjkEpaPenetrationSolver); +#endif //#USE_SUBSIMPLEX_CONVEX_CAST + btConvexCast::CastResult castResult; castResult.m_fraction = btScalar(1.); castResult.m_allowedPenetration = m_allowedPenetration; - if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult)) + if (convexCaster.calcTimeOfImpact(m_convexShapeFrom, m_convexShapeTo, m_triangleToWorld, m_triangleToWorld, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) - { + { if (castResult.m_fraction < m_hitFraction) { -/* btContinuousConvexCast's normal is already in world space */ -/* + /* btContinuousConvexCast's normal is already in world space */ + /* #ifdef USE_SUBSIMPLEX_CONVEX_CAST //rotate normal into worldspace castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal; @@ -167,11 +164,11 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, */ castResult.m_normal.normalize(); - reportHit (castResult.m_normal, - castResult.m_hitPoint, - castResult.m_fraction, - partId, - triangleIndex); + reportHit(castResult.m_normal, + castResult.m_hitPoint, + castResult.m_fraction, + partId, + triangleIndex); } } } diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index f2ed0cd39..2d0df718a 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -21,35 +21,34 @@ subject to the following restrictions: struct btBroadphaseProxy; class btConvexShape; -class btTriangleRaycastCallback: public btTriangleCallback +class btTriangleRaycastCallback : public btTriangleCallback { public: - //input btVector3 m_from; btVector3 m_to; - //@BP Mod - allow backface filtering and unflipped normals - enum EFlags - { - kF_None = 0, - kF_FilterBackfaces = 1 << 0, - kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle - ///SubSimplexConvexCastRaytest is the default, even if kF_None is set. - kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm - kF_UseGjkConvexCastRaytest = 1 << 3, - kF_Terminator = 0xFFFFFFFF - }; - unsigned int m_flags; + //@BP Mod - allow backface filtering and unflipped normals + enum EFlags + { + kF_None = 0, + kF_FilterBackfaces = 1 << 0, + kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle + ///SubSimplexConvexCastRaytest is the default, even if kF_None is set. + kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm + kF_UseGjkConvexCastRaytest = 1 << 3, + kF_DisableHeightfieldAccelerator = 1 << 4, //don't use the heightfield raycast accelerator. See https://github.com/bulletphysics/bullet3/pull/2062 + kF_Terminator = 0xFFFFFFFF + }; + unsigned int m_flags; - btScalar m_hitFraction; + btScalar m_hitFraction; + + btTriangleRaycastCallback(const btVector3& from, const btVector3& to, unsigned int flags = 0); - btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0); - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0; - + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; }; class btTriangleConvexcastCallback : public btTriangleCallback @@ -63,12 +62,11 @@ public: btScalar m_triangleCollisionMargin; btScalar m_allowedPenetration; - btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin); + btTriangleConvexcastCallback(const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin); - virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex); + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; }; -#endif //BT_RAYCAST_TRI_CALLBACK_H - +#endif //BT_RAYCAST_TRI_CALLBACK_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h index da8a13914..ccd227109 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_SIMPLEX_SOLVER_INTERFACE_H #define BT_SIMPLEX_SOLVER_INTERFACE_H @@ -31,33 +29,30 @@ subject to the following restrictions: /// voronoi regions or barycentric coordinates class btSimplexSolverInterface { - public: - virtual ~btSimplexSolverInterface() {}; +public: + virtual ~btSimplexSolverInterface(){}; virtual void reset() = 0; virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0; - + virtual bool closest(btVector3& v) = 0; virtual btScalar maxVertex() = 0; virtual bool fullSimplex() const = 0; - virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0; + virtual int getSimplex(btVector3* pBuf, btVector3* qBuf, btVector3* yBuf) const = 0; virtual bool inSimplex(const btVector3& w) = 0; - + virtual void backup_closest(btVector3& v) = 0; virtual bool emptySimplex() const = 0; virtual void compute_points(btVector3& p1, btVector3& p2) = 0; - virtual int numVertices() const =0; - - + virtual int numVertices() const = 0; }; #endif -#endif //BT_SIMPLEX_SOLVER_INTERFACE_H - +#endif //BT_SIMPLEX_SOLVER_INTERFACE_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index ec638f60b..37458339e 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSubSimplexConvexCast.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" @@ -22,32 +21,26 @@ subject to the following restrictions: #include "btPointCollector.h" #include "LinearMath/btTransformUtil.h" -btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) -:m_simplexSolver(simplexSolver), -m_convexA(convexA),m_convexB(convexB) +btSubsimplexConvexCast::btSubsimplexConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver) + : m_simplexSolver(simplexSolver), + m_convexA(convexA), + m_convexB(convexB) { } -///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. -///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 -#ifdef BT_USE_DOUBLE_PRECISION -#define MAX_ITERATIONS 64 -#else -#define MAX_ITERATIONS 32 -#endif -bool btSubsimplexConvexCast::calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) -{ +bool btSubsimplexConvexCast::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) +{ m_simplexSolver->reset(); - btVector3 linVelA,linVelB; - linVelA = toA.getOrigin()-fromA.getOrigin(); - linVelB = toB.getOrigin()-fromB.getOrigin(); + btVector3 linVelA, linVelB; + linVelA = toA.getOrigin() - fromA.getOrigin(); + linVelB = toB.getOrigin() - fromB.getOrigin(); btScalar lambda = btScalar(0.); @@ -55,36 +48,31 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( btTransform interpolatedTransB = fromB; ///take relative motion - btVector3 r = (linVelA-linVelB); + btVector3 r = (linVelA - linVelB); btVector3 v; - - btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis())); - btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis())); - v = supVertexA-supVertexB; - int maxIter = MAX_ITERATIONS; + + btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r * fromA.getBasis())); + btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r * fromB.getBasis())); + v = supVertexA - supVertexB; + int maxIter = result.m_subSimplexCastMaxIterations; btVector3 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - + n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 c; - - - btScalar dist2 = v.length2(); -#ifdef BT_USE_DOUBLE_PRECISION - btScalar epsilon = btScalar(0.0001); -#else - btScalar epsilon = btScalar(0.0001); -#endif //BT_USE_DOUBLE_PRECISION - btVector3 w,p; + + + + btVector3 w, p; btScalar VdotR; - - while ( (dist2 > epsilon) && maxIter--) + + while ((dist2 > result.m_subSimplexCastEpsilon) && maxIter--) { - supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis())); - supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis())); - w = supVertexA-supVertexB; + supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v * interpolatedTransA.getBasis())); + supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v * interpolatedTransB.getBasis())); + w = supVertexA - supVertexB; btScalar VdotW = v.dot(w); @@ -93,68 +81,63 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( return false; } - if ( VdotW > btScalar(0.)) + if (VdotW > btScalar(0.)) { VdotR = v.dot(r); - if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON)) + if (VdotR >= -(SIMD_EPSILON * SIMD_EPSILON)) return false; else { lambda = lambda - VdotW / VdotR; //interpolate to next lambda // x = s + lambda * r; - interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); - interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); + interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(), toA.getOrigin(), lambda); + interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda); //m_simplexSolver->reset(); //check next line - w = supVertexA-supVertexB; - + w = supVertexA - supVertexB; + n = v; - } - } + } ///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc. if (!m_simplexSolver->inSimplex(w)) - m_simplexSolver->addVertex( w, supVertexA , supVertexB); + m_simplexSolver->addVertex(w, supVertexA, supVertexB); if (m_simplexSolver->closest(v)) { dist2 = v.length2(); - + //todo: check this normal for validity //n=v; //printf("V=%f , %f, %f\n",v[0],v[1],v[2]); //printf("DIST2=%f\n",dist2); //printf("numverts = %i\n",m_simplexSolver->numVertices()); - } else + } + else { dist2 = btScalar(0.); - } + } } //int numiter = MAX_ITERATIONS - maxIter; -// printf("number of iterations: %d", numiter); - + // printf("number of iterations: %d", numiter); + //don't report a time of impact when moving 'away' from the hitnormal - result.m_fraction = lambda; - if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON)) + if (n.length2() >= (SIMD_EPSILON * SIMD_EPSILON)) result.m_normal = n.normalized(); else result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0)); //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (result.m_normal.dot(r)>=-result.m_allowedPenetration) + if (result.m_normal.dot(r) >= -result.m_allowedPenetration) return false; - btVector3 hitA,hitB; - m_simplexSolver->compute_points(hitA,hitB); - result.m_hitPoint=hitB; + btVector3 hitA, hitB; + m_simplexSolver->compute_points(hitA, hitB); + result.m_hitPoint = hitB; return true; } - - - - diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h index 6c8127983..0638a30eb 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_SUBSIMPLEX_CONVEX_CAST_H #define BT_SUBSIMPLEX_CONVEX_CAST_H @@ -28,23 +27,21 @@ class btConvexShape; class btSubsimplexConvexCast : public btConvexCast { btSimplexSolverInterface* m_simplexSolver; - const btConvexShape* m_convexA; - const btConvexShape* m_convexB; + const btConvexShape* m_convexA; + const btConvexShape* m_convexB; public: - - btSubsimplexConvexCast (const btConvexShape* shapeA,const btConvexShape* shapeB,btSimplexSolverInterface* simplexSolver); + btSubsimplexConvexCast(const btConvexShape* shapeA, const btConvexShape* shapeB, btSimplexSolverInterface* simplexSolver); //virtual ~btSubsimplexConvexCast(); ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector. - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result); - + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); }; -#endif //BT_SUBSIMPLEX_CONVEX_CAST_H +#endif //BT_SUBSIMPLEX_CONVEX_CAST_H diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index 23b4f79cf..8fda94d2a 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -23,26 +23,24 @@ subject to the following restrictions: */ - #include "btVoronoiSimplexSolver.h" -#define VERTA 0 -#define VERTB 1 -#define VERTC 2 -#define VERTD 3 +#define VERTA 0 +#define VERTB 1 +#define VERTC 2 +#define VERTD 3 #define CATCH_DEGENERATE_TETRAHEDRON 1 -void btVoronoiSimplexSolver::removeVertex(int index) +void btVoronoiSimplexSolver::removeVertex(int index) { - - btAssert(m_numVertices>0); + btAssert(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 btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts) +void btVoronoiSimplexSolver::reduceVertices(const btUsageBitfield& usedVerts) { if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) removeVertex(3); @@ -52,29 +50,22 @@ void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts) if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) removeVertex(1); - + if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) removeVertex(0); - } - - - - //clear the simplex, remove all the vertices void btVoronoiSimplexSolver::reset() { m_cachedValidClosest = false; m_numVertices = 0; m_needsUpdate = true; - m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + m_lastW = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); m_cachedBC.reset(); } - - - //add a vertex +//add a vertex void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q) { m_lastW = w; @@ -87,9 +78,8 @@ void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, c m_numVertices++; } -bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() +bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() { - if (m_needsUpdate) { m_cachedBC.reset(); @@ -98,127 +88,131 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() switch (numVertices()) { - case 0: + case 0: m_cachedValidClosest = false; break; - case 1: + case 1: { m_cachedP1 = m_simplexPointsP[0]; m_cachedP2 = m_simplexPointsQ[0]; - m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] + m_cachedV = m_cachedP1 - m_cachedP2; //== m_simplexVectorW[0] m_cachedBC.reset(); - m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); + m_cachedBC.setBarycentricCoordinates(btScalar(1.), btScalar(0.), btScalar(0.), btScalar(0.)); m_cachedValidClosest = m_cachedBC.isValid(); break; }; - case 2: + case 2: { - //closest point origin from line segment - const btVector3& from = m_simplexVectorW[0]; - const btVector3& to = m_simplexVectorW[1]; - btVector3 nearest; + //closest point origin from line segment + const btVector3& from = m_simplexVectorW[0]; + const btVector3& to = m_simplexVectorW[1]; + btVector3 nearest; - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 diff = p - from; - btVector3 v = to - from; - btScalar t = v.dot(diff); - - if (t > 0) { - btScalar dotVV = v.dot(v); - if (t < dotVV) { - t /= dotVV; - diff -= t*v; - 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 + btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 diff = p - from; + btVector3 v = to - from; + btScalar t = v.dot(diff); + + if (t > 0) + { + btScalar dotVV = v.dot(v); + if (t < dotVV) { - t = 0; - //reduce to 1 point + t /= dotVV; + diff -= t * v; m_cachedBC.m_usedVertices.usedVertexA = true; + m_cachedBC.m_usedVertices.usedVertexB = true; } - m_cachedBC.setBarycentricCoordinates(1-t,t); - nearest = from + t*v; + 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_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; - m_cachedValidClosest = m_cachedBC.isValid(); - break; + reduceVertices(m_cachedBC.m_usedVertices); + + m_cachedValidClosest = m_cachedBC.isValid(); + break; } - case 3: - { - //closest point origin from triangle - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - - const btVector3& a = m_simplexVectorW[0]; - const btVector3& b = m_simplexVectorW[1]; - const btVector3& 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: + case 3: { + //closest point origin from triangle + btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.)); + + const btVector3& a = m_simplexVectorW[0]; + const btVector3& b = m_simplexVectorW[1]; + const btVector3& 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: + { + btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.)); - - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - const btVector3& a = m_simplexVectorW[0]; const btVector3& b = m_simplexVectorW[1]; 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] + - 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_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_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 + m_cachedV = m_cachedP1 - m_cachedP2; + reduceVertices(m_cachedBC.m_usedVertices); + } + else { -// printf("sub distance got penetration\n"); + // printf("sub distance got penetration\n"); if (m_cachedBC.m_degenerate) { m_cachedValidClosest = false; - } else + } + else { m_cachedValidClosest = true; //degenerate case == false, penetration = true + zero - m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_cachedV.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } break; } @@ -228,7 +222,7 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() //closest point origin from tetrahedron break; } - default: + default: { m_cachedValidClosest = false; } @@ -236,7 +230,6 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() } return m_cachedValidClosest; - } //return/calculate the closest vertex @@ -247,13 +240,11 @@ bool btVoronoiSimplexSolver::closest(btVector3& v) return succes; } - - btScalar btVoronoiSimplexSolver::maxVertex() { int i, numverts = numVertices(); btScalar maxV = btScalar(0.); - for (i=0;i= btScalar(0.0) && d4 <= d3) + // Check if P in vertex region outside B + btVector3 bp = p - b; + btScalar d3 = ab.dot(bp); + btScalar d4 = ac.dot(bp); + if (d3 >= btScalar(0.0) && d4 <= d3) { result.m_closestPointOnSimplex = b; result.m_usedVertices.usedVertexB = true; - result.setBarycentricCoordinates(0,1,0); + result.setBarycentricCoordinates(0, 1, 0); - return true; // b; // barycentric coordinates (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 - btScalar vc = d1*d4 - d3*d2; - if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { - btScalar v = d1 / (d1 - d3); + // Check if P in edge region of AB, if so return projection of P onto AB + btScalar vc = d1 * d4 - d3 * d2; + if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) + { + btScalar v = d1 / (d1 - d3); result.m_closestPointOnSimplex = a + v * ab; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; - result.setBarycentricCoordinates(1-v,v,0); + result.setBarycentricCoordinates(1 - v, v, 0); return true; - //return a + v * ab; // barycentric coordinates (1-v,v,0) - } + //return a + v * ab; // barycentric coordinates (1-v,v,0) + } - // Check if P in vertex region outside C - btVector3 cp = p - c; - btScalar d5 = ab.dot(cp); - btScalar d6 = ac.dot(cp); - if (d6 >= btScalar(0.0) && d5 <= d6) + // Check if P in vertex region outside C + btVector3 cp = p - c; + btScalar d5 = ab.dot(cp); + btScalar d6 = ac.dot(cp); + if (d6 >= btScalar(0.0) && d5 <= d6) { result.m_closestPointOnSimplex = c; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(0,0,1); - return true;//c; // barycentric coordinates (0,0,1) + 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 - btScalar vb = d5*d2 - d1*d6; - if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { - btScalar w = d2 / (d2 - d6); + // Check if P in edge region of AC, if so return projection of P onto AC + btScalar vb = d5 * d2 - d1 * d6; + if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) + { + btScalar w = d2 / (d2 - d6); result.m_closestPointOnSimplex = a + w * ac; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(1-w,0,w); + result.setBarycentricCoordinates(1 - w, 0, w); return true; - //return a + w * ac; // barycentric coordinates (1-w,0,w) - } + //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 + btScalar va = d3 * d6 - d5 * d4; + if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) + { + btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - // Check if P in edge region of BC, if so return projection of P onto BC - btScalar va = d3*d6 - d5*d4; - if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { - btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - result.m_closestPointOnSimplex = b + w * (c - b); result.m_usedVertices.usedVertexB = true; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(0,1-w,w); - return true; - // return b + w * (c - b); // barycentric coordinates (0,1-w,w) - } + 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) + btScalar denom = btScalar(1.0) / (va + vb + vc); + btScalar v = vb * denom; + btScalar w = vc * denom; - // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - btScalar denom = btScalar(1.0) / (va + vb + vc); - btScalar v = vb * denom; - btScalar w = vc * denom; - result.m_closestPointOnSimplex = a + ab * v + ac * w; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(1-v-w,v,w); - + result.setBarycentricCoordinates(1 - v - w, v, w); + return true; -// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w - + // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w } - - - - /// Test if point p and d lie on opposite sides of plane through abc int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d) { - btVector3 normal = (b-a).cross(c-a); + btVector3 normal = (b - a).cross(c - a); - btScalar signp = (p - a).dot(normal); // [AP AB AC] - btScalar signd = (d - a).dot( normal); // [AD AB AC] + btScalar signp = (p - a).dot(normal); // [AP AB AC] + btScalar signd = (d - a).dot(normal); // [AD AB AC] #ifdef CATCH_DEGENERATE_TETRAHEDRON #ifdef BT_USE_DOUBLE_PRECISION -if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) + if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) { return -1; } #else if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) { -// printf("affine dependent/degenerate\n");// + // printf("affine dependent/degenerate\n");// return -1; } #endif #endif // Points on opposite sides if expression signs are opposite - return signp * signd < btScalar(0.); + return signp * signd < btScalar(0.); } - -bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult) +bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult) { btSubSimplexClosestResult tempResult; - // Start out assuming point inside all halfspaces, so closest to itself + // 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.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 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); + 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; - } - - - btScalar bestSqDist = FLT_MAX; - // If point outside face abc then compute closest point on abc - if (pointOutsideABC) + if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) { - closestPtPointTriangle(p, a, b, c,tempResult); + finalResult.m_degenerate = true; + return false; + } + + if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) + { + return false; + } + + btScalar bestSqDist = FLT_MAX; + // If point outside face abc then compute closest point on abc + if (pointOutsideABC) + { + closestPtPointTriangle(p, a, b, c, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; - - btScalar sqDist = (q - p).dot( q - p); - // Update best closest point if (squared) distance is less than current best - if (sqDist < bestSqDist) { + + btScalar 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! @@ -504,25 +481,22 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const 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 - ); - + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC], + 0); } - } - + } // Repeat test for face acd - if (pointOutsideACD) + if (pointOutsideACD) { - closestPtPointTriangle(p, a, c, d,tempResult); + closestPtPointTriangle(p, a, c, d, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) + btScalar sqDist = (q - p).dot(q - p); + if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; @@ -532,52 +506,46 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const 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] - ); - + tempResult.m_barycentricCoords[VERTA], + 0, + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC]); } - } - // Repeat test for face adb + } + // Repeat test for face adb - if (pointOutsideADB) { - closestPtPointTriangle(p, a, d, b,tempResult); + closestPtPointTriangle(p, a, d, b, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) + btScalar sqDist = (q - p).dot(q - p); + if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.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] - ); - + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + 0, + tempResult.m_barycentricCoords[VERTB]); } - } - // Repeat test for face bdc - + } + // Repeat test for face bdc if (pointOutsideBDC) { - closestPtPointTriangle(p, b, d, c,tempResult); + closestPtPointTriangle(p, b, d, c, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) + btScalar sqDist = (q - p).dot(q - p); + if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; @@ -588,25 +556,22 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( - 0, - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTC], - tempResult.m_barycentricCoords[VERTB] - ); - + 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) + finalResult.m_usedVertices.usedVertexD) { return true; } - return true; + return true; } - diff --git a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index 80fd490f4..24a0a8f2d 100644 --- a/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -13,15 +13,11 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_VORONOI_SIMPLEX_SOLVER_H #define BT_VORONOI_SIMPLEX_SOLVER_H #include "btSimplexSolverInterface.h" - - #define VORONOI_SIMPLEX_MAX_VERTS 5 ///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure @@ -31,9 +27,10 @@ subject to the following restrictions: #define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 1e-12f #else #define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f -#endif//BT_USE_DOUBLE_PRECISION +#endif //BT_USE_DOUBLE_PRECISION -struct btUsageBitfield{ +struct btUsageBitfield +{ btUsageBitfield() { reset(); @@ -46,140 +43,131 @@ struct btUsageBitfield{ 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; + 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 btSubSimplexClosestResult +struct btSubSimplexClosestResult { - btVector3 m_closestPointOnSimplex; + btVector3 m_closestPointOnSimplex; //MASK for m_usedVertices - //stores the simplex vertex-usage, using the MASK, + //stores the simplex vertex-usage, using the MASK, // if m_usedVertices & MASK then the related vertex is used - btUsageBitfield m_usedVertices; - btScalar m_barycentricCoords[4]; + btUsageBitfield m_usedVertices; + btScalar m_barycentricCoords[4]; bool m_degenerate; - void reset() + void reset() { m_degenerate = false; setBarycentricCoordinates(); m_usedVertices.reset(); } - bool isValid() + bool isValid() { bool valid = (m_barycentricCoords[0] >= btScalar(0.)) && - (m_barycentricCoords[1] >= btScalar(0.)) && - (m_barycentricCoords[2] >= btScalar(0.)) && - (m_barycentricCoords[3] >= btScalar(0.)); - + (m_barycentricCoords[1] >= btScalar(0.)) && + (m_barycentricCoords[2] >= btScalar(0.)) && + (m_barycentricCoords[3] >= btScalar(0.)); return valid; } - void setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.)) + void setBarycentricCoordinates(btScalar a = btScalar(0.), btScalar b = btScalar(0.), btScalar c = btScalar(0.), btScalar d = btScalar(0.)) { m_barycentricCoords[0] = a; m_barycentricCoords[1] = b; m_barycentricCoords[2] = c; m_barycentricCoords[3] = d; } - }; /// btVoronoiSimplexSolver 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. #ifdef NO_VIRTUAL_INTERFACE -ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver +ATTRIBUTE_ALIGNED16(class) +btVoronoiSimplexSolver #else -ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver : public btSimplexSolverInterface +ATTRIBUTE_ALIGNED16(class) +btVoronoiSimplexSolver : public btSimplexSolverInterface #endif { public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - int m_numVertices; + int m_numVertices; - btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; - btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; - btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; - - - btVector3 m_cachedP1; - btVector3 m_cachedP2; - btVector3 m_cachedV; - btVector3 m_lastW; - - btScalar m_equalVertexThreshold; - bool m_cachedValidClosest; + btVector3 m_cachedP1; + btVector3 m_cachedP2; + btVector3 m_cachedV; + btVector3 m_lastW; + btScalar m_equalVertexThreshold; + bool m_cachedValidClosest; btSubSimplexClosestResult m_cachedBC; - bool m_needsUpdate; - - void removeVertex(int index); - void reduceVertices (const btUsageBitfield& usedVerts); - bool updateClosestVectorAndPoints(); + bool m_needsUpdate; - bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult); - int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); - bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result); + void removeVertex(int index); + void reduceVertices(const btUsageBitfield& usedVerts); + bool updateClosestVectorAndPoints(); + + bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult); + int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); + bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btSubSimplexClosestResult& result); public: - btVoronoiSimplexSolver() - : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) + : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) { } - void reset(); + void reset(); - void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); + void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); - void setEqualVertexThreshold(btScalar threshold) - { - m_equalVertexThreshold = threshold; - } + void setEqualVertexThreshold(btScalar threshold) + { + m_equalVertexThreshold = threshold; + } - btScalar getEqualVertexThreshold() const - { - return m_equalVertexThreshold; - } + btScalar getEqualVertexThreshold() const + { + return m_equalVertexThreshold; + } - bool closest(btVector3& v); + bool closest(btVector3 & v); - btScalar maxVertex(); + btScalar maxVertex(); - bool fullSimplex() const - { - return (m_numVertices == 4); - } + bool fullSimplex() const + { + return (m_numVertices == 4); + } - int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; + int getSimplex(btVector3 * pBuf, btVector3 * qBuf, btVector3 * yBuf) const; - bool inSimplex(const btVector3& w); - - void backup_closest(btVector3& v) ; + bool inSimplex(const btVector3& w); - bool emptySimplex() const ; + void backup_closest(btVector3 & v); - void compute_points(btVector3& p1, btVector3& p2) ; - - int numVertices() const - { - return m_numVertices; - } + bool emptySimplex() const; + void compute_points(btVector3 & p1, btVector3 & p2); + int numVertices() const + { + return m_numVertices; + } }; -#endif //BT_VORONOI_SIMPLEX_SOLVER_H - +#endif //BT_VORONOI_SIMPLEX_SOLVER_H diff --git a/Engine/lib/bullet/src/BulletCollision/premake4.lua b/Engine/lib/bullet/src/BulletCollision/premake4.lua index 70019df8f..e1c9bdbab 100644 --- a/Engine/lib/bullet/src/BulletCollision/premake4.lua +++ b/Engine/lib/bullet/src/BulletCollision/premake4.lua @@ -1,6 +1,9 @@ project "BulletCollision" kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end includedirs { "..", } diff --git a/Engine/lib/bullet/src/BulletDynamics/CMakeLists.txt b/Engine/lib/bullet/src/BulletDynamics/CMakeLists.txt index f8a6f34ba..3332440f2 100644 --- a/Engine/lib/bullet/src/BulletDynamics/CMakeLists.txt +++ b/Engine/lib/bullet/src/BulletDynamics/CMakeLists.txt @@ -15,6 +15,8 @@ SET(BulletDynamics_SRCS ConstraintSolver/btHingeConstraint.cpp ConstraintSolver/btPoint2PointConstraint.cpp ConstraintSolver/btSequentialImpulseConstraintSolver.cpp + ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp + ConstraintSolver/btBatchedConstraints.cpp ConstraintSolver/btNNCGConstraintSolver.cpp ConstraintSolver/btSliderConstraint.cpp ConstraintSolver/btSolve2LinearConstraint.cpp @@ -29,15 +31,17 @@ SET(BulletDynamics_SRCS Vehicle/btRaycastVehicle.cpp Vehicle/btWheelInfo.cpp Featherstone/btMultiBody.cpp + Featherstone/btMultiBodyConstraint.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 + Featherstone/btMultiBodyJointLimitConstraint.cpp + Featherstone/btMultiBodyJointMotor.cpp + Featherstone/btMultiBodyMLCPConstraintSolver.cpp + Featherstone/btMultiBodyPoint2Point.cpp + Featherstone/btMultiBodySliderConstraint.cpp + Featherstone/btMultiBodySphericalJointMotor.cpp MLCPSolvers/btDantzigLCP.cpp MLCPSolvers/btMLCPSolver.cpp MLCPSolvers/btLemkeAlgorithm.cpp @@ -62,6 +66,7 @@ SET(ConstraintSolver_HDRS ConstraintSolver/btJacobianEntry.h ConstraintSolver/btPoint2PointConstraint.h ConstraintSolver/btSequentialImpulseConstraintSolver.h + ConstraintSolver/btSequentialImpulseConstraintSolverMt.h ConstraintSolver/btNNCGConstraintSolver.h ConstraintSolver/btSliderConstraint.h ConstraintSolver/btSolve2LinearConstraint.h @@ -87,19 +92,19 @@ SET(Vehicle_HDRS SET(Featherstone_HDRS Featherstone/btMultiBody.h + Featherstone/btMultiBodyConstraint.h Featherstone/btMultiBodyConstraintSolver.h Featherstone/btMultiBodyDynamicsWorld.h + Featherstone/btMultiBodyFixedConstraint.h + Featherstone/btMultiBodyGearConstraint.h + Featherstone/btMultiBodyJointLimitConstraint.h + Featherstone/btMultiBodyJointMotor.h Featherstone/btMultiBodyLink.h Featherstone/btMultiBodyLinkCollider.h - Featherstone/btMultiBodySolverConstraint.h - Featherstone/btMultiBodyConstraint.h - Featherstone/btMultiBodyJointLimitConstraint.h - Featherstone/btMultiBodyConstraint.h + Featherstone/btMultiBodyMLCPConstraintSolver.h Featherstone/btMultiBodyPoint2Point.h - Featherstone/btMultiBodyFixedConstraint.h Featherstone/btMultiBodySliderConstraint.h - Featherstone/btMultiBodyJointMotor.h - Featherstone/btMultiBodyGearConstraint.h + Featherstone/btMultiBodySolverConstraint.h ) SET(MLCPSolvers_HDRS diff --git a/Engine/lib/bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h b/Engine/lib/bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h index c3a3ac6c8..2ccf317b9 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h +++ b/Engine/lib/bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h @@ -26,22 +26,21 @@ class btCollisionWorld; class btCharacterControllerInterface : public btActionInterface { public: - btCharacterControllerInterface () {}; - virtual ~btCharacterControllerInterface () {}; - - virtual void setWalkDirection(const btVector3& walkDirection) = 0; - virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0; - virtual void reset ( btCollisionWorld* collisionWorld ) = 0; - virtual void warp (const btVector3& origin) = 0; + btCharacterControllerInterface(){}; + virtual ~btCharacterControllerInterface(){}; - virtual void preStep ( btCollisionWorld* collisionWorld) = 0; - virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0; - virtual bool canJump () const = 0; - virtual void jump(const btVector3& dir = btVector3()) = 0; + virtual void setWalkDirection(const btVector3& walkDirection) = 0; + virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0; + virtual void reset(btCollisionWorld* collisionWorld) = 0; + virtual void warp(const btVector3& origin) = 0; - virtual bool onGround () const = 0; - virtual void setUpInterpolate (bool value) = 0; + virtual void preStep(btCollisionWorld* collisionWorld) = 0; + virtual void playerStep(btCollisionWorld* collisionWorld, btScalar dt) = 0; + virtual bool canJump() const = 0; + virtual void jump(const btVector3& dir = btVector3(0, 0, 0)) = 0; + + virtual bool onGround() const = 0; + virtual void setUpInterpolate(bool value) = 0; }; -#endif //BT_CHARACTER_CONTROLLER_INTERFACE_H - +#endif //BT_CHARACTER_CONTROLLER_INTERFACE_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp index cb1aa71a1..2bbccb291 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionDispatch/btGhostObject.h" @@ -24,20 +23,19 @@ subject to the following restrictions: #include "LinearMath/btDefaultMotionState.h" #include "btKinematicCharacterController.h" - // static helper method static btVector3 getNormalizedVector(const btVector3& v) { btVector3 n(0, 0, 0); - if (v.length() > SIMD_EPSILON) { + if (v.length() > SIMD_EPSILON) + { n = v.normalized(); } return n; } - ///@todo Interact with dynamic objects, ///Ride kinematicly animated platforms properly ///More realistic (or maybe just a config option) falling @@ -47,18 +45,19 @@ getNormalizedVector(const btVector3& v) class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback { public: - btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) + btKinematicClosestNotMeRayResultCallback(btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) { m_me = me; } - virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) { if (rayResult.m_collisionObject == m_me) return 1.0; - return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace); + return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); } + protected: btCollisionObject* m_me; }; @@ -66,15 +65,12 @@ protected: class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback { public: - btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot) - : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) - , m_me(me) - , m_up(up) - , m_minSlopeDot(minSlopeDot) + btKinematicClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& up, btScalar minSlopeDot) + : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)), m_me(me), m_up(up), m_minSlopeDot(minSlopeDot) { } - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace) { if (convexResult.m_hitCollisionObject == m_me) return btScalar(1.0); @@ -86,19 +82,22 @@ public: if (normalInWorldSpace) { hitNormalWorld = convexResult.m_hitNormalLocal; - } else + } + else { ///need to transform normal into worldspace - hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; + hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal; } btScalar dotUp = m_up.dot(hitNormalWorld); - if (dotUp < m_minSlopeDot) { + if (dotUp < m_minSlopeDot) + { return btScalar(1.0); } - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); + return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace); } + protected: btCollisionObject* m_me; const btVector3 m_up; @@ -110,7 +109,7 @@ protected: * * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html */ -btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal) +btVector3 btKinematicCharacterController::computeReflectionDirection(const btVector3& direction, const btVector3& normal) { return direction - (btScalar(2.0) * direction.dot(normal)) * normal; } @@ -118,7 +117,7 @@ btVector3 btKinematicCharacterController::computeReflectionDirection (const btVe /* * Returns the portion of 'direction' that is parallel to 'normal' */ -btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal) +btVector3 btKinematicCharacterController::parallelComponent(const btVector3& direction, const btVector3& normal) { btScalar magnitude = direction.dot(normal); return normal * magnitude; @@ -127,29 +126,29 @@ btVector3 btKinematicCharacterController::parallelComponent (const btVector3& di /* * Returns the portion of 'direction' that is perpindicular to 'normal' */ -btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal) +btVector3 btKinematicCharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal) { return direction - parallelComponent(direction, normal); } -btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up) +btKinematicCharacterController::btKinematicCharacterController(btPairCachingGhostObject* ghostObject, btConvexShape* convexShape, btScalar stepHeight, const btVector3& up) { m_ghostObject = ghostObject; 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_walkDirection.setValue(0.0, 0.0, 0.0); m_AngVel.setValue(0.0, 0.0, 0.0); - m_useGhostObjectSweepTest = true; + m_useGhostObjectSweepTest = true; m_turnAngle = btScalar(0.0); - m_convexShape=convexShape; - m_useWalkDirection = true; // use walk direction by default, legacy behavior + 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.0 ; // 3G acceleration. - m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s. - m_jumpSpeed = 10.0; // ? + 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; @@ -166,7 +165,7 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho setMaxSlope(btRadians(45.0)); } -btKinematicCharacterController::~btKinematicCharacterController () +btKinematicCharacterController::~btKinematicCharacterController() { } @@ -175,7 +174,7 @@ btPairCachingGhostObject* btKinematicCharacterController::getGhostObject() return m_ghostObject; } -bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld) +bool btKinematicCharacterController::recoverFromPenetration(btCollisionWorld* collisionWorld) { // Here we must refresh the overlapping paircache as the penetrating movement itself or the // previous recovery iteration might have used setWorldTransform and pushed us into an object @@ -186,19 +185,19 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* // paircache and the ghostobject's internal paircache at the same time. /BW btVector3 minAabb, maxAabb; - m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb,maxAabb); - collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(), - minAabb, - maxAabb, - collisionWorld->getDispatcher()); - + m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb, maxAabb); + collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(), + minAabb, + maxAabb, + collisionWorld->getDispatcher()); + bool penetration = false; collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher()); 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); @@ -206,25 +205,24 @@ 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); - - for (int j=0;jgetBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0); - for (int p=0;pgetNumContacts();p++) + for (int p = 0; p < manifold->getNumContacts(); p++) { - const btManifoldPoint&pt = manifold->getContactPoint(p); + const btManifoldPoint& pt = manifold->getContactPoint(p); btScalar dist = pt.getDistance(); @@ -239,22 +237,24 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* //} m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2); penetration = true; - } else { + } + else + { //printf("touching %f\n", dist); } } - + //manifold->clearManifold(); } } btTransform newTrans = m_ghostObject->getWorldTransform(); newTrans.setOrigin(m_currentPosition); m_ghostObject->setWorldTransform(newTrans); -// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]); + // printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]); return penetration; } -void btKinematicCharacterController::stepUp ( btCollisionWorld* world) +void btKinematicCharacterController::stepUp(btCollisionWorld* world) { btScalar stepHeight = 0.0f; if (m_verticalVelocity < 0.0) @@ -263,8 +263,8 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) // phase 1: up btTransform start, end; - start.setIdentity (); - end.setIdentity (); + start.setIdentity(); + end.setIdentity(); /* FIXME: Handle penetration properly */ start.setOrigin(m_currentPosition); @@ -272,7 +272,7 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) 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); + end.setOrigin(m_targetPosition); start.setRotation(m_currentOrientation); end.setRotation(m_targetOrientation); @@ -280,10 +280,10 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - + if (m_useGhostObjectSweepTest) { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); + m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); } else { @@ -298,7 +298,7 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) // we moved up only a fraction of the step height m_currentStepOffset = stepHeight * callback.m_closestHitFraction; if (m_interpolateUp == true) - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction); else m_currentPosition = m_targetPosition; } @@ -329,7 +329,9 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) m_verticalVelocity = 0.0; m_currentStepOffset = m_stepHeight; } - } else { + } + else + { m_currentStepOffset = stepHeight; m_currentPosition = m_targetPosition; } @@ -342,43 +344,44 @@ bool btKinematicCharacterController::needsCollision(const btCollisionObject* bod return collides; } -void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) +void btKinematicCharacterController::updateTargetPositionBasedOnCollision(const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) { btVector3 movementDirection = m_targetPosition - m_currentPosition; btScalar movementLength = movementDirection.length(); - if (movementLength>SIMD_EPSILON) + if (movementLength > SIMD_EPSILON) { movementDirection.normalize(); - btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal); + btVector3 reflectDir = computeReflectionDirection(movementDirection, hitNormal); reflectDir.normalize(); btVector3 parallelDir, perpindicularDir; - parallelDir = parallelComponent (reflectDir, hitNormal); - perpindicularDir = perpindicularComponent (reflectDir, hitNormal); + parallelDir = parallelComponent(reflectDir, hitNormal); + perpindicularDir = perpindicularComponent(reflectDir, hitNormal); m_targetPosition = m_currentPosition; - if (0)//tangentMag != 0.0) + if (0) //tangentMag != 0.0) { - btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength); -// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]); - m_targetPosition += parComponent; + btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength); + // printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]); + m_targetPosition += parComponent; } if (normalMag != 0.0) { - btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength); -// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]); + btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength); + // printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]); m_targetPosition += perpComponent; } - } else + } + else { -// printf("movementLength don't normalize a zero vector\n"); + // printf("movementLength don't normalize a zero vector\n"); } } -void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove) +void btKinematicCharacterController::stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove) { // printf("m_normalizedDirection=%f,%f,%f\n", // m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]); @@ -387,29 +390,28 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co m_targetPosition = m_currentPosition + walkMove; - start.setIdentity (); - end.setIdentity (); - + start.setIdentity(); + end.setIdentity(); + btScalar fraction = 1.0; - btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); -// printf("distance2=%f\n",distance2); + btScalar distance2 = (m_currentPosition - m_targetPosition).length2(); + // printf("distance2=%f\n",distance2); int maxIter = 10; while (fraction > btScalar(0.01) && maxIter-- > 0) { - start.setOrigin (m_currentPosition); - end.setOrigin (m_targetPosition); + start.setOrigin(m_currentPosition); + 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)); + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.0)); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - btScalar margin = m_convexShape->getMargin(); m_convexShape->setMargin(margin + m_addedMargin); @@ -426,18 +428,17 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co } m_convexShape->setMargin(margin); - fraction -= callback.m_closestHitFraction; if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) - { + { // we moved only a fraction //btScalar hitDistance; //hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); -// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + // m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld); + updateTargetPositionBasedOnCollision(callback.m_hitNormalWorld); btVector3 currentDir = m_targetPosition - m_currentPosition; distance2 = currentDir.length2(); if (distance2 > SIMD_EPSILON) @@ -448,21 +449,21 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co { break; } - } else + } + else { -// printf("currentDir: don't normalize a zero vector\n"); + // printf("currentDir: don't normalize a zero vector\n"); break; } - } - else - { - m_currentPosition = m_targetPosition; + else + { + m_currentPosition = m_targetPosition; } } } -void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt) +void btKinematicCharacterController::stepDown(btCollisionWorld* collisionWorld, btScalar dt) { btTransform start, end, end_double; bool runonce = false; @@ -475,64 +476,64 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld m_targetPosition -= (step_drop + gravity_drop);*/ btVector3 orig_position = m_targetPosition; - - btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + + 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)) + if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) downVelocity = m_fallSpeed; btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine); - callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; while (1) { - start.setIdentity (); - end.setIdentity (); + start.setIdentity(); + end.setIdentity(); - end_double.setIdentity (); + end_double.setIdentity(); - start.setOrigin (m_currentPosition); - end.setOrigin (m_targetPosition); + 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); + end_double.setOrigin(m_targetPosition - step_drop); if (m_useGhostObjectSweepTest) { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, 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 (full) or not (partial) - m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + m_ghostObject->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); } - } else + } + else { - collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + collisionWorld->convexSweepTest(m_convexShape, start, end, callback, 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); + collisionWorld->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); } } - - btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + + btScalar downVelocity2 = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt; bool has_hit; if (bounce_fix == true) has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject); @@ -543,8 +544,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld if (m_verticalVelocity < 0.0) stepHeight = m_stepHeight; - if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false - && (m_wasOnGround || !m_wasJumping)) + 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 @@ -555,7 +555,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; runonce = true; - continue; //re-run previous tests + continue; //re-run previous tests } break; } @@ -570,30 +570,32 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld if (bounce_fix == true) { if (full_drop == true) - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - else + 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, fraction); } else - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction); full_drop = false; m_verticalVelocity = 0.0; m_verticalOffset = 0.0; m_wasJumping = false; - } else { + } + else + { // we dropped the full height full_drop = true; if (bounce_fix == true) { - downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + downVelocity = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt; if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { - m_targetPosition += step_drop; //undo previous target change + m_targetPosition += step_drop; //undo previous target change downVelocity = m_fallSpeed; step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; @@ -605,30 +607,22 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld } } - - -void btKinematicCharacterController::setWalkDirection -( -const btVector3& walkDirection -) +void btKinematicCharacterController::setWalkDirection( + const btVector3& walkDirection) { m_useWalkDirection = true; m_walkDirection = walkDirection; m_normalizedDirection = getNormalizedVector(m_walkDirection); } - - -void btKinematicCharacterController::setVelocityForTimeInterval -( -const btVector3& velocity, -btScalar timeInterval -) +void btKinematicCharacterController::setVelocityForTimeInterval( + const btVector3& velocity, + btScalar timeInterval) { -// printf("setVelocity!\n"); -// printf(" interval: %f\n", timeInterval); -// printf(" velocity: (%f, %f, %f)\n", -// velocity.x(), velocity.y(), velocity.z()); + // printf("setVelocity!\n"); + // printf(" interval: %f\n", timeInterval); + // printf(" velocity: (%f, %f, %f)\n", + // velocity.x(), velocity.y(), velocity.z()); m_useWalkDirection = false; m_walkDirection = velocity; @@ -661,7 +655,7 @@ void btKinematicCharacterController::setLinearVelocity(const btVector3& 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; @@ -678,46 +672,45 @@ btVector3 btKinematicCharacterController::getLinearVelocity() const return m_walkDirection + (m_verticalVelocity * m_up); } -void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld ) +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; + 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()); - } + //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) +void btKinematicCharacterController::warp(const btVector3& origin) { btTransform xform; xform.setIdentity(); - xform.setOrigin (origin); - m_ghostObject->setWorldTransform (xform); + xform.setOrigin(origin); + m_ghostObject->setWorldTransform(xform); } - -void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld) +void btKinematicCharacterController::preStep(btCollisionWorld* collisionWorld) { m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); m_targetPosition = m_currentPosition; 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]); + // printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]); } -void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt) +void btKinematicCharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt) { -// printf("playerStep(): "); -// printf(" dt = %f", dt); + // printf("playerStep(): "); + // printf(" dt = %f", dt); if (m_AngVel.length2() > 0.0f) { @@ -744,16 +737,17 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo } // quick check... - if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) { -// printf("\n"); - return; // no motion + if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) + { + // printf("\n"); + return; // no motion } m_wasOnGround = onGround(); //btVector3 lvel = m_walkDirection; //btScalar c = 0.0f; - + if (m_walkDirection.length2() > 0) { // apply damping @@ -761,7 +755,7 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo } 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) @@ -777,12 +771,12 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo btTransform xform; xform = m_ghostObject->getWorldTransform(); -// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); -// printf("walkSpeed=%f\n",walkSpeed); + // printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); + // printf("walkSpeed=%f\n",walkSpeed); stepUp(collisionWorld); //todo: Experimenting with behavior of controller when it hits a ceiling.. - //bool hitUp = stepUp (collisionWorld); + //bool hitUp = stepUp (collisionWorld); //if (hitUp) //{ // m_verticalVelocity -= m_gravity * dt; @@ -799,9 +793,12 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo // xform = m_ghostObject->getWorldTransform(); //} - if (m_useWalkDirection) { - stepForwardAndStrafe (collisionWorld, m_walkDirection); - } else { + if (m_useWalkDirection) + { + stepForwardAndStrafe(collisionWorld, m_walkDirection); + } + else + { //printf(" time: %f", m_velocityTimeInterval); // still have some time left for moving! btScalar dtMoving = @@ -816,7 +813,7 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo // okay, step stepForwardAndStrafe(collisionWorld, move); } - stepDown (collisionWorld, dt); + stepDown(collisionWorld, dt); //todo: Experimenting with max jump height //if (m_wasJumping) @@ -827,7 +824,7 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo // // substract the overshoot // m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight; - // // max height was reached, so potential energy is at max + // // 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; @@ -835,8 +832,8 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo //} // printf("\n"); - xform.setOrigin (m_currentPosition); - m_ghostObject->setWorldTransform (xform); + xform.setOrigin(m_currentPosition); + m_ghostObject->setWorldTransform(xform); int numPenetrationLoops = 0; m_touchingContact = false; @@ -852,23 +849,23 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo } } -void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) +void btKinematicCharacterController::setFallSpeed(btScalar fallSpeed) { m_fallSpeed = fallSpeed; } -void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) +void btKinematicCharacterController::setJumpSpeed(btScalar jumpSpeed) { m_jumpSpeed = jumpSpeed; m_SetjumpSpeed = m_jumpSpeed; } -void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) +void btKinematicCharacterController::setMaxJumpHeight(btScalar maxJumpHeight) { m_maxJumpHeight = maxJumpHeight; } -bool btKinematicCharacterController::canJump () const +bool btKinematicCharacterController::canJump() const { return onGround(); } @@ -927,20 +924,20 @@ btScalar btKinematicCharacterController::getMaxPenetrationDepth() const return m_maxPenetrationDepth; } -bool btKinematicCharacterController::onGround () const +bool btKinematicCharacterController::onGround() const { return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON); } -void btKinematicCharacterController::setStepHeight(btScalar h) +void btKinematicCharacterController::setStepHeight(btScalar h) { m_stepHeight = h; } btVector3* btKinematicCharacterController::getUpAxisDirections() { - static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) }; - + static btVector3 sUpAxisDirection[3] = {btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f)}; + return sUpAxisDirection; } @@ -997,4 +994,3 @@ btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector return shortestArcQuatNormalize2(v0, v1); } - diff --git a/Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.h b/Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.h index 3d677e647..ff34fc871 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.h +++ b/Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_KINEMATIC_CHARACTER_CONTROLLER_H #define BT_KINEMATIC_CHARACTER_CONTROLLER_H @@ -23,7 +22,6 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" - class btCollisionShape; class btConvexShape; class btRigidBody; @@ -34,15 +32,15 @@ class btPairCachingGhostObject; ///btKinematicCharacterController is an object that supports a sliding motion in a world. ///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations. ///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user. -ATTRIBUTE_ALIGNED16(class) btKinematicCharacterController : public btCharacterControllerInterface +ATTRIBUTE_ALIGNED16(class) +btKinematicCharacterController : public btCharacterControllerInterface { protected: - btScalar m_halfHeight; - + 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 - + 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; @@ -50,33 +48,33 @@ protected: 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) + 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) btScalar m_gravity; btScalar m_turnAngle; - + btScalar m_stepHeight; - btScalar m_addedMargin;//@todo: remove this and fix the code + btScalar m_addedMargin; //@todo: remove this and fix the code ///this is the desired walk direction, set by the user - btVector3 m_walkDirection; - btVector3 m_normalizedDirection; - btVector3 m_AngVel; + btVector3 m_walkDirection; + btVector3 m_normalizedDirection; + btVector3 m_AngVel; - btVector3 m_jumpPosition; + btVector3 m_jumpPosition; //some internal variables btVector3 m_currentPosition; - btScalar m_currentStepOffset; + btScalar m_currentStepOffset; btVector3 m_targetPosition; btQuaternion m_currentOrientation; btQuaternion m_targetOrientation; ///keep track of the contact manifolds - btManifoldArray m_manifoldArray; + btManifoldArray m_manifoldArray; bool m_touchingContact; btVector3 m_touchingNormal; @@ -84,52 +82,50 @@ protected: btScalar m_linearDamping; btScalar m_angularDamping; - bool m_wasOnGround; - bool m_wasJumping; - bool m_useGhostObjectSweepTest; - bool m_useWalkDirection; - btScalar m_velocityTimeInterval; + bool m_wasOnGround; + bool m_wasJumping; + bool m_useGhostObjectSweepTest; + bool m_useWalkDirection; + btScalar m_velocityTimeInterval; btVector3 m_up; btVector3 m_jumpAxis; static btVector3* getUpAxisDirections(); - bool m_interpolateUp; - bool full_drop; - bool bounce_fix; + bool m_interpolateUp; + bool full_drop; + bool bounce_fix; - btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal); - btVector3 parallelComponent (const btVector3& direction, const btVector3& normal); - btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal); + btVector3 computeReflectionDirection(const btVector3& direction, const btVector3& normal); + btVector3 parallelComponent(const btVector3& direction, const btVector3& normal); + btVector3 perpindicularComponent(const btVector3& direction, const btVector3& normal); - bool recoverFromPenetration ( btCollisionWorld* collisionWorld); - void stepUp (btCollisionWorld* collisionWorld); - 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); + bool recoverFromPenetration(btCollisionWorld * collisionWorld); + void stepUp(btCollisionWorld * collisionWorld); + 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; + btQuaternion getRotation(btVector3 & v0, btVector3 & v1) const; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0)); - ~btKinematicCharacterController (); - + btKinematicCharacterController(btPairCachingGhostObject * ghostObject, btConvexShape * convexShape, btScalar stepHeight, const btVector3& up = btVector3(1.0, 0.0, 0.0)); + ~btKinematicCharacterController(); ///btActionInterface interface - virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime) + virtual void updateAction(btCollisionWorld * collisionWorld, btScalar deltaTime) { - preStep ( collisionWorld); - playerStep (collisionWorld, deltaTime); + preStep(collisionWorld); + playerStep(collisionWorld, deltaTime); } - + ///btActionInterface interface - void debugDraw(btIDebugDraw* debugDrawer); + void debugDraw(btIDebugDraw * debugDrawer); void setUp(const btVector3& up); @@ -140,7 +136,7 @@ public: /// increment the position each simulation iteration, regardless /// of dt. /// This call will reset any velocity set by setVelocityForTimeInterval(). - virtual void setWalkDirection(const btVector3& walkDirection); + virtual void setWalkDirection(const btVector3& walkDirection); /// Caller provides a velocity with which the character should move for /// the given time period. After the time period, velocity is reset @@ -148,7 +144,7 @@ public: /// This call will reset any walk direction set by setWalkDirection(). /// Negative time intervals will result in no motion. virtual void setVelocityForTimeInterval(const btVector3& velocity, - btScalar timeInterval); + btScalar timeInterval); virtual void setAngularVelocity(const btVector3& velocity); virtual const btVector3& getAngularVelocity() const; @@ -157,26 +153,26 @@ public: 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; } + 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; } + btScalar getAngularDamping() const { return m_angularDamping; } - void reset ( btCollisionWorld* collisionWorld ); - void warp (const btVector3& origin); + void reset(btCollisionWorld * collisionWorld); + void warp(const btVector3& origin); - void preStep ( btCollisionWorld* collisionWorld); - void playerStep ( btCollisionWorld* collisionWorld, btScalar dt); + void preStep(btCollisionWorld * collisionWorld); + void playerStep(btCollisionWorld * collisionWorld, btScalar dt); void setStepHeight(btScalar h); btScalar getStepHeight() const { return m_stepHeight; } - void setFallSpeed (btScalar fallSpeed); + void setFallSpeed(btScalar fallSpeed); btScalar getFallSpeed() const { return m_fallSpeed; } - void setJumpSpeed (btScalar jumpSpeed); + void setJumpSpeed(btScalar jumpSpeed); btScalar getJumpSpeed() const { return m_jumpSpeed; } - void setMaxJumpHeight (btScalar maxJumpHeight); - bool canJump () const; + void setMaxJumpHeight(btScalar maxJumpHeight); + bool canJump() const; - void jump(const btVector3& v = btVector3()); + void jump(const btVector3& v = btVector3(0, 0, 0)); void applyImpulse(const btVector3& v) { jump(v); } @@ -192,13 +188,13 @@ public: btScalar getMaxPenetrationDepth() const; btPairCachingGhostObject* getGhostObject(); - void setUseGhostSweepTest(bool useGhostObjectSweepTest) + void setUseGhostSweepTest(bool useGhostObjectSweepTest) { m_useGhostObjectSweepTest = useGhostObjectSweepTest; } - bool onGround () const; - void setUpInterpolate (bool value); + bool onGround() const; + void setUpInterpolate(bool value); }; -#endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H +#endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp new file mode 100644 index 000000000..2a5efc649 --- /dev/null +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp @@ -0,0 +1,1082 @@ +/* +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 + +#include + +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 = std::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/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h new file mode 100644 index 000000000..5d982ca37 --- /dev/null +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h @@ -0,0 +1,62 @@ +/* +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/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 0572256f7..ac046aa6e 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -15,49 +15,38 @@ subject to the following restrictions: Written by: Marcus Hennix */ - #include "btConeTwistConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" #include "LinearMath/btMinMax.h" +#include #include - - //#define CONETWIST_USE_OBSOLETE_SOLVER true #define CONETWIST_USE_OBSOLETE_SOLVER false #define CONETWIST_DEF_FIX_THRESH btScalar(.05f) - SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld) { btVector3 vec = axis * invInertiaWorld; return axis.dot(vec); } - - - -btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, - const btTransform& rbAFrame,const btTransform& rbBFrame) - :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), - m_angularOnly(false), - m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) +btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, btRigidBody& rbB, + const btTransform& rbAFrame, const btTransform& rbBFrame) + : btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { init(); } -btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) - :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), - m_angularOnly(false), - m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) +btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, const btTransform& rbAFrame) + : btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { m_rbBFrame = m_rbAFrame; m_rbBFrame.setOrigin(btVector3(0., 0., 0.)); - init(); + init(); } - void btConeTwistConstraint::init() { m_angularOnly = false; @@ -75,30 +64,29 @@ void btConeTwistConstraint::init() m_angCFM = btScalar(0.f); } - -void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) +void btConeTwistConstraint::getInfo1(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; info->nub = 0; - } + } else { info->m_numConstraintRows = 3; info->nub = 3; - calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); - if(m_solveSwingLimit) + calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld()); + if (m_solveSwingLimit) { info->m_numConstraintRows++; info->nub--; - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { info->m_numConstraintRows++; info->nub--; } } - if(m_solveTwistLimit) + if (m_solveTwistLimit) { info->m_numConstraintRows++; info->nub--; @@ -106,90 +94,88 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) } } -void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info) +void btConeTwistConstraint::getInfo1NonVirtual(btConstraintInfo1* info) { //always reserve 6 rows: object transform is not available on SPU info->m_numConstraintRows = 6; info->nub = 0; - -} - - -void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) -{ - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); } -void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) +void btConeTwistConstraint::getInfo2(btConstraintInfo2* info) { - calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB); - + getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld()); +} + +void btConeTwistConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB) +{ + calcAngleInfo2(transA, transB, invInertiaWorldA, invInertiaWorldB); + btAssert(!m_useSolveConstraintObsolete); - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip + 1] = 1; + info->m_J1linearAxis[2 * info->rowskip + 2] = 1; btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip); btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2); } - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip + 1] = -1; + info->m_J2linearAxis[2 * info->rowskip + 2] = -1; btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip); + a2.getSkewSymmetricMatrix(angular0, angular1, angular2); } - // set right hand side + // set right hand side btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp; - btScalar k = info->fps * linERP; - int j; - for (j=0; j<3; j++) - { - info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]); - info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY; - info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY; - if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM) + btScalar k = info->fps * linERP; + int j; + for (j = 0; j < 3; j++) + { + info->m_constraintError[j * info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]); + info->m_lowerLimit[j * info->rowskip] = -SIMD_INFINITY; + info->m_upperLimit[j * info->rowskip] = SIMD_INFINITY; + if (m_flags & BT_CONETWIST_FLAGS_LIN_CFM) { - info->cfm[j*info->rowskip] = m_linCFM; + info->cfm[j * info->rowskip] = m_linCFM; } - } + } int row = 3; - int srow = row * info->rowskip; + int srow = row * info->rowskip; btVector3 ax1; // angular limits - if(m_solveSwingLimit) + if (m_solveSwingLimit) { - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + btScalar* J1 = info->m_J1angularAxis; + btScalar* J2 = info->m_J2angularAxis; + if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { - btTransform trA = transA*m_rbAFrame; + btTransform trA = transA * m_rbAFrame; btVector3 p = trA.getBasis().getColumn(1); btVector3 q = trA.getBasis().getColumn(2); int srow1 = srow + info->rowskip; - J1[srow+0] = p[0]; - J1[srow+1] = p[1]; - J1[srow+2] = p[2]; - J1[srow1+0] = q[0]; - J1[srow1+1] = q[1]; - J1[srow1+2] = q[2]; - J2[srow+0] = -p[0]; - J2[srow+1] = -p[1]; - J2[srow+2] = -p[2]; - J2[srow1+0] = -q[0]; - J2[srow1+1] = -q[1]; - J2[srow1+2] = -q[2]; + J1[srow + 0] = p[0]; + J1[srow + 1] = p[1]; + J1[srow + 2] = p[2]; + J1[srow1 + 0] = q[0]; + J1[srow1 + 1] = q[1]; + J1[srow1 + 2] = q[2]; + J2[srow + 0] = -p[0]; + J2[srow + 1] = -p[1]; + J2[srow + 2] = -p[2]; + J2[srow1 + 0] = -q[0]; + J2[srow1 + 1] = -q[1]; + J2[srow1 + 2] = -q[2]; btScalar fact = info->fps * m_relaxationFactor; - info->m_constraintError[srow] = fact * m_swingAxis.dot(p); - info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); + info->m_constraintError[srow] = fact * m_swingAxis.dot(p); + info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; info->m_lowerLimit[srow1] = -SIMD_INFINITY; @@ -199,16 +185,16 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt else { ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; + J1[srow + 0] = ax1[0]; + J1[srow + 1] = ax1[1]; + J1[srow + 2] = ax1[2]; + J2[srow + 0] = -ax1[0]; + J2[srow + 1] = -ax1[1]; + J2[srow + 2] = -ax1[2]; btScalar k = info->fps * m_biasFactor; info->m_constraintError[srow] = k * m_swingCorrection; - if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) + if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM) { info->cfm[srow] = m_angCFM; } @@ -218,36 +204,35 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt srow += info->rowskip; } } - if(m_solveTwistLimit) + if (m_solveTwistLimit) { ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; + btScalar* J1 = info->m_J1angularAxis; + btScalar* J2 = info->m_J2angularAxis; + J1[srow + 0] = ax1[0]; + J1[srow + 1] = ax1[1]; + J1[srow + 2] = ax1[2]; + J2[srow + 0] = -ax1[0]; + J2[srow + 1] = -ax1[1]; + J2[srow + 2] = -ax1[2]; btScalar k = info->fps * m_biasFactor; info->m_constraintError[srow] = k * m_twistCorrection; - if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) + if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM) { info->cfm[srow] = m_angCFM; } - if(m_twistSpan > 0.0f) + if (m_twistSpan > 0.0f) { - - if(m_twistCorrection > 0.0f) + if (m_twistCorrection > 0.0f) { info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; - } + } else { info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; - } + } } else { @@ -257,22 +242,20 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt srow += info->rowskip; } } - - -void btConeTwistConstraint::buildJacobian() +void btConeTwistConstraint::buildJacobian() { if (m_useSolveConstraintObsolete) { m_appliedImpulse = btScalar(0.); m_accTwistLimitImpulse = btScalar(0.); m_accSwingLimitImpulse = btScalar(0.); - m_accMotorImpulse = btVector3(0.,0.,0.); + m_accMotorImpulse = btVector3(0., 0., 0.); if (!m_angularOnly) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin(); btVector3 relPos = pivotBInW - pivotAInW; btVector3 normal[3]; @@ -282,71 +265,68 @@ void btConeTwistConstraint::buildJacobian() } else { - normal[0].setValue(btScalar(1.0),0,0); + normal[0].setValue(btScalar(1.0), 0, 0); } btPlaneSpace1(normal[0], normal[1], normal[2]); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normal[i], + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); } } - calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); + calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld()); } } - - -void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA, btSolverBody& bodyB, btScalar timeStep) { - #ifndef __SPU__ +#ifndef __SPU__ if (m_useSolveConstraintObsolete) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin(); btScalar tau = btScalar(0.3); //linear part if (!m_angularOnly) { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1; - bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1, vel1); btVector3 vel2; - bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); + bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2, vel2); btVector3 vel = vel1 - vel2; - for (int i=0;i<3;i++) - { + for (int i = 0; i < 3; i++) + { const btVector3& normal = m_jac[i].m_linearJointAxis; btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btScalar rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; - + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); - bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); - + bodyA.internalApplyImpulse(normal * m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld() * ftorqueAxis1, impulse); + bodyB.internalApplyImpulse(normal * m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld() * ftorqueAxis2, -impulse); } } @@ -356,13 +336,17 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver // compute current and predicted transforms btTransform trACur = m_rbA.getCenterOfMassTransform(); btTransform trBCur = m_rbB.getCenterOfMassTransform(); - btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA); - btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB); - btTransform trAPred; trAPred.setIdentity(); - btVector3 zerovec(0,0,0); + btVector3 omegaA; + bodyA.internalGetAngularVelocity(omegaA); + btVector3 omegaB; + bodyB.internalGetAngularVelocity(omegaB); + btTransform trAPred; + trAPred.setIdentity(); + btVector3 zerovec(0, 0, 0); btTransformUtil::integrateTransform( trACur, zerovec, omegaA, timeStep, trAPred); - btTransform trBPred; trBPred.setIdentity(); + btTransform trBPred; + trBPred.setIdentity(); btTransformUtil::integrateTransform( trBCur, zerovec, omegaB, timeStep, trBPred); @@ -374,7 +358,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver // compute desired omegas in world btVector3 omegaADes, omegaBDes; - + btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); @@ -415,10 +399,10 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver { btScalar fMaxImpulse = m_maxMotorImpulse; if (m_bNormalizedMotorStrength) - fMaxImpulse = fMaxImpulse/kAxisAInv; + fMaxImpulse = fMaxImpulse / kAxisAInv; btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; - btScalar newUnclampedMag = newUnclampedAccImpulse.length(); + btScalar newUnclampedMag = newUnclampedAccImpulse.length(); if (newUnclampedMag > fMaxImpulse) { newUnclampedAccImpulse.normalize(); @@ -428,31 +412,32 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver m_accMotorImpulse += impulse; } - btScalar impulseMag = impulse.length(); - btVector3 impulseAxis = impulse / impulseMag; - - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + btScalar impulseMag = impulse.length(); + btVector3 impulseAxis = impulse / impulseMag; + bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag); } } - else if (m_damping > SIMD_EPSILON) // no motor: do a little damping + else if (m_damping > SIMD_EPSILON) // no motor: do a little damping { - btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA); - btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB); + btVector3 angVelA; + bodyA.internalGetAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.internalGetAngularVelocity(angVelB); btVector3 relVel = angVelB - angVelA; if (relVel.length2() > SIMD_EPSILON) { btVector3 relVelAxis = relVel.normalized(); - btScalar m_kDamping = btScalar(1.) / - (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + - getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); + btScalar m_kDamping = btScalar(1.) / + (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + + getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); btVector3 impulse = m_damping * m_kDamping * relVel; - btScalar impulseMag = impulse.length(); + btScalar impulseMag = impulse.length(); btVector3 impulseAxis = impulse / impulseMag; - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag); } } @@ -467,7 +452,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver // solve swing limit if (m_solveSwingLimit) { - btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; + btScalar amplitude = m_swingLimitRatio * m_swingCorrection * m_biasFactor / timeStep; btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); if (relSwingVel > 0) amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; @@ -475,7 +460,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver // Clamp the accumulated impulse btScalar temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); + m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0)); impulseMag = m_accSwingLimitImpulse - temp; btVector3 impulse = m_swingAxis * impulseMag; @@ -491,47 +476,41 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver impulseMag = impulse.length(); btVector3 noTwistSwingAxis = impulse / impulseMag; - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); + bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * noTwistSwingAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * noTwistSwingAxis, -impulseMag); } - // solve twist limit if (m_solveTwistLimit) { - btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep; - btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis ); - if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) + btScalar amplitude = m_twistLimitRatio * m_twistCorrection * m_biasFactor / timeStep; + btScalar relTwistVel = (angVelB - angVelA).dot(m_twistAxis); + if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; btScalar impulseMag = amplitude * m_kTwist; // Clamp the accumulated impulse btScalar temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); + m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0)); impulseMag = m_accTwistLimitImpulse - temp; - // btVector3 impulse = m_twistAxis * impulseMag; + // btVector3 impulse = m_twistAxis * impulseMag; - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); - } + bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * m_twistAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * m_twistAxis, -impulseMag); + } } } #else -btAssert(0); -#endif //__SPU__ + btAssert(0); +#endif //__SPU__ } - - - -void btConeTwistConstraint::updateRHS(btScalar timeStep) +void btConeTwistConstraint::updateRHS(btScalar timeStep) { (void)timeStep; - } - #ifndef __SPU__ void btConeTwistConstraint::calcAngleInfo() { @@ -540,15 +519,15 @@ void btConeTwistConstraint::calcAngleInfo() m_solveTwistLimit = false; m_solveSwingLimit = false; - btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0); - btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0); + btVector3 b1Axis1(0, 0, 0), b1Axis2(0, 0, 0), b1Axis3(0, 0, 0); + btVector3 b2Axis1(0, 0, 0), b2Axis2(0, 0, 0); b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); - btScalar swing1=btScalar(0.),swing2 = btScalar(0.); + btScalar swing1 = btScalar(0.), swing2 = btScalar(0.); - btScalar swx=btScalar(0.),swy = btScalar(0.); + btScalar swx = btScalar(0.), swy = btScalar(0.); btScalar thresh = btScalar(10.); btScalar fact; @@ -558,33 +537,33 @@ void btConeTwistConstraint::calcAngleInfo() b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis2); - swing1 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; + swing1 = btAtan2Fast(swy, swx); + fact = (swy * swy + swx * swx) * thresh * thresh; fact = fact / (fact + btScalar(1.0)); - swing1 *= fact; + swing1 *= fact; } if (m_swingSpan2 >= btScalar(0.05f)) { - b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); + b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis3); - swing2 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; + swing2 = btAtan2Fast(swy, swx); + fact = (swy * swy + swx * swx) * thresh * thresh; fact = fact / (fact + btScalar(1.0)); - swing2 *= fact; + swing2 *= fact; } - btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); - btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); - btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; + btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1); + btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2); + btScalar EllipseAngle = btFabs(swing1 * swing1) * RMaxAngle1Sq + btFabs(swing2 * swing2) * RMaxAngle2Sq; if (EllipseAngle > 1.0f) { - m_swingCorrection = EllipseAngle-1.0f; + m_swingCorrection = EllipseAngle - 1.0f; m_solveSwingLimit = true; // Calculate necessary axis & factors - m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); + m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3)); m_swingAxis.normalize(); btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; @@ -594,14 +573,14 @@ void btConeTwistConstraint::calcAngleInfo() if (m_twistSpan >= btScalar(0.)) { btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); - btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); - btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); - btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); + btQuaternion rotationArc = shortestArcQuat(b2Axis1, b1Axis1); + btVector3 TwistRef = quatRotate(rotationArc, b2Axis2); + btScalar twist = btAtan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); m_twistAngle = twist; -// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); + // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); - if (twist <= -m_twistSpan*lockedFreeFactor) + if (twist <= -m_twistSpan * lockedFreeFactor) { m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; @@ -609,7 +588,7 @@ void btConeTwistConstraint::calcAngleInfo() m_twistAxis.normalize(); m_twistAxis *= -1.0f; } - else if (twist > m_twistSpan*lockedFreeFactor) + else if (twist > m_twistSpan * lockedFreeFactor) { m_twistCorrection = (twist - m_twistSpan); m_solveTwistLimit = true; @@ -618,13 +597,11 @@ void btConeTwistConstraint::calcAngleInfo() } } } -#endif //__SPU__ +#endif //__SPU__ -static btVector3 vTwist(1,0,0); // twist axis in constraint's space +static btVector3 vTwist(1, 0, 0); // twist axis in constraint's space - - -void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) +void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB) { m_swingCorrection = btScalar(0.); m_twistLimitSign = btScalar(0.); @@ -632,7 +609,7 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr m_solveSwingLimit = false; // compute rotation of A wrt B (in constraint space) if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) - { // it is assumed that setMotorTarget() was alredy called + { // it is assumed that setMotorTarget() was alredy called // and motor target m_qTarget is within constraint limits // TODO : split rotation to pure swing and pure twist // compute desired transforms in world @@ -641,23 +618,22 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btTransform trB = transB * m_rbBFrame; btTransform trDeltaAB = trB * trPose * trA.inverse(); btQuaternion qDeltaAB = trDeltaAB.getRotation(); - btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); + btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); btScalar swingAxisLen2 = swingAxis.length2(); - if(btFuzzyZero(swingAxisLen2)) + if (btFuzzyZero(swingAxisLen2)) { - return; + return; } m_swingAxis = swingAxis; m_swingAxis.normalize(); m_swingCorrection = qDeltaAB.getAngle(); - if(!btFuzzyZero(m_swingCorrection)) + if (!btFuzzyZero(m_swingCorrection)) { m_solveSwingLimit = true; } return; } - { // compute rotation of A wrt B (in constraint space) btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation(); @@ -665,13 +641,17 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btQuaternion qAB = qB.inverse() * qA; // split rotation into cone and twist // (all this is done from B's perspective. Maybe I should be averaging axes...) - btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); - btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); - btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); + btVector3 vConeNoTwist = quatRotate(qAB, vTwist); + vConeNoTwist.normalize(); + btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); + qABCone.normalize(); + btQuaternion qABTwist = qABCone.inverse() * qAB; + qABTwist.normalize(); if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) { - btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; + btScalar swingAngle, swingLimit = 0; + btVector3 swingAxis; computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); if (swingAngle > swingLimit * m_limitSoftness) @@ -684,9 +664,9 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr m_swingLimitRatio = 1.f; if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) { - m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ + m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) / (swingLimit - swingLimit * m_limitSoftness); - } + } // swing correction tries to get back to soft limit m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); @@ -694,14 +674,14 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr // adjustment of swing axis (based on ellipse normal) adjustSwingAxisToUseEllipseNormal(swingAxis); - // Calculate necessary axis & factors + // Calculate necessary axis & factors m_swingAxis = quatRotate(qB, -swingAxis); - m_twistAxisA.setValue(0,0,0); + m_twistAxisA.setValue(0, 0, 0); - m_kSwing = btScalar(1.) / - (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) + - computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB)); + m_kSwing = btScalar(1.) / + (computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldA) + + computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldB)); } } else @@ -717,9 +697,9 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btScalar x = ivB.dot(ivA); btScalar y = ivB.dot(jvA); btScalar z = ivB.dot(kvA); - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { // fixed. We'll need to add one more row to constraint - if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) + if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + { // fixed. We'll need to add one more row to constraint + if ((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) { m_solveSwingLimit = true; m_swingAxis = -ivB.cross(ivA); @@ -727,47 +707,47 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr } else { - if(m_swingSpan1 < m_fixThresh) - { // hinge around Y axis -// if(!(btFuzzyZero(y))) - if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z)))) + if (m_swingSpan1 < m_fixThresh) + { // hinge around Y axis + // if(!(btFuzzyZero(y))) + if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(z)))) { m_solveSwingLimit = true; - if(m_swingSpan2 >= m_fixThresh) + if (m_swingSpan2 >= m_fixThresh) { y = btScalar(0.f); btScalar span2 = btAtan2(z, x); - if(span2 > m_swingSpan2) + if (span2 > m_swingSpan2) { x = btCos(m_swingSpan2); z = btSin(m_swingSpan2); } - else if(span2 < -m_swingSpan2) + else if (span2 < -m_swingSpan2) { - x = btCos(m_swingSpan2); + x = btCos(m_swingSpan2); z = -btSin(m_swingSpan2); } } } } else - { // hinge around Z axis -// if(!btFuzzyZero(z)) - if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y)))) + { // hinge around Z axis + // if(!btFuzzyZero(z)) + if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(y)))) { m_solveSwingLimit = true; - if(m_swingSpan1 >= m_fixThresh) + if (m_swingSpan1 >= m_fixThresh) { z = btScalar(0.f); btScalar span1 = btAtan2(y, x); - if(span1 > m_swingSpan1) + if (span1 > m_swingSpan1) { x = btCos(m_swingSpan1); y = btSin(m_swingSpan1); } - else if(span1 < -m_swingSpan1) + else if (span1 < -m_swingSpan1) { - x = btCos(m_swingSpan1); + x = btCos(m_swingSpan1); y = -btSin(m_swingSpan1); } } @@ -778,10 +758,10 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; target.normalize(); m_swingAxis = -ivB.cross(target); - m_swingCorrection = m_swingAxis.length(); + m_swingCorrection = m_swingAxis.length(); - if (!btFuzzyZero(m_swingCorrection)) - m_swingAxis.normalize(); + if (!btFuzzyZero(m_swingCorrection)) + m_swingAxis.normalize(); } } @@ -790,15 +770,15 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btVector3 twistAxis; computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); - if (m_twistAngle > m_twistSpan*m_limitSoftness) + if (m_twistAngle > m_twistSpan * m_limitSoftness) { m_solveTwistLimit = true; m_twistLimitRatio = 1.f; if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) { - m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ - (m_twistSpan - m_twistSpan * m_limitSoftness); + m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) / + (m_twistSpan - m_twistSpan * m_limitSoftness); } // twist correction tries to get back to soft limit @@ -807,8 +787,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr m_twistAxis = quatRotate(qB, -twistAxis); m_kTwist = btScalar(1.) / - (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) + - computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB)); + (computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldA) + + computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldB)); } if (m_solveSwingLimit) @@ -821,15 +801,13 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr } } - - // given a cone rotation in constraint space, (pre: twist must already be removed) // this method computes its corresponding swing angle and axis. // more interestingly, it computes the cone/swing limit (angle) for this cone "pose". void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, - btScalar& swingAngle, // out - btVector3& vSwingAxis, // out - btScalar& swingLimit) // out + btScalar& swingAngle, // out + btVector3& vSwingAxis, // out + btScalar& swingLimit) // out { swingAngle = qCone.getAngle(); if (swingAngle > SIMD_EPSILON) @@ -840,7 +818,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, // non-zero twist?! this should never happen. btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON)); #endif - + // Compute limit for given swing. tricky: // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) @@ -848,7 +826,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, // For starters, compute the direction from center to surface of ellipse. // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) - btScalar xEllipse = vSwingAxis.y(); + btScalar xEllipse = vSwingAxis.y(); btScalar yEllipse = -vSwingAxis.z(); // Now, we use the slope of the vector (using x/yEllipse) and find the length @@ -858,14 +836,14 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, // a^2 b^2 // Do the math and it should be clear. - swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 + swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 if (fabs(xEllipse) > SIMD_EPSILON) { - btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); + btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse); btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); btScalar swingLimit2 = (1 + surfaceSlope2) / norm; - swingLimit = sqrt(swingLimit2); + swingLimit = std::sqrt(swingLimit2); } // test! @@ -887,7 +865,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, #if 0 btAssert(0); #endif - } + } } btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const @@ -903,34 +881,34 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc // a^2 b^2 // Do the math and it should be clear. - btScalar 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); + btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse); btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); btScalar swingLimit2 = (1 + surfaceSlope2) / norm; - swingLimit = sqrt(swingLimit2); + swingLimit = std::sqrt(swingLimit2); } // convert into point in constraint space: // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively btVector3 vSwingAxis(0, xEllipse, -yEllipse); btQuaternion qSwing(vSwingAxis, swingLimit); - btVector3 vPointInConstraintSpace(fLength,0,0); + btVector3 vPointInConstraintSpace(fLength, 0, 0); return quatRotate(qSwing, vPointInConstraintSpace); } // given a twist rotation in constraint space, (pre: cone must already be removed) // this method computes its corresponding angle and axis. void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, - btScalar& twistAngle, // out - btVector3& vTwistAxis) // out + btScalar& twistAngle, // out + btVector3& vTwistAxis) // out { btQuaternion qMinTwist = qTwist; twistAngle = qTwist.getAngle(); - if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. + if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. { qMinTwist = -(qTwist); twistAngle = qMinTwist.getAngle(); @@ -948,80 +926,79 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, vTwistAxis.normalize(); } - void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const { // the swing axis is computed as the "twist-free" cone rotation, // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). - // so, if we're outside the limits, the closest way back inside the cone isn't + // so, if we're outside the limits, the closest way back inside the cone isn't // along the vector back to the center. better (and more stable) to use the ellipse normal. // convert swing axis to direction from center to surface of ellipse // (ie. rotate 2D vector by PI/2) btScalar y = -vSwingAxis.z(); - btScalar z = vSwingAxis.y(); + btScalar z = vSwingAxis.y(); // do the math... - if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. + if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. { // compute gradient/normal of ellipse surface at current "point" - btScalar grad = y/z; + btScalar grad = y / z; grad *= m_swingSpan2 / m_swingSpan1; // adjust y/z to represent normal at point (instead of vector to point) if (y > 0) - y = fabs(grad * z); + y = fabs(grad * z); else y = -fabs(grad * z); // convert ellipse direction back to swing axis vSwingAxis.setZ(-y); - vSwingAxis.setY( z); + vSwingAxis.setY(z); vSwingAxis.normalize(); } } - - -void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) +void btConeTwistConstraint::setMotorTarget(const btQuaternion& q) { //btTransform trACur = m_rbA.getCenterOfMassTransform(); //btTransform trBCur = m_rbB.getCenterOfMassTransform(); -// btTransform trABCur = trBCur.inverse() * trACur; -// btQuaternion qABCur = trABCur.getRotation(); -// btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); + // btTransform trABCur = trBCur.inverse() * trACur; + // btQuaternion qABCur = trABCur.getRotation(); + // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); //btQuaternion qConstraintCur = trConstraintCur.getRotation(); btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); setMotorTargetInConstraintSpace(qConstraint); } - -void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) +void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion& q) { m_qTarget = q; // clamp motor target to within limits { - btScalar softness = 1.f;//m_limitSoftness; + btScalar softness = 1.f; //m_limitSoftness; // split into twist and cone btVector3 vTwisted = quatRotate(m_qTarget, vTwist); - btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); - btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); + btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); + qTargetCone.normalize(); + btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; + qTargetTwist.normalize(); // clamp cone if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) { - btScalar swingAngle, swingLimit; btVector3 swingAxis; + btScalar swingAngle, swingLimit; + btVector3 swingAxis; computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); if (fabs(swingAngle) > SIMD_EPSILON) { - if (swingAngle > swingLimit*softness) - swingAngle = swingLimit*softness; - else if (swingAngle < -swingLimit*softness) - swingAngle = -swingLimit*softness; + if (swingAngle > swingLimit * softness) + swingAngle = swingLimit * softness; + else if (swingAngle < -swingLimit * softness) + swingAngle = -swingLimit * softness; qTargetCone = btQuaternion(swingAxis, swingAngle); } } @@ -1029,16 +1006,17 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion & // clamp twist if (m_twistSpan >= btScalar(0.05f)) { - btScalar twistAngle; btVector3 twistAxis; + btScalar twistAngle; + btVector3 twistAxis; computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); if (fabs(twistAngle) > SIMD_EPSILON) { // eddy todo: limitSoftness used here??? - if (twistAngle > m_twistSpan*softness) - twistAngle = m_twistSpan*softness; - else if (twistAngle < -m_twistSpan*softness) - twistAngle = -m_twistSpan*softness; + if (twistAngle > m_twistSpan * softness) + twistAngle = m_twistSpan * softness; + else if (twistAngle < -m_twistSpan * softness) + twistAngle = -m_twistSpan * softness; qTargetTwist = btQuaternion(twistAxis, twistAngle); } } @@ -1047,15 +1025,15 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion & } } -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///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 btConeTwistConstraint::setParam(int num, btScalar value, int axis) { - switch(num) + switch (num) { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - if((axis >= 0) && (axis < 3)) + case BT_CONSTRAINT_ERP: + case BT_CONSTRAINT_STOP_ERP: + if ((axis >= 0) && (axis < 3)) { m_linERP = value; m_flags |= BT_CONETWIST_FLAGS_LIN_ERP; @@ -1065,9 +1043,9 @@ void btConeTwistConstraint::setParam(int num, btScalar value, int axis) m_biasFactor = value; } break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - if((axis >= 0) && (axis < 3)) + case BT_CONSTRAINT_CFM: + case BT_CONSTRAINT_STOP_CFM: + if ((axis >= 0) && (axis < 3)) { m_linCFM = value; m_flags |= BT_CONETWIST_FLAGS_LIN_CFM; @@ -1085,19 +1063,19 @@ void btConeTwistConstraint::setParam(int num, btScalar value, int axis) } ///return the local value of parameter -btScalar btConeTwistConstraint::getParam(int num, int axis) const +btScalar btConeTwistConstraint::getParam(int num, int axis) const { btScalar retVal = 0; - switch(num) + switch (num) { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - if((axis >= 0) && (axis < 3)) + case BT_CONSTRAINT_ERP: + case BT_CONSTRAINT_STOP_ERP: + if ((axis >= 0) && (axis < 3)) { btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP); retVal = m_linERP; } - else if((axis >= 3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { retVal = m_biasFactor; } @@ -1106,14 +1084,14 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const btAssertConstrParams(0); } break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - if((axis >= 0) && (axis < 3)) + case BT_CONSTRAINT_CFM: + case BT_CONSTRAINT_STOP_CFM: + if ((axis >= 0) && (axis < 3)) { btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM); retVal = m_linCFM; } - else if((axis >= 3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM); retVal = m_angCFM; @@ -1123,21 +1101,16 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const btAssertConstrParams(0); } break; - default : + default: btAssertConstrParams(0); } return retVal; } - -void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) +void btConeTwistConstraint::setFrames(const btTransform& frameA, const btTransform& frameB) { m_rbAFrame = frameA; m_rbBFrame = frameB; buildJacobian(); //calculateTransforms(); } - - - - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 7a33d01d1..64f44df1c 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -15,8 +15,6 @@ subject to the following restrictions: Written by: Marcus Hennix */ - - /* Overview: @@ -31,8 +29,6 @@ twist is along the x-axis, and swing 1 and 2 are along the z and y axes respectively. */ - - #ifndef BT_CONETWISTCONSTRAINT_H #define BT_CONETWISTCONSTRAINT_H @@ -41,13 +37,12 @@ and swing 1 and 2 are along the z and y axes respectively. #include "btTypedConstraint.h" #ifdef BT_USE_DOUBLE_PRECISION -#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData -#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData" +#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData +#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData" #else -#define btConeTwistConstraintData2 btConeTwistConstraintData -#define btConeTwistConstraintDataName "btConeTwistConstraintData" -#endif //BT_USE_DOUBLE_PRECISION - +#define btConeTwistConstraintData2 btConeTwistConstraintData +#define btConeTwistConstraintDataName "btConeTwistConstraintData" +#endif //BT_USE_DOUBLE_PRECISION class btRigidBody; @@ -59,103 +54,99 @@ enum btConeTwistFlags }; ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc) -ATTRIBUTE_ALIGNED16(class) btConeTwistConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) +btConeTwistConstraint : public btTypedConstraint { #ifdef IN_PARALLELL_SOLVER public: #endif - btJacobianEntry m_jac[3]; //3 orthogonal linear constraints + btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - btTransform m_rbAFrame; + btTransform m_rbAFrame; btTransform m_rbBFrame; - btScalar m_limitSoftness; - btScalar m_biasFactor; - btScalar m_relaxationFactor; + btScalar m_limitSoftness; + btScalar m_biasFactor; + btScalar m_relaxationFactor; - btScalar m_damping; + btScalar m_damping; - btScalar m_swingSpan1; - btScalar m_swingSpan2; - btScalar m_twistSpan; + btScalar m_swingSpan1; + btScalar m_swingSpan2; + btScalar m_twistSpan; - btScalar m_fixThresh; + btScalar m_fixThresh; - btVector3 m_swingAxis; - btVector3 m_twistAxis; + btVector3 m_swingAxis; + btVector3 m_twistAxis; - btScalar m_kSwing; - btScalar m_kTwist; + btScalar m_kSwing; + btScalar m_kTwist; - btScalar m_twistLimitSign; - btScalar m_swingCorrection; - btScalar m_twistCorrection; + btScalar m_twistLimitSign; + btScalar m_swingCorrection; + btScalar m_twistCorrection; - btScalar m_twistAngle; + btScalar m_twistAngle; - btScalar m_accSwingLimitImpulse; - btScalar m_accTwistLimitImpulse; + btScalar m_accSwingLimitImpulse; + btScalar m_accTwistLimitImpulse; - bool m_angularOnly; - bool m_solveTwistLimit; - bool m_solveSwingLimit; + bool m_angularOnly; + bool m_solveTwistLimit; + bool m_solveSwingLimit; - bool m_useSolveConstraintObsolete; + bool m_useSolveConstraintObsolete; // not yet used... - btScalar m_swingLimitRatio; - btScalar m_twistLimitRatio; - btVector3 m_twistAxisA; + btScalar m_swingLimitRatio; + btScalar m_twistLimitRatio; + btVector3 m_twistAxisA; // motor - bool m_bMotorEnabled; - bool m_bNormalizedMotorStrength; + bool m_bMotorEnabled; + bool m_bNormalizedMotorStrength; btQuaternion m_qTarget; - btScalar m_maxMotorImpulse; - btVector3 m_accMotorImpulse; - - // parameters - int m_flags; - btScalar m_linCFM; - btScalar m_linERP; - btScalar m_angCFM; - -protected: + btScalar m_maxMotorImpulse; + btVector3 m_accMotorImpulse; + // parameters + int m_flags; + btScalar m_linCFM; + btScalar m_linERP; + btScalar m_angCFM; + +protected: void init(); - void computeConeLimitInfo(const btQuaternion& qCone, // in - btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs + void computeConeLimitInfo(const btQuaternion& qCone, // in + btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs - void computeTwistLimitInfo(const btQuaternion& qTwist, // in - btScalar& twistAngle, btVector3& vTwistAxis); // all outs - - void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const; + void computeTwistLimitInfo(const btQuaternion& qTwist, // in + btScalar& twistAngle, btVector3& vTwistAxis); // all outs + void adjustSwingAxisToUseEllipseNormal(btVector3 & vSwingAxis) const; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame); - - btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame); + btConeTwistConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame); - virtual void buildJacobian(); + btConeTwistConstraint(btRigidBody & rbA, const btTransform& rbAFrame); - virtual void getInfo1 (btConstraintInfo1* info); + virtual void buildJacobian(); - void getInfo1NonVirtual(btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); + virtual void getInfo1(btConstraintInfo1 * info); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + void getInfo1NonVirtual(btConstraintInfo1 * info); - - void updateRHS(btScalar timeStep); + virtual void getInfo2(btConstraintInfo2 * info); + void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB); + + virtual void solveConstraintObsolete(btSolverBody & bodyA, btSolverBody & bodyB, btScalar timeStep); + + void updateRHS(btScalar timeStep); const btRigidBody& getRigidBodyA() const { @@ -166,64 +157,64 @@ public: return m_rbB; } - void setAngularOnly(bool angularOnly) + void setAngularOnly(bool angularOnly) { m_angularOnly = angularOnly; } - - bool getAngularOnly() const + + bool getAngularOnly() const { - return m_angularOnly; + return m_angularOnly; } - void setLimit(int limitIndex,btScalar limitValue) + void setLimit(int limitIndex, btScalar limitValue) { switch (limitIndex) { - case 3: + case 3: { m_twistSpan = limitValue; break; } - case 4: + case 4: { m_swingSpan2 = limitValue; break; } - case 5: + case 5: { m_swingSpan1 = limitValue; break; } - default: + default: { } }; } - btScalar getLimit(int limitIndex) const + btScalar getLimit(int limitIndex) const { switch (limitIndex) { - case 3: + case 3: { return m_twistSpan; break; } - case 4: + case 4: { return m_swingSpan2; break; } - case 5: + case 5: { return m_swingSpan1; break; } - default: + default: { - btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint"); - return 0.0; + btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint"); + return 0.0; } }; } @@ -239,18 +230,18 @@ public: // __relaxationFactor: // 0->1, recommend to stay near 1. // the lower the value, the less the constraint will fight velocities which violate the angular limits. - void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) + void setLimit(btScalar _swingSpan1, btScalar _swingSpan2, btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { m_swingSpan1 = _swingSpan1; m_swingSpan2 = _swingSpan2; - m_twistSpan = _twistSpan; + m_twistSpan = _twistSpan; - m_limitSoftness = _softness; + m_limitSoftness = _softness; m_biasFactor = _biasFactor; m_relaxationFactor = _relaxationFactor; } - const btTransform& getAFrame() const { return m_rbAFrame; }; + const btTransform& getAFrame() const { return m_rbAFrame; }; const btTransform& getBFrame() const { return m_rbBFrame; }; inline int getSolveTwistLimit() @@ -269,7 +260,7 @@ public: } void calcAngleInfo(); - void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); + void calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB); inline btScalar getSwingSpan1() const { @@ -308,8 +299,16 @@ public: bool isMotorEnabled() const { return m_bMotorEnabled; } btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; } bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; } - void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; } - void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; } + void setMaxMotorImpulse(btScalar maxMotorImpulse) + { + m_maxMotorImpulse = maxMotorImpulse; + m_bNormalizedMotorStrength = false; + } + void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) + { + m_maxMotorImpulse = maxMotorImpulse; + m_bNormalizedMotorStrength = true; + } btScalar getFixThresh() { return m_fixThresh; } void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; } @@ -318,17 +317,17 @@ public: // q: the desired rotation of bodyA wrt bodyB. // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability) // note: don't forget to enableMotor() - void setMotorTarget(const btQuaternion &q); + void setMotorTarget(const btQuaternion& q); const btQuaternion& getMotorTarget() const { return m_qTarget; } // same as above, but q is the desired rotation of frameA wrt frameB in constraint space - void setMotorTargetInConstraintSpace(const btQuaternion &q); + void setMotorTargetInConstraintSpace(const btQuaternion& q); btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const; - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///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 void setParam(int num, btScalar value, int axis = -1); virtual void setFrames(const btTransform& frameA, const btTransform& frameB); @@ -342,84 +341,74 @@ public: return m_rbBFrame; } - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; + virtual btScalar getParam(int num, int axis = -1) const; int getFlags() const { return m_flags; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; - - -struct btConeTwistConstraintDoubleData +struct btConeTwistConstraintDoubleData { - btTypedConstraintDoubleData m_typeConstraintData; + btTypedConstraintDoubleData m_typeConstraintData; btTransformDoubleData m_rbAFrame; btTransformDoubleData m_rbBFrame; //limits - double m_swingSpan1; - double m_swingSpan2; - double m_twistSpan; - double m_limitSoftness; - double m_biasFactor; - double m_relaxationFactor; - - double m_damping; - - + double m_swingSpan1; + double m_swingSpan2; + double m_twistSpan; + double m_limitSoftness; + double m_biasFactor; + double m_relaxationFactor; + double m_damping; }; #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION ///this structure is not used, except for loading pre-2.82 .bullet files -struct btConeTwistConstraintData +struct btConeTwistConstraintData { - btTypedConstraintData m_typeConstraintData; + btTypedConstraintData m_typeConstraintData; btTransformFloatData m_rbAFrame; btTransformFloatData m_rbBFrame; //limits - float m_swingSpan1; - float m_swingSpan2; - float m_twistSpan; - float m_limitSoftness; - float m_biasFactor; - float m_relaxationFactor; + float m_swingSpan1; + float m_swingSpan2; + float m_twistSpan; + float m_limitSoftness; + float m_biasFactor; + float m_relaxationFactor; + + float m_damping; - float m_damping; - char m_pad[4]; - }; -#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION +#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION // -SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const { return sizeof(btConeTwistConstraintData2); - } - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { - btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer; - btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer); + btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*)dataBuffer; + btTypedConstraint::serialize(&cone->m_typeConstraintData, serializer); m_rbAFrame.serialize(cone->m_rbAFrame); m_rbBFrame.serialize(cone->m_rbBFrame); - + cone->m_swingSpan1 = m_swingSpan1; cone->m_swingSpan2 = m_swingSpan2; cone->m_twistSpan = m_twistSpan; @@ -431,5 +420,4 @@ SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, return btConeTwistConstraintDataName; } - -#endif //BT_CONETWISTCONSTRAINT_H +#endif //BT_CONETWISTCONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 890afe6da..68a4a07a1 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -26,40 +26,34 @@ struct btContactSolverInfo; struct btBroadphaseProxy; class btIDebugDraw; class btStackAlloc; -class btDispatcher; +class btDispatcher; /// btConstraintSolver provides solver interface - enum btConstraintSolverType { - BT_SEQUENTIAL_IMPULSE_SOLVER=1, - BT_MLCP_SOLVER=2, - BT_NNCG_SOLVER=4 + BT_SEQUENTIAL_IMPULSE_SOLVER = 1, + BT_MLCP_SOLVER = 2, + BT_NNCG_SOLVER = 4, + BT_MULTIBODY_SOLVER = 8, + BT_BLOCK_SOLVER = 16, }; class btConstraintSolver { - public: - virtual ~btConstraintSolver() {} - - virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;} + + virtual void prepareSolve(int /* numBodies */, int /* numManifolds */) { ; } ///solve a group of constraints - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer,btDispatcher* dispatcher) = 0; + virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher) = 0; - virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */) {;} + virtual void allSolved(const btContactSolverInfo& /* info */, class btIDebugDraw* /* debugDrawer */) { ; } ///clear internal cached data and reset random seed - virtual void reset() = 0; - - virtual btConstraintSolverType getSolverType() const=0; - + virtual void reset() = 0; + virtual btConstraintSolverType getSolverType() const = 0; }; - - - -#endif //BT_CONSTRAINT_SOLVER_H +#endif //BT_CONSTRAINT_SOLVER_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 1098d0c96..4b22b2fff 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btContactConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btVector3.h" @@ -22,44 +21,33 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" - - -btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB) -:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB), - m_contactManifold(*contactManifold) +btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold, btRigidBody& rbA, btRigidBody& rbB) + : btTypedConstraint(CONTACT_CONSTRAINT_TYPE, rbA, rbB), + m_contactManifold(*contactManifold) { - } btContactConstraint::~btContactConstraint() { - } -void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold) +void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold) { m_contactManifold = *contactManifold; } -void btContactConstraint::getInfo1 (btConstraintInfo1* info) +void btContactConstraint::getInfo1(btConstraintInfo1* info) { - } -void btContactConstraint::getInfo2 (btConstraintInfo2* info) +void btContactConstraint::getInfo2(btConstraintInfo2* info) { - } -void btContactConstraint::buildJacobian() +void btContactConstraint::buildJacobian() { - } - - - - #include "btContactConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btVector3.h" @@ -68,64 +56,59 @@ void btContactConstraint::buildJacobian() #include "LinearMath/btMinMax.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" - - //response between two dynamic objects without friction and no restitution, assuming 0 penetration depth btScalar resolveSingleCollision( - btRigidBody* body1, - btCollisionObject* colObj2, - const btVector3& contactPositionWorld, - const btVector3& contactNormalOnB, - const btContactSolverInfo& solverInfo, - btScalar distance) + btRigidBody* body1, + btCollisionObject* colObj2, + const btVector3& contactPositionWorld, + const btVector3& contactNormalOnB, + const btContactSolverInfo& solverInfo, + btScalar distance) { btRigidBody* body2 = btRigidBody::upcast(colObj2); - - - const btVector3& normal = contactNormalOnB; - btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin(); - btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin(); - - btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; - rel_vel = normal.dot(vel); - - btScalar combinedRestitution = 0.f; - btScalar restitution = combinedRestitution* -rel_vel; + const btVector3& normal = contactNormalOnB; - btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ; - btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping; - btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal); - btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f; + btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin(); + btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin(); + + btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2 ? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = normal.dot(vel); + + btScalar combinedRestitution = 0.f; + btScalar restitution = combinedRestitution * -rel_vel; + + btScalar positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep; + btScalar velocityError = -(1.0f + restitution) * rel_vel; // * damping; + btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld, normal); + btScalar denom1 = body2 ? body2->computeImpulseDenominator(contactPositionWorld, normal) : 0.f; btScalar relaxation = 1.f; - btScalar jacDiagABInv = relaxation/(denom0+denom1); + btScalar jacDiagABInv = relaxation / (denom0 + denom1); - btScalar penetrationImpulse = positionalError * jacDiagABInv; - btScalar velocityImpulse = velocityError * jacDiagABInv; + btScalar penetrationImpulse = positionalError * jacDiagABInv; + btScalar velocityImpulse = velocityError * jacDiagABInv; - btScalar normalImpulse = penetrationImpulse+velocityImpulse; - normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse; + btScalar normalImpulse = penetrationImpulse + velocityImpulse; + normalImpulse = 0.f > normalImpulse ? 0.f : normalImpulse; - body1->applyImpulse(normal*(normalImpulse), rel_pos1); - if (body2) - body2->applyImpulse(-normal*(normalImpulse), rel_pos2); - - return normalImpulse; + body1->applyImpulse(normal * (normalImpulse), rel_pos1); + if (body2) + body2->applyImpulse(-normal * (normalImpulse), rel_pos2); + + return normalImpulse; } - //bilateral constraint between two dynamic objects void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, - btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep) + btRigidBody& body2, const btVector3& pos2, + btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep) { (void)timeStep; (void)distance; - btScalar normalLenSqr = normal.length2(); btAssert(btFabs(normalLenSqr) < btScalar(1.1)); if (normalLenSqr > btScalar(1.1)) @@ -133,45 +116,38 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, impulse = btScalar(0.); return; } - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); + btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations - + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; - - btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), - body2.getCenterOfMassTransform().getBasis().transpose(), - rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), - body2.getInvInertiaDiagLocal(),body2.getInvMass()); + btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), + body2.getCenterOfMassTransform().getBasis().transpose(), + rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(), body1.getInvMass(), + body2.getInvInertiaDiagLocal(), body2.getInvMass()); btScalar jacDiagAB = jac.getDiagonal(); btScalar jacDiagABInv = btScalar(1.) / jacDiagAB; - - btScalar rel_vel = jac.getRelativeVelocity( + + btScalar rel_vel = jac.getRelativeVelocity( body1.getLinearVelocity(), body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), body2.getLinearVelocity(), - body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); - - + body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); rel_vel = normal.dot(vel); - + //todo: move this into proper structure btScalar contactDamping = btScalar(0.2); #ifdef ONLY_USE_LINEAR_MASS btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass()); - impulse = - contactDamping * rel_vel * massTerm; -#else + impulse = -contactDamping * rel_vel * massTerm; +#else btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; #endif } - - - - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index adb226835..255489be9 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -22,20 +22,17 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" ///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface -ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) +btContactConstraint : public btTypedConstraint { protected: - btPersistentManifold m_contactManifold; protected: - - - btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB); + btContactConstraint(btPersistentManifold * contactManifold, btRigidBody & rbA, btRigidBody & rbB); public: - - void setContactManifold(btPersistentManifold* contactManifold); + void setContactManifold(btPersistentManifold * contactManifold); btPersistentManifold* getContactManifold() { @@ -49,25 +46,20 @@ public: virtual ~btContactConstraint(); - virtual void getInfo1 (btConstraintInfo1* info); + virtual void getInfo1(btConstraintInfo1 * info); - virtual void getInfo2 (btConstraintInfo2* info); + virtual void getInfo2(btConstraintInfo2 * info); ///obsolete methods - virtual void buildJacobian(); - - + virtual void buildJacobian(); }; ///very basic collision resolution without friction -btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld,const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo,btScalar distance); - +btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld, const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo, btScalar distance); ///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, - btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep); + btRigidBody& body2, const btVector3& pos2, + btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep); - - -#endif //BT_CONTACT_CONSTRAINT_H +#endif //BT_CONTACT_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 28d0c1dd4..63d7c98e1 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -18,7 +18,7 @@ subject to the following restrictions: #include "LinearMath/btScalar.h" -enum btSolverMode +enum btSolverMode { SOLVER_RANDMIZE_ORDER = 1, SOLVER_FRICTION_SEPARATE = 2, @@ -29,139 +29,137 @@ 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 { - + btScalar m_tau; + btScalar m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. + btScalar m_friction; + btScalar m_timeStep; + btScalar m_restitution; + int m_numIterations; + btScalar m_maxErrorReduction; + 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 - btScalar m_tau; - btScalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. - btScalar m_friction; - btScalar m_timeStep; - btScalar m_restitution; - int m_numIterations; - btScalar m_maxErrorReduction; - 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; - btScalar m_linearSlop; - btScalar m_warmstartingFactor; - - int m_solverMode; - int m_restingContactRestitutionThreshold; - int m_minimumSolverBatchSize; - btScalar m_maxGyroscopicForce; - btScalar m_singleAxisRollingFrictionThreshold; - btScalar m_leastSquaresResidualThreshold; - btScalar m_restitutionVelocityThreshold; + int m_splitImpulse; + btScalar m_splitImpulsePenetrationThreshold; + btScalar m_splitImpulseTurnErp; + btScalar m_linearSlop; + btScalar m_warmstartingFactor; + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + btScalar m_maxGyroscopicForce; + btScalar m_singleAxisRollingFrictionThreshold; + btScalar m_leastSquaresResidualThreshold; + btScalar m_restitutionVelocityThreshold; + bool m_jointFeedbackInWorldSpace; + bool m_jointFeedbackInJointFrame; + int m_reportSolverAnalytics; }; struct btContactSolverInfo : public btContactSolverInfoData { - - - inline btContactSolverInfo() { m_tau = btScalar(0.6); m_damping = btScalar(1.0); m_friction = btScalar(0.3); - m_timeStep = btScalar(1.f/60.f); + m_timeStep = btScalar(1.f / 60.f); m_restitution = btScalar(0.); m_maxErrorReduction = btScalar(20.); m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); m_globalCfm = btScalar(0.); - m_frictionERP = btScalar(0.2);//positional friction 'anchors' are disabled by default + 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; m_splitImpulseTurnErp = 0.1f; m_linearSlop = btScalar(0.0); - m_warmstartingFactor=btScalar(0.85); + m_warmstartingFactor = btScalar(0.85); //m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER; - m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | 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; ///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_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD; // | 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; ///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 + m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution + m_jointFeedbackInWorldSpace = false; + m_jointFeedbackInJointFrame = false; + m_reportSolverAnalytics = 0; } }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btContactSolverInfoDoubleData { - 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;///it is only used for 'explicit' version of gyroscopic force - double m_singleAxisRollingFrictionThreshold; - - int m_numIterations; - int m_solverMode; - int m_restingContactRestitutionThreshold; - int m_minimumSolverBatchSize; - int m_splitImpulse; - char m_padding[4]; + 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; ///it is only used for 'explicit' version of gyroscopic force + 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 btContactSolverInfoFloatData { - 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_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_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_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; + 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_numIterations; + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; - int m_splitImpulse; - char m_padding[4]; + int m_splitImpulse; + char m_padding[4]; }; - - -#endif //BT_CONTACT_SOLVER_INFO +#endif //BT_CONTACT_SOLVER_INFO diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp index 75d81cc08..bba102d90 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp @@ -13,25 +13,20 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btFixedConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" #include - -btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) -:btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB) +btFixedConstraint::btFixedConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) + : btGeneric6DofSpring2Constraint(rbA, rbB, frameInA, frameInB) { - setAngularLowerLimit(btVector3(0,0,0)); - setAngularUpperLimit(btVector3(0,0,0)); - setLinearLowerLimit(btVector3(0,0,0)); - setLinearUpperLimit(btVector3(0,0,0)); + setAngularLowerLimit(btVector3(0, 0, 0)); + setAngularUpperLimit(btVector3(0, 0, 0)); + setLinearLowerLimit(btVector3(0, 0, 0)); + setLinearUpperLimit(btVector3(0, 0, 0)); } - - - -btFixedConstraint::~btFixedConstraint () +btFixedConstraint::~btFixedConstraint() { } diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h index bff2008b2..6d474ea81 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h @@ -18,16 +18,13 @@ subject to the following restrictions: #include "btGeneric6DofSpring2Constraint.h" - -ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint +ATTRIBUTE_ALIGNED16(class) +btFixedConstraint : public btGeneric6DofSpring2Constraint { - public: - btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); + btFixedConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB); - virtual ~btFixedConstraint(); - }; -#endif //BT_FIXED_CONSTRAINT_H +#endif //BT_FIXED_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp index bcd457b67..7535c52c0 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp @@ -17,38 +17,36 @@ subject to the following restrictions: #include "btGearConstraint.h" -btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio) -:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB), -m_axisInA(axisInA), -m_axisInB(axisInB), -m_ratio(ratio) +btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA, const btVector3& axisInB, btScalar ratio) + : btTypedConstraint(GEAR_CONSTRAINT_TYPE, rbA, rbB), + m_axisInA(axisInA), + m_axisInB(axisInB), + m_ratio(ratio) { } -btGearConstraint::~btGearConstraint () +btGearConstraint::~btGearConstraint() { } -void btGearConstraint::getInfo1 (btConstraintInfo1* info) +void btGearConstraint::getInfo1(btConstraintInfo1* info) { info->m_numConstraintRows = 1; info->nub = 1; } -void btGearConstraint::getInfo2 (btConstraintInfo2* info) +void btGearConstraint::getInfo2(btConstraintInfo2* info) { btVector3 globalAxisA, globalAxisB; - globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA; - globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB; + globalAxisA = m_rbA.getWorldTransform().getBasis() * this->m_axisInA; + globalAxisB = m_rbB.getWorldTransform().getBasis() * this->m_axisInB; info->m_J1angularAxis[0] = globalAxisA[0]; info->m_J1angularAxis[1] = globalAxisA[1]; info->m_J1angularAxis[2] = globalAxisA[2]; - info->m_J2angularAxis[0] = m_ratio*globalAxisB[0]; - info->m_J2angularAxis[1] = m_ratio*globalAxisB[1]; - info->m_J2angularAxis[2] = m_ratio*globalAxisB[2]; - + info->m_J2angularAxis[0] = m_ratio * globalAxisB[0]; + info->m_J2angularAxis[1] = m_ratio * globalAxisB[1]; + info->m_J2angularAxis[2] = m_ratio * globalAxisB[2]; } - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h index e4613455a..64b15dfbc 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h @@ -13,45 +13,40 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_GEAR_CONSTRAINT_H #define BT_GEAR_CONSTRAINT_H #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" - #ifdef BT_USE_DOUBLE_PRECISION -#define btGearConstraintData btGearConstraintDoubleData -#define btGearConstraintDataName "btGearConstraintDoubleData" +#define btGearConstraintData btGearConstraintDoubleData +#define btGearConstraintDataName "btGearConstraintDoubleData" #else -#define btGearConstraintData btGearConstraintFloatData -#define btGearConstraintDataName "btGearConstraintFloatData" -#endif //BT_USE_DOUBLE_PRECISION - - +#define btGearConstraintData btGearConstraintFloatData +#define btGearConstraintDataName "btGearConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION ///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio. ///See Bullet/Demos/ConstraintDemo for an example use. class btGearConstraint : public btTypedConstraint { protected: - btVector3 m_axisInA; - btVector3 m_axisInB; - bool m_useFrameA; - btScalar m_ratio; + btVector3 m_axisInA; + btVector3 m_axisInB; + bool m_useFrameA; + btScalar m_ratio; public: - btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f); - virtual ~btGearConstraint (); + btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA, const btVector3& axisInB, btScalar ratio = 1.f); + virtual ~btGearConstraint(); ///internal method used by the constraint solver, don't use them directly - virtual void getInfo1 (btConstraintInfo1* info); + virtual void getInfo1(btConstraintInfo1* info); ///internal method used by the constraint solver, don't use them directly - virtual void getInfo2 (btConstraintInfo2* info); + virtual void getInfo2(btConstraintInfo2* info); - void setAxisA(btVector3& axisA) + void setAxisA(btVector3& axisA) { m_axisInA = axisA; } @@ -76,68 +71,64 @@ public: return m_ratio; } - - virtual void setParam(int num, btScalar value, int axis = -1) + virtual void setParam(int num, btScalar value, int axis = -1) { - (void) num; - (void) value; - (void) axis; + (void)num; + (void)value; + (void)axis; btAssert(0); } ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const - { - (void) num; - (void) axis; + virtual btScalar getParam(int num, int axis = -1) const + { + (void)num; + (void)axis; btAssert(0); return 0.f; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; - - - ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btGearConstraintFloatData { - btTypedConstraintFloatData m_typeConstraintData; + btTypedConstraintFloatData m_typeConstraintData; - btVector3FloatData m_axisInA; - btVector3FloatData m_axisInB; + btVector3FloatData m_axisInA; + btVector3FloatData m_axisInB; - float m_ratio; - char m_padding[4]; + float m_ratio; + char m_padding[4]; }; struct btGearConstraintDoubleData { - btTypedConstraintDoubleData m_typeConstraintData; + btTypedConstraintDoubleData m_typeConstraintData; - btVector3DoubleData m_axisInA; - btVector3DoubleData m_axisInB; + btVector3DoubleData m_axisInA; + btVector3DoubleData m_axisInB; - double m_ratio; + double m_ratio; }; -SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const { return sizeof(btGearConstraintData); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { btGearConstraintData* gear = (btGearConstraintData*)dataBuffer; - btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer); + btTypedConstraint::serialize(&gear->m_typeConstraintData, serializer); - m_axisInA.serialize( gear->m_axisInA ); - m_axisInB.serialize( gear->m_axisInB ); + m_axisInA.serialize(gear->m_axisInA); + m_axisInB.serialize(gear->m_axisInB); gear->m_ratio = m_ratio; @@ -152,9 +143,4 @@ SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSe return btGearConstraintDataName; } - - - - - -#endif //BT_GEAR_CONSTRAINT_H +#endif //BT_GEAR_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index fa17254ec..1f5420353 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -25,83 +25,61 @@ http://gimpact.sf.net #include "LinearMath/btTransformUtil.h" #include - - #define D6_USE_OBSOLETE_METHOD false #define D6_USE_FRAME_OFFSET true - - - - - btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) -: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) -, m_frameInA(frameInA) -, m_frameInB(frameInB), -m_useLinearReferenceFrameA(useLinearReferenceFrameA), -m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), -m_flags(0), -m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) + : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0), m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) { calculateTransforms(); } - - btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) - : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameB), - m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), - m_flags(0), - m_useSolveConstraintObsolete(false) + : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameB), + m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), + m_flags(0), + m_useSolveConstraintObsolete(false) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; calculateTransforms(); } - - - #define GENERIC_D6_DISABLE_WARMSTARTING 1 - - btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) { - int i = index%3; - int j = index/3; + int i = index % 3; + int j = index / 3; return mat[i][j]; } - - ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html -bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); -bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) +bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz); +bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz) { // // rot = cy*cz -cy*sz sy // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy // - btScalar fi = btGetMatrixElem(mat,2); + 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)); + 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[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4)); xyz[1] = -SIMD_HALF_PI; xyz[2] = btScalar(0.0); return false; @@ -110,7 +88,7 @@ bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) else { // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4)); xyz[1] = SIMD_HALF_PI; xyz[2] = 0.0; } @@ -121,52 +99,49 @@ bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) int btRotationalLimitMotor::testLimitValue(btScalar test_value) { - if(m_loLimit>m_hiLimit) + if (m_loLimit > m_hiLimit) { - m_currentLimit = 0;//Free from violation + 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>SIMD_PI) - m_currentLimitError-=SIMD_2_PI; - else if(m_currentLimitError<-SIMD_PI) - m_currentLimitError+=SIMD_2_PI; + m_currentLimit = 1; //low limit violation + m_currentLimitError = test_value - m_loLimit; + if (m_currentLimitError > SIMD_PI) + m_currentLimitError -= SIMD_2_PI; + else if (m_currentLimitError < -SIMD_PI) + m_currentLimitError += SIMD_2_PI; return 1; } - else if (test_value> m_hiLimit) + else if (test_value > m_hiLimit) { - m_currentLimit = 2;//High limit violation + m_currentLimit = 2; //High limit violation m_currentLimitError = test_value - m_hiLimit; - if(m_currentLimitError>SIMD_PI) - m_currentLimitError-=SIMD_2_PI; - else if(m_currentLimitError<-SIMD_PI) - m_currentLimitError+=SIMD_2_PI; + if (m_currentLimitError > SIMD_PI) + m_currentLimitError -= SIMD_2_PI; + else if (m_currentLimitError < -SIMD_PI) + m_currentLimitError += SIMD_2_PI; return 2; }; - m_currentLimit = 0;//Free from violation + m_currentLimit = 0; //Free from violation return 0; - } - - btScalar btRotationalLimitMotor::solveAngularLimits( - btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, - btRigidBody * body0, btRigidBody * body1 ) + btScalar timeStep, btVector3& axis, btScalar jacDiagABInv, + btRigidBody* body0, btRigidBody* body1) { - if (needApplyTorques()==false) return 0.0f; + if (needApplyTorques() == false) return 0.0f; btScalar target_velocity = m_targetVelocity; btScalar maxMotorForce = m_maxMotorForce; //current error correction - if (m_currentLimit!=0) + if (m_currentLimit != 0) { - target_velocity = -m_stopERP*m_currentLimitError/(timeStep); + target_velocity = -m_stopERP * m_currentLimitError / (timeStep); maxMotorForce = m_maxLimitForce; } @@ -178,42 +153,37 @@ btScalar btRotationalLimitMotor::solveAngularLimits( btVector3 angVelB = body1->getAngularVelocity(); btVector3 vel_diff; - vel_diff = angVelA-angVelB; - - + vel_diff = angVelA - angVelB; btScalar rel_vel = axis.dot(vel_diff); // correction velocity - btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); + btScalar motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel); - - if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) + if (motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON) { - return 0.0f;//no need for applying force + return 0.0f; //no need for applying force } - // correction impulse - btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; + btScalar unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv; // clip correction impulse btScalar clippedMotorImpulse; ///@todo: should clip against accumulated impulse - if (unclippedMotorImpulse>0.0f) + if (unclippedMotorImpulse > 0.0f) { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse; } else { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse; } - // sort with accumulated impulses - btScalar lo = btScalar(-BT_LARGE_FLOAT); - btScalar hi = btScalar(BT_LARGE_FLOAT); + btScalar lo = btScalar(-BT_LARGE_FLOAT); + btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar oldaccumImpulse = m_accumulatedImpulse; btScalar sum = oldaccumImpulse + clippedMotorImpulse; @@ -227,59 +197,50 @@ btScalar btRotationalLimitMotor::solveAngularLimits( body1->applyTorqueImpulse(-motorImp); return clippedMotorImpulse; - - } //////////////////////////// End btRotationalLimitMotor //////////////////////////////////// - - - //////////////////////////// btTranslationalLimitMotor //////////////////////////////////// - int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value) { btScalar loLimit = m_lowerLimit[limitIndex]; btScalar hiLimit = m_upperLimit[limitIndex]; - if(loLimit > hiLimit) + if (loLimit > hiLimit) { - m_currentLimit[limitIndex] = 0;//Free from violation + m_currentLimit[limitIndex] = 0; //Free from violation m_currentLimitError[limitIndex] = btScalar(0.f); return 0; } if (test_value < loLimit) { - m_currentLimit[limitIndex] = 2;//low limit violation - m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimit[limitIndex] = 2; //low limit violation + m_currentLimitError[limitIndex] = test_value - loLimit; return 2; } - else if (test_value> hiLimit) + else if (test_value > hiLimit) { - m_currentLimit[limitIndex] = 1;//High limit violation + m_currentLimit[limitIndex] = 1; //High limit violation m_currentLimitError[limitIndex] = test_value - hiLimit; return 1; }; - m_currentLimit[limitIndex] = 0;//Free from violation + m_currentLimit[limitIndex] = 0; //Free from violation m_currentLimitError[limitIndex] = btScalar(0.f); return 0; } - - btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, + btRigidBody& body1, const btVector3& pointInA, + btRigidBody& body2, const btVector3& pointInB, int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos) + const btVector3& axis_normal_on_a, + const btVector3& anchorPos) { - ///find relative velocity // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); @@ -292,14 +253,12 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar rel_vel = axis_normal_on_a.dot(vel); - - /// apply displacement correction //positional error (zeroth order error) btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); - btScalar lo = btScalar(-BT_LARGE_FLOAT); - btScalar hi = btScalar(BT_LARGE_FLOAT); + btScalar lo = btScalar(-BT_LARGE_FLOAT); + btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar minLimit = m_lowerLimit[limit_index]; btScalar maxLimit = m_upperLimit[limit_index]; @@ -312,7 +271,6 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( { depth -= maxLimit; lo = btScalar(0.); - } else { @@ -329,10 +287,7 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( } } - btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; - - - + btScalar normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv; btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; btScalar sum = oldNormalImpulse + normalImpulse; @@ -340,11 +295,9 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; btVector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1.applyImpulse( impulse_vector, rel_pos1); + body1.applyImpulse(impulse_vector, rel_pos1); body2.applyImpulse(-impulse_vector, rel_pos2); - - return normalImpulse; } @@ -352,8 +305,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( void btGeneric6DofConstraint::calculateAngleInfo() { - btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); - matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); + btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis(); + matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff); // in euler angle mode we do not actually constrain the angular velocity // along the axes axis[0] and axis[2] (although we do use axis[1]) : // @@ -378,31 +331,30 @@ void btGeneric6DofConstraint::calculateAngleInfo() m_calculatedAxis[0].normalize(); m_calculatedAxis[1].normalize(); m_calculatedAxis[2].normalize(); - } void btGeneric6DofConstraint::calculateTransforms() { - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); } -void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) +void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA, const btTransform& transB) { m_calculatedTransformA = transA * m_frameInA; m_calculatedTransformB = transB * m_frameInB; calculateLinearInfo(); calculateAngleInfo(); - if(m_useOffsetForConstraintFrame) - { // get weight factors depending on masses + if (m_useOffsetForConstraintFrame) + { // get weight factors depending on masses 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)) + if (miS > btScalar(0.f)) { m_factA = miB / miS; } - else + else { m_factA = btScalar(0.5f); } @@ -410,39 +362,32 @@ void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,cons } } - - void btGeneric6DofConstraint::buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW) + btJacobianEntry& jacLinear, const btVector3& normalWorld, + const btVector3& pivotAInW, const btVector3& pivotBInW) { new (&jacLinear) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normalWorld, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normalWorld, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); } - - void btGeneric6DofConstraint::buildAngularJacobian( - btJacobianEntry & jacAngular,const btVector3 & jointAxisW) + btJacobianEntry& jacAngular, const btVector3& jointAxisW) { - new (&jacAngular) btJacobianEntry(jointAxisW, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - + new (&jacAngular) btJacobianEntry(jointAxisW, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); } - - bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { btScalar angle = m_calculatedAxisAngleDiff[axis_index]; @@ -453,23 +398,20 @@ bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) return m_angularLimits[axis_index].needApplyTorques(); } - - void btGeneric6DofConstraint::buildJacobian() { #ifndef __SPU__ if (m_useSolveConstraintObsolete) { - // Clear accumulated impulses for the next simulation step m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); int i; - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); } //calculates transform - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); @@ -483,7 +425,7 @@ void btGeneric6DofConstraint::buildJacobian() btVector3 normalWorld; //linear part - for (i=0;i<3;i++) + for (i = 0; i < 3; i++) { if (m_linearLimits.isLimited(i)) { @@ -493,56 +435,53 @@ void btGeneric6DofConstraint::buildJacobian() normalWorld = m_calculatedTransformB.getBasis().getColumn(i); buildLinearJacobian( - m_jacLinear[i],normalWorld , - pivotAInW,pivotBInW); - + m_jacLinear[i], normalWorld, + pivotAInW, pivotBInW); } } // angular part - for (i=0;i<3;i++) + for (i = 0; i < 3; i++) { //calculates error angle if (testAngularLimitMotor(i)) { normalWorld = this->getAxis(i); // Create angular atom - buildAngularJacobian(m_jacAng[i],normalWorld); + buildAngularJacobian(m_jacAng[i], normalWorld); } } - } -#endif //__SPU__ - +#endif //__SPU__ } - -void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) +void btGeneric6DofConstraint::getInfo1(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; info->nub = 0; - } else + } + else { //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); info->m_numConstraintRows = 0; info->nub = 6; int i; //test linear limits - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { - if(m_linearLimits.needApplyForce(i)) + if (m_linearLimits.needApplyForce(i)) { info->m_numConstraintRows++; info->nub--; } } //test angular limits - for (i=0;i<3 ;i++ ) + for (i = 0; i < 3; i++) { - if(testAngularLimitMotor(i)) + if (testAngularLimitMotor(i)) { info->m_numConstraintRows++; info->nub--; @@ -551,13 +490,14 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) } } -void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info) +void btGeneric6DofConstraint::getInfo1NonVirtual(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; info->nub = 0; - } else + } + else { //pre-allocate all 6 info->m_numConstraintRows = 6; @@ -565,8 +505,7 @@ void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info) } } - -void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) +void btGeneric6DofConstraint::getInfo2(btConstraintInfo2* info) { btAssert(!m_useSolveConstraintObsolete); @@ -577,136 +516,124 @@ void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) const btVector3& angVelA = m_rbA.getAngularVelocity(); const btVector3& angVelB = m_rbB.getAngularVelocity(); - 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); + 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); + { // 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 btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +void btGeneric6DofConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB) { - btAssert(!m_useSolveConstraintObsolete); //prepare constraint - calculateTransforms(transA,transB); + calculateTransforms(transA, transB); int i; - for (i=0;i<3 ;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); + 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); + { // 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 btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB) { -// int row = 0; + // int row = 0; //solve linear limits btRotationalLimitMotor limot; - for (int i=0;i<3 ;i++ ) + for (int i = 0; i < 3; i++) { - if(m_linearLimits.needApplyForce(i)) - { // re-use rotational motor code + if (m_linearLimits.needApplyForce(i)) + { // re-use rotational motor code limot.m_bounce = btScalar(0.f); limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; - limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; - limot.m_damping = m_linearLimits.m_damping; - limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; - limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; - limot.m_limitSoftness = m_linearLimits.m_limitSoftness; - limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; - limot.m_maxLimitForce = btScalar(0.f); - limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; - limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; + 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 = btScalar(0.f); + limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; + limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT); - limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0]; - limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; - limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp; - if(m_useOffsetForConstraintFrame) + limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0]; + limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; + limot.m_stopERP = (flags & BT_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) + 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); + 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); + row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0); } } } return row; } - - -int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2* info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB) { - btGeneric6DofConstraint * d6constraint = this; + btGeneric6DofConstraint* d6constraint = this; int row = row_offset; //solve angular limits - for (int i=0;i<3 ;i++ ) + for (int i = 0; i < 3; i++) { - if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) + if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) { btVector3 axis = d6constraint->getAxis(i); int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT); - if(!(flags & BT_6DOF_FLAGS_CFM_NORM)) + if (!(flags & BT_6DOF_FLAGS_CFM_NORM)) { m_angularLimits[i].m_normalCFM = info->cfm[0]; } - if(!(flags & BT_6DOF_FLAGS_CFM_STOP)) + if (!(flags & BT_6DOF_FLAGS_CFM_STOP)) { m_angularLimits[i].m_stopCFM = info->cfm[0]; } - if(!(flags & BT_6DOF_FLAGS_ERP_STOP)) + if (!(flags & BT_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); + transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1); } } return row; } - - - -void btGeneric6DofConstraint::updateRHS(btScalar timeStep) +void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { (void)timeStep; - } - void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB) { m_frameInA = frameA; @@ -715,33 +642,27 @@ void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTrans calculateTransforms(); } - - btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; } - -btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const +btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const { return m_calculatedLinearDiff[axisIndex]; } - btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const { return m_calculatedAxisAngleDiff[axisIndex]; } - - void btGeneric6DofConstraint::calcAnchorPos(void) { btScalar imA = m_rbA.getInvMass(); btScalar imB = m_rbB.getInvMass(); btScalar weight; - if(imB == btScalar(0.0)) + if (imB == btScalar(0.0)) { weight = btScalar(1.0); } @@ -755,43 +676,39 @@ void btGeneric6DofConstraint::calcAnchorPos(void) return; } - - void btGeneric6DofConstraint::calculateLinearInfo() { m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); } } - - int btGeneric6DofConstraint::get_limit_motor_info2( - btRotationalLimitMotor * limot, - const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed) + btRotationalLimitMotor* limot, + const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB, + btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int 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 - 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]; + 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 + 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]; + J2[srow + 0] = -ax1[0]; + J2[srow + 1] = -ax1[1]; + J2[srow + 2] = -ax1[2]; - if((!rotational)) - { + if ((!rotational)) + { if (m_useOffsetForConstraintFrame) { btVector3 tmpA, tmpB, relA, relB; @@ -814,55 +731,56 @@ int btGeneric6DofConstraint::get_limit_motor_info2( relB = orthoB - totalDist * m_factB; tmpA = relA.cross(ax1); tmpB = relB.cross(ax1); - if(m_hasStaticBody && (!rotAllowed)) + 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 + 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 { - btVector3 ltd; // Linear Torque Decoupling vector + btVector3 ltd; // Linear Torque Decoupling vector btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin(); ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = ltd[0]; - info->m_J1angularAxis[srow+1] = ltd[1]; - info->m_J1angularAxis[srow+2] = ltd[2]; + 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]; + 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] = btScalar(0.f); - if (powered) - { + } + // 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] = btScalar(0.f); + if (powered) + { info->cfm[srow] = limot->m_normalCFM; - if(!limit) - { + if (!limit) + { btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; - btScalar mot_fact = getMotorFactor( limot->m_currentPosition, - limot->m_loLimit, - limot->m_hiLimit, - tag_vel, - info->fps * limot->m_stopERP); + btScalar 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->m_upperLimit[srow] = limot->m_maxMotorForce; - } - } - if(limit) - { - btScalar k = info->fps * limot->m_stopERP; - if(!rotational) + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; + } + } + if (limit) + { + btScalar k = info->fps * limot->m_stopERP; + if (!rotational) { info->m_constraintError[srow] += k * limot->m_currentLimitError; } @@ -871,116 +789,112 @@ int btGeneric6DofConstraint::get_limit_motor_info2( 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] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - if (limit == 1) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - // deal with bounce - if (limot->m_bounce > 0) - { - // calculate joint velocity - btScalar vel; - if (rotational) - { - vel = angVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= angVelB.dot(ax1); - } - else - { - vel = linVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= linVelB.dot(ax1); - } - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) - { - if (vel < 0) - { - btScalar newc = -limot->m_bounce* vel; - if (newc > info->m_constraintError[srow]) + if (limot->m_loLimit == limot->m_hiLimit) + { // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { + if (limit == 1) + { + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + // deal with bounce + if (limot->m_bounce > 0) + { + // calculate joint velocity + btScalar vel; + if (rotational) + { + vel = angVelA.dot(ax1); + //make sure that if no body -> angVelB == zero vec + // if (body1) + vel -= angVelB.dot(ax1); + } + else + { + vel = linVelA.dot(ax1); + //make sure that if no body -> angVelB == zero vec + // if (body1) + vel -= linVelB.dot(ax1); + } + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if (limit == 1) + { + if (vel < 0) + { + btScalar newc = -limot->m_bounce * vel; + if (newc > info->m_constraintError[srow]) info->m_constraintError[srow] = newc; - } - } - else - { - if (vel > 0) - { - btScalar newc = -limot->m_bounce * vel; - if (newc < info->m_constraintError[srow]) + } + } + else + { + if (vel > 0) + { + btScalar newc = -limot->m_bounce * vel; + if (newc < info->m_constraintError[srow]) info->m_constraintError[srow] = newc; - } - } - } - } - } - return 1; - } - else return 0; + } + } + } + } + } + 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. +///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 btGeneric6DofConstraint::setParam(int num, btScalar value, int axis) { - if((axis >= 0) && (axis < 3)) + if ((axis >= 0) && (axis < 3)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + case BT_CONSTRAINT_STOP_ERP: m_linearLimits.m_stopERP[axis] = value; m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: m_linearLimits.m_stopCFM[axis] = value; m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: m_linearLimits.m_normalCFM[axis] = value; m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - default : + default: btAssertConstrParams(0); } } - else if((axis >=3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + case BT_CONSTRAINT_STOP_ERP: m_angularLimits[axis - 3].m_stopERP = value; m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: m_angularLimits[axis - 3].m_stopCFM = value; m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: m_angularLimits[axis - 3].m_normalCFM = value; m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); break; - default : + default: btAssertConstrParams(0); } } @@ -990,47 +904,47 @@ void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis) } } - ///return the local value of parameter -btScalar btGeneric6DofConstraint::getParam(int num, int axis) const +///return the local value of parameter +btScalar btGeneric6DofConstraint::getParam(int num, int axis) const { btScalar retVal = 0; - if((axis >= 0) && (axis < 3)) + if ((axis >= 0) && (axis < 3)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + case BT_CONSTRAINT_STOP_ERP: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_linearLimits.m_stopERP[axis]; break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_linearLimits.m_stopCFM[axis]; break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_linearLimits.m_normalCFM[axis]; break; - default : + default: btAssertConstrParams(0); } } - else if((axis >=3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + case BT_CONSTRAINT_STOP_ERP: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_angularLimits[axis - 3].m_stopERP; break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_angularLimits[axis - 3].m_stopCFM; break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_angularLimits[axis - 3].m_normalCFM; break; - default : + default: btAssertConstrParams(0); } } @@ -1041,23 +955,21 @@ btScalar btGeneric6DofConstraint::getParam(int num, int axis) const return retVal; } - - -void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2) +void btGeneric6DofConstraint::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 - + 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]); - + 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(); } diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index bea8629c3..b9e762e17 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -23,7 +23,6 @@ email: projectileman@yahoo.com http://gimpact.sf.net */ - #ifndef BT_GENERIC_6DOF_CONSTRAINT_H #define BT_GENERIC_6DOF_CONSTRAINT_H @@ -33,96 +32,91 @@ http://gimpact.sf.net class btRigidBody; - - #ifdef BT_USE_DOUBLE_PRECISION -#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2 -#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2" +#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2 +#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2" #else -#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData -#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData" -#endif //BT_USE_DOUBLE_PRECISION - +#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData +#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData" +#endif //BT_USE_DOUBLE_PRECISION //! Rotation Limit structure for generic joints class btRotationalLimitMotor { public: - //! limit_parameters - //!@{ - btScalar m_loLimit;//!< joint limit - btScalar m_hiLimit;//!< joint limit - btScalar m_targetVelocity;//!< target motor velocity - btScalar m_maxMotorForce;//!< max force on motor - btScalar m_maxLimitForce;//!< max force on limit - btScalar m_damping;//!< Damping. - btScalar m_limitSoftness;//! Relaxation factor - btScalar m_normalCFM;//!< Constraint force mixing factor - btScalar m_stopERP;//!< Error tolerance factor when joint is at limit - btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit - btScalar m_bounce;//!< restitution factor - bool m_enableMotor; + //! limit_parameters + //!@{ + btScalar m_loLimit; //!< joint limit + btScalar m_hiLimit; //!< joint limit + btScalar m_targetVelocity; //!< target motor velocity + btScalar m_maxMotorForce; //!< max force on motor + btScalar m_maxLimitForce; //!< max force on limit + btScalar m_damping; //!< Damping. + btScalar m_limitSoftness; //! Relaxation factor + btScalar m_normalCFM; //!< Constraint force mixing factor + btScalar m_stopERP; //!< Error tolerance factor when joint is at limit + btScalar m_stopCFM; //!< Constraint force mixing factor when joint is at limit + btScalar m_bounce; //!< restitution factor + bool m_enableMotor; - //!@} + //!@} - //! temp_variables - //!@{ - btScalar m_currentLimitError;//! How much is violated this limit - btScalar m_currentPosition; //! current value of angle - int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit - btScalar m_accumulatedImpulse; - //!@} + //! temp_variables + //!@{ + btScalar m_currentLimitError; //! How much is violated this limit + btScalar m_currentPosition; //! current value of angle + int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit + btScalar m_accumulatedImpulse; + //!@} - btRotationalLimitMotor() - { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 0.1f; - m_maxLimitForce = 300.0f; - m_loLimit = 1.0f; - m_hiLimit = -1.0f; + btRotationalLimitMotor() + { + 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; - } + m_bounce = 0.0f; + m_damping = 1.0f; + m_limitSoftness = 0.5f; + m_currentLimit = 0; + m_currentLimitError = 0; + m_enableMotor = false; + } - btRotationalLimitMotor(const btRotationalLimitMotor & 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; + btRotationalLimitMotor(const btRotationalLimitMotor& 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; - } - - + 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() const - { - if(m_loLimit > m_hiLimit) return false; - return true; - } + bool isLimited() const + { + if (m_loLimit > m_hiLimit) return false; + return true; + } //! Need apply correction - bool needApplyTorques() const - { - if(m_currentLimit == 0 && m_enableMotor == false) return false; - return true; - } + bool needApplyTorques() const + { + if (m_currentLimit == 0 && m_enableMotor == false) return false; + return true; + } //! calculates error /*! @@ -131,104 +125,98 @@ public: int testLimitValue(btScalar test_value); //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); - + btScalar solveAngularLimits(btScalar timeStep, btVector3& axis, btScalar jacDiagABInv, btRigidBody* body0, btRigidBody* body1); }; - - class btTranslationalLimitMotor { public: - btVector3 m_lowerLimit;//!< the constraint lower limits - btVector3 m_upperLimit;//!< the constraint upper limits - btVector3 m_accumulatedImpulse; - //! Linear_Limit_parameters - //!@{ - btScalar m_limitSoftness;//!< Softness for linear limit - btScalar m_damping;//!< Damping for linear limit - btScalar m_restitution;//! Bounce parameter for linear limit - btVector3 m_normalCFM;//!< Constraint force mixing factor - btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit - btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit - //!@} - bool m_enableMotor[3]; - btVector3 m_targetVelocity;//!< target motor velocity - btVector3 m_maxMotorForce;//!< max force on motor - btVector3 m_currentLimitError;//! How much is violated this limit - btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames - int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit + btVector3 m_lowerLimit; //!< the constraint lower limits + btVector3 m_upperLimit; //!< the constraint upper limits + btVector3 m_accumulatedImpulse; + //! Linear_Limit_parameters + //!@{ + btScalar m_limitSoftness; //!< Softness for linear limit + btScalar m_damping; //!< Damping for linear limit + btScalar m_restitution; //! Bounce parameter for linear limit + btVector3 m_normalCFM; //!< Constraint force mixing factor + btVector3 m_stopERP; //!< Error tolerance factor when joint is at limit + btVector3 m_stopCFM; //!< Constraint force mixing factor when joint is at limit + //!@} + bool m_enableMotor[3]; + btVector3 m_targetVelocity; //!< target motor velocity + btVector3 m_maxMotorForce; //!< max force on motor + btVector3 m_currentLimitError; //! How much is violated this limit + btVector3 m_currentLinearDiff; //! Current relative offset of constraint frames + int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit - btTranslationalLimitMotor() - { - 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); + btTranslationalLimitMotor() + { + 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 = btScalar(1.0f); - m_restitution = btScalar(0.5f); - for(int i=0; i < 3; i++) + m_limitSoftness = 0.7f; + m_damping = btScalar(1.0f); + m_restitution = btScalar(0.5f); + for (int i = 0; i < 3; i++) { m_enableMotor[i] = false; m_targetVelocity[i] = btScalar(0.f); m_maxMotorForce[i] = btScalar(0.f); } - } + } - btTranslationalLimitMotor(const btTranslationalLimitMotor & other ) - { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; + btTranslationalLimitMotor(const btTranslationalLimitMotor& 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_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++) + 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 + //! 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) const - { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - inline bool needApplyForce(int limitIndex) const - { - if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; - return true; - } + inline bool isLimited(int limitIndex) const + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + inline bool needApplyForce(int limitIndex) const + { + if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; + return true; + } int testLimitValue(int limitIndex, btScalar test_value); - - btScalar solveLinearAxis( - btScalar timeStep, - btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, - int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos); - - + btScalar solveLinearAxis( + btScalar timeStep, + btScalar jacDiagABInv, + btRigidBody& body1, const btVector3& pointInA, + btRigidBody& body2, const btVector3& pointInB, + int limit_index, + const btVector3& axis_normal_on_a, + const btVector3& anchorPos); }; enum bt6DofFlags @@ -237,8 +225,7 @@ enum bt6DofFlags BT_6DOF_FLAGS_CFM_STOP = 2, BT_6DOF_FLAGS_ERP_STOP = 4 }; -#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis - +#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space /*! @@ -276,254 +263,245 @@ This brings support for limit parameters and motors.
    • */ -ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) +btGeneric6DofConstraint : public btTypedConstraint { protected: - //! relative_frames - //!@{ - btTransform m_frameInA;//!< the constraint space w.r.t body A - btTransform m_frameInB;//!< the constraint space w.r.t body B - //!@} - - //! Jacobians - //!@{ - btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints - btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints - //!@} - - //! Linear_Limit_parameters - //!@{ - btTranslationalLimitMotor m_linearLimits; - //!@} - - - //! hinge_parameters - //!@{ - btRotationalLimitMotor m_angularLimits[3]; + //!@{ + btTransform m_frameInA; //!< the constraint space w.r.t body A + btTransform m_frameInB; //!< the constraint space w.r.t body B //!@} + //! Jacobians + //!@{ + btJacobianEntry m_jacLinear[3]; //!< 3 orthogonal linear constraints + btJacobianEntry m_jacAng[3]; //!< 3 orthogonal angular constraints + //!@} + + //! Linear_Limit_parameters + //!@{ + btTranslationalLimitMotor m_linearLimits; + //!@} + + //! hinge_parameters + //!@{ + btRotationalLimitMotor m_angularLimits[3]; + //!@} protected: - //! temporal variables - //!@{ - btScalar m_timeStep; - 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; - - btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes + //! temporal variables + //!@{ + btScalar m_timeStep; + 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; - bool m_useLinearReferenceFrameA; - bool m_useOffsetForConstraintFrame; - - int m_flags; + btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes - //!@} + bool m_useLinearReferenceFrameA; + bool m_useOffsetForConstraintFrame; - btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) - { - btAssert(0); - (void) other; - return *this; - } + int m_flags; + //!@} - 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); + btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) + { + btAssert(0); + (void)other; + return *this; + } - int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); + 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); - void buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW); + 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 buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW); + void buildLinearJacobian( + btJacobianEntry & jacLinear, const btVector3& normalWorld, + const btVector3& pivotAInW, const btVector3& pivotBInW); + + void buildAngularJacobian(btJacobianEntry & jacAngular, const btVector3& jointAxisW); // tests linear limits void calculateLinearInfo(); //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - - + void calculateAngleInfo(); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; - btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); - + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; + + btGeneric6DofConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA); + btGeneric6DofConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); + //! 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 btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo */ - void calculateTransforms(const btTransform& transA,const btTransform& transB); + void calculateTransforms(const btTransform& transA, const btTransform& transB); void calculateTransforms(); //! Gets the global transform of the offset for body A - /*! + /*! \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. */ - const btTransform & getCalculatedTransformA() const - { - return m_calculatedTransformA; - } + const btTransform& getCalculatedTransformA() const + { + return m_calculatedTransformA; + } - //! Gets the global transform of the offset for body B - /*! + //! Gets the global transform of the offset for body B + /*! \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. */ - const btTransform & getCalculatedTransformB() const - { - return m_calculatedTransformB; - } + const btTransform& getCalculatedTransformB() const + { + return m_calculatedTransformB; + } - const btTransform & getFrameOffsetA() const - { - return m_frameInA; - } + const btTransform& getFrameOffsetA() const + { + return m_frameInA; + } - const btTransform & getFrameOffsetB() const - { - return m_frameInB; - } + const btTransform& getFrameOffsetB() const + { + return m_frameInB; + } + btTransform& getFrameOffsetA() + { + return m_frameInA; + } - btTransform & getFrameOffsetA() - { - return m_frameInA; - } - - btTransform & getFrameOffsetB() - { - return m_frameInB; - } - + btTransform& getFrameOffsetB() + { + return m_frameInB; + } //! performs Jacobian calculation, and also calculates angle differences and axis - virtual void buildJacobian(); + virtual void buildJacobian(); - virtual void getInfo1 (btConstraintInfo1* info); + virtual void getInfo1(btConstraintInfo1 * info); - void getInfo1NonVirtual (btConstraintInfo1* info); + void getInfo1NonVirtual(btConstraintInfo1 * info); - virtual void getInfo2 (btConstraintInfo2* info); + virtual void getInfo2(btConstraintInfo2 * info); - void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); + void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB); - - void updateRHS(btScalar timeStep); + void updateRHS(btScalar timeStep); //! Get the rotation axis in global coordinates /*! \pre btGeneric6DofConstraint.buildJacobian must be called previously. */ - btVector3 getAxis(int axis_index) const; + btVector3 getAxis(int axis_index) const; - //! Get the relative Euler angle - /*! + //! Get the relative Euler angle + /*! \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. */ - btScalar getAngle(int axis_index) const; + btScalar getAngle(int axis_index) const; //! Get the relative position of the constraint pivot - /*! + /*! \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. */ btScalar getRelativePivotPosition(int axis_index) const; - void setFrames(const btTransform & frameA, const btTransform & frameB); + void setFrames(const btTransform& frameA, const btTransform& frameB); //! Test angular limit. /*! Calculates angular correction and returns true if limit needs to be corrected. \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. */ - bool testAngularLimitMotor(int axis_index); + bool testAngularLimitMotor(int axis_index); - void setLinearLowerLimit(const btVector3& linearLower) - { - m_linearLimits.m_lowerLimit = linearLower; - } + void setLinearLowerLimit(const btVector3& linearLower) + { + m_linearLimits.m_lowerLimit = linearLower; + } - void getLinearLowerLimit(btVector3& linearLower) const + void getLinearLowerLimit(btVector3 & linearLower) const { linearLower = m_linearLimits.m_lowerLimit; } - void setLinearUpperLimit(const btVector3& linearUpper) + void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; } - void getLinearUpperLimit(btVector3& linearUpper) const + void getLinearUpperLimit(btVector3 & linearUpper) const { 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 getAngularLowerLimit(btVector3& angularLower) const + void setAngularLowerLimit(const btVector3& angularLower) { - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); + } + + void getAngularLowerLimit(btVector3 & angularLower) const + { + for (int i = 0; i < 3; i++) angularLower[i] = m_angularLimits[i].m_loLimit; } - void setAngularUpperLimit(const btVector3& angularUpper) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); - } - - void getAngularUpperLimit(btVector3& angularUpper) const + void setAngularUpperLimit(const btVector3& angularUpper) { - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); + } + + void getAngularUpperLimit(btVector3 & angularUpper) const + { + for (int i = 0; i < 3; i++) angularUpper[i] = m_angularLimits[i].m_hiLimit; } //! Retrieves the angular limit informacion - btRotationalLimitMotor * getRotationalLimitMotor(int index) - { - return &m_angularLimits[index]; - } + btRotationalLimitMotor* getRotationalLimitMotor(int index) + { + return &m_angularLimits[index]; + } - //! Retrieves the limit informacion - btTranslationalLimitMotor * getTranslationalLimitMotor() - { - return &m_linearLimits; - } + //! Retrieves the limit informacion + btTranslationalLimitMotor* getTranslationalLimitMotor() + { + return &m_linearLimits; + } - //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 - { + //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; - } - } + m_angularLimits[axis - 3].m_loLimit = lo; + m_angularLimits[axis - 3].m_hiLimit = hi; + } + } //! Test limit /*! @@ -532,116 +510,106 @@ public: - limited means upper > lower - limitIndex: first 3 are linear, next 3 are angular */ - bool isLimited(int limitIndex) const - { - if(limitIndex<3) - { + bool isLimited(int limitIndex) const + { + if (limitIndex < 3) + { return m_linearLimits.isLimited(limitIndex); + } + return m_angularLimits[limitIndex - 3].isLimited(); + } - } - return m_angularLimits[limitIndex-3].isLimited(); - } + virtual void calcAnchorPos(void); // overridable - virtual void calcAnchorPos(void); // overridable - - int get_limit_motor_info2( btRotationalLimitMotor * limot, - const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false); + int get_limit_motor_info2(btRotationalLimitMotor * limot, + const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB, + btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false); // access for UseFrameOffset bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; } void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } - + bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; } void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; } - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///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 void setParam(int num, btScalar value, int axis = -1); ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; + virtual btScalar getParam(int num, int axis = -1) const; - void setAxis( const btVector3& axis1, const btVector3& axis2); + void setAxis(const btVector3& axis1, const btVector3& axis2); - virtual int getFlags() const - { - return m_flags; + virtual int getFlags() const + { + return m_flags; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; - struct btGeneric6DofConstraintData { - btTypedConstraintData m_typeConstraintData; - btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTypedConstraintData m_typeConstraintData; + btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. btTransformFloatData m_rbBFrame; - - btVector3FloatData m_linearUpperLimit; - btVector3FloatData m_linearLowerLimit; - btVector3FloatData m_angularUpperLimit; - btVector3FloatData m_angularLowerLimit; - - int m_useLinearReferenceFrameA; + btVector3FloatData m_linearUpperLimit; + btVector3FloatData m_linearLowerLimit; + + btVector3FloatData m_angularUpperLimit; + btVector3FloatData m_angularLowerLimit; + + int m_useLinearReferenceFrameA; int m_useOffsetForConstraintFrame; }; struct btGeneric6DofConstraintDoubleData2 { - btTypedConstraintDoubleData m_typeConstraintData; - btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTypedConstraintDoubleData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. btTransformDoubleData m_rbBFrame; - - btVector3DoubleData m_linearUpperLimit; - btVector3DoubleData m_linearLowerLimit; - btVector3DoubleData m_angularUpperLimit; - btVector3DoubleData m_angularLowerLimit; - - int m_useLinearReferenceFrameA; + btVector3DoubleData m_linearUpperLimit; + btVector3DoubleData m_linearLowerLimit; + + btVector3DoubleData m_angularUpperLimit; + btVector3DoubleData m_angularLowerLimit; + + int m_useLinearReferenceFrameA; int m_useOffsetForConstraintFrame; }; -SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const { return sizeof(btGeneric6DofConstraintData2); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { - btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer; - btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); + 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++) + 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_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit; + dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit; dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i]; dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i]; } - - dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0; + + dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA ? 1 : 0; dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0; return btGeneric6DofConstraintDataName; } - - - - -#endif //BT_GENERIC_6DOF_CONSTRAINT_H +#endif //BT_GENERIC_6DOF_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp index f0976ee49..93626f18f 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -37,67 +37,55 @@ email: projectileman@yahoo.com http://gimpact.sf.net */ - - #include "btGeneric6DofSpring2Constraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" +#include #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) + : 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) + : 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; + 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) +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); + 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)); + 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[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4)); xyz[1] = -SIMD_HALF_PI; xyz[2] = btScalar(0.0); return false; @@ -106,32 +94,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btV else { // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + 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) +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); + 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)); + 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[0] = -btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8)); xyz[1] = btScalar(0.0); xyz[2] = SIMD_HALF_PI; return false; @@ -139,33 +127,33 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btV } else { - xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + 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) +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); + 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)); + 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[1] = -btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0)); xyz[2] = btScalar(0.0); return false; } @@ -173,32 +161,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btV else { xyz[0] = -SIMD_HALF_PI; - xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[1] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0)); xyz[2] = 0.0; } return false; } -bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz) +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); + 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)); + 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[1] = -btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8)); xyz[2] = -SIMD_HALF_PI; return false; } @@ -206,33 +194,33 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btV else { xyz[0] = btScalar(0.0); - xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); + xyz[1] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8)); xyz[2] = SIMD_HALF_PI; } return false; } -bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz) +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); + 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)); + 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)); + xyz[2] = -btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0)); return false; } } @@ -240,32 +228,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btV { xyz[0] = SIMD_HALF_PI; xyz[1] = btScalar(0.0); - xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + xyz[2] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0)); } return false; } -bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz) +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); + 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)); + 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)); + xyz[2] = -btAtan2(btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 2)); return false; } } @@ -273,23 +261,36 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btV { xyz[0] = btScalar(0.0); xyz[1] = -SIMD_HALF_PI; - xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2)); + xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), -btGetMatrixElem(mat, 2)); } return false; } void btGeneric6DofSpring2Constraint::calculateAngleInfo() { - btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); + 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); + 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]) : @@ -307,14 +308,14 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo() // to the components of w and set that to 0. switch (m_rotateOrder) { - case RO_XYZ : + 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 + //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) + //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) + // y' = N = axis2.cross(axis0) // z' = z // // x" = X @@ -324,7 +325,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo() //first rotate around z //second rotate around y'= z.cross(X) //third rotate around x" = X - //Original XYZ extrinsic rotation order. + //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); @@ -333,7 +334,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo() m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); break; } - case RO_XZY : + case RO_XZY: { //planes: xz,ZY normals: y, X //first rotate around y @@ -346,7 +347,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo() m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); break; } - case RO_YXZ : + case RO_YXZ: { //planes: yx,XZ normals: z, Y //first rotate around z @@ -359,7 +360,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo() m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); break; } - case RO_YZX : + case RO_YZX: { //planes: yz,ZX normals: x, Y //first rotate around x @@ -372,7 +373,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo() m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); break; } - case RO_ZXY : + case RO_ZXY: { //planes: zx,XY normals: y, Z //first rotate around y @@ -385,7 +386,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo() m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); break; } - case RO_ZYX : + case RO_ZYX: { //planes: zy,YX normals: x, Z //first rotate around x @@ -398,22 +399,21 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo() m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); break; } - default: - btAssert(false); + 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()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); } -void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB) +void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA, const btTransform& transB) { m_calculatedTransformA = transA * m_frameInA; m_calculatedTransformB = transB * m_frameInB; @@ -424,18 +424,17 @@ void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& tran btScalar miB = getRigidBodyB().getInvMass(); m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); btScalar miS = miA + miB; - if(miS > btScalar(0.f)) + if (miS > btScalar(0.f)) { m_factA = miB / miS; } - else + 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]; @@ -444,35 +443,37 @@ void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index) m_angularLimits[axis_index].testLimitValue(angle); } - -void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info) +void btGeneric6DofSpring2Constraint::getInfo1(btConstraintInfo1* info) { //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + 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++) + 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_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++ ) + 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_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) +void btGeneric6DofSpring2Constraint::getInfo2(btConstraintInfo2* info) { const btTransform& transA = m_rbA.getCenterOfMassTransform(); const btTransform& transB = m_rbB.getCenterOfMassTransform(); @@ -482,118 +483,138 @@ void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info) 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 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) +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++ ) + 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]; + 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]; + 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_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 + 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 ) ); + 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 ) + 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); - + 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 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) + 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); + 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++ ) + 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) + 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)) + if (!(flags & BT_6DOF_FLAGS_CFM_STOP2)) { m_angularLimits[i].m_stopCFM = info->cfm[0]; } - if(!(flags & BT_6DOF_FLAGS_ERP_STOP2)) + if (!(flags & BT_6DOF_FLAGS_ERP_STOP2)) { m_angularLimits[i].m_stopERP = info->erp; } - if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2)) + if (!(flags & BT_6DOF_FLAGS_CFM_MOTO2)) { m_angularLimits[i].m_motorCFM = info->cfm[0]; } - if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2)) + 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); + 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; @@ -602,32 +623,31 @@ void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const 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++) + 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) +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; + 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]; + 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]; + J2[srow + 0] = -ax1[0]; + J2[srow + 1] = -ax1[1]; + J2[srow + 2] = -ax1[2]; - if(!rotational) + if (!rotational) { btVector3 tmpA, tmpB, relA, relB; // get vector from bodyB to frameB in WCS @@ -636,40 +656,44 @@ void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * l relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); tmpA = relA.cross(ax1); tmpB = relB.cross(ax1); - if(m_hasStaticBody && (!rotAllowed)) + 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]; + 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) + 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) + 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); + 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 (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; + } + 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; } } @@ -679,16 +703,21 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( srow += info->rowskip; ++count; - calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + 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 (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; + } + 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; } } @@ -697,10 +726,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( info->cfm[srow] = limot->m_stopCFM; srow += info->rowskip; ++count; - } else - if (limot->m_currentLimit==3) + } + else if (limot->m_currentLimit == 3) { - calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + 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; @@ -711,16 +740,16 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( if (limot->m_enableMotor && !limot->m_servoMotor) { - calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + 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); + 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->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; @@ -735,42 +764,42 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( if (error > SIMD_PI) { error -= SIMD_2_PI; - curServoTarget +=SIMD_2_PI; + curServoTarget += SIMD_2_PI; } if (error < -SIMD_PI) { error += SIMD_2_PI; - curServoTarget -=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; + 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) + if (error != 0) { btScalar lowLimit; btScalar hiLimit; - if(limot->m_loLimit > limot->m_hiLimit) + if (limot->m_loLimit > limot->m_hiLimit) { lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY; - hiLimit = 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; + lowLimit = error > 0 && curServoTarget > limot->m_loLimit ? curServoTarget : limot->m_loLimit; + hiLimit = error < 0 && curServoTarget < limot->m_hiLimit ? curServoTarget : limot->m_hiLimit; } mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP); - } - else + } + 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->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; @@ -779,7 +808,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( if (limot->m_enableSpring) { btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint; - calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + 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) @@ -792,34 +821,66 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( 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 vel; + if (rotational) + { + vel = angVelA.dot(ax1) - angVelB.dot(ax1); + } + else + { + btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin()); + btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin()); + vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1); + } btScalar cfm = BT_ZERO; btScalar mA = BT_ONE / m_rbA.getInvMass(); btScalar mB = BT_ONE / m_rbB.getInvMass(); - btScalar m = mA > mB ? mB : mA; - btScalar angularfreq = sqrt(ks / m); - + 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; + if (m_rbA.getInvMass() == 0) m = mB; else + if (m_rbB.getInvMass() == 0) m = mA; else + m = mA*mB / (mA + mB); + btScalar angularfreq = btSqrt(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) + 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) + 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); + btScalar f = (fs + fd); - info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ; + // 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 than 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) + // (Even with our best intent the "new" velocity is only an estimation. If we underestimate + // the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't + // matter, because the solver will limit it according the force limit) + // 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 answer 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) + if (!rotational) { info->m_lowerLimit[srow] = minf > 0 ? 0 : minf; info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf; @@ -838,56 +899,55 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( return count; } - -//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +//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)) + if ((axis >= 0) && (axis < 3)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + 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 : + 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 : + 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 : + 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 : + default: btAssertConstrParams(0); } } - else if((axis >=3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + 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 : + 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 : + 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 : + 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 : + default: btAssertConstrParams(0); } } @@ -898,54 +958,54 @@ void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis) } //return the local value of parameter -btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const +btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const { btScalar retVal = 0; - if((axis >= 0) && (axis < 3)) + if ((axis >= 0) && (axis < 3)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + 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 : + 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 : + 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 : + 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 : + default: btAssertConstrParams(0); } } - else if((axis >=3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : + 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 : + 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 : + 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 : + 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 : + default: btAssertConstrParams(0); } } @@ -956,31 +1016,29 @@ btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const return retVal; } - - -void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2) +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 - + 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]); - + 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) + if (index < 3) m_linearLimits.m_bounce[index] = bounce; else m_angularLimits[index - 3].m_bounce = bounce; @@ -989,7 +1047,7 @@ void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce) void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff) { btAssert((index >= 0) && (index < 6)); - if (index<3) + if (index < 3) m_linearLimits.m_enableMotor[index] = onOff; else m_angularLimits[index - 3].m_enableMotor = onOff; @@ -998,7 +1056,7 @@ void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff) void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff) { btAssert((index >= 0) && (index < 6)); - if (index<3) + if (index < 3) m_linearLimits.m_servoMotor[index] = onOff; else m_angularLimits[index - 3].m_servoMotor = onOff; @@ -1007,18 +1065,16 @@ void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff) void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity) { btAssert((index >= 0) && (index < 6)); - if (index<3) + 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) + if (index < 3) { m_linearLimits.m_servoTarget[index] = targetOrg; } @@ -1027,23 +1083,24 @@ void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOr //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; + btScalar target = targetOrg + SIMD_PI; if (1) { - btScalar m = target - SIMD_2_PI * floor(target/SIMD_2_PI); + btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI); // handle boundary cases resulted from floating-point cut off: { - if (m>=SIMD_2_PI) + if (m >= SIMD_2_PI) { target = 0; - } else + } + else { - if (m<0 ) + if (m < 0) { - if (SIMD_2_PI+m == SIMD_2_PI) + if (SIMD_2_PI + m == SIMD_2_PI) target = 0; else - target = SIMD_2_PI+m; + target = SIMD_2_PI + m; } else { @@ -1053,7 +1110,7 @@ void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOr } target -= SIMD_PI; } - + m_angularLimits[index - 3].m_servoTarget = target; } } @@ -1061,7 +1118,7 @@ void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOr void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force) { btAssert((index >= 0) && (index < 6)); - if (index<3) + if (index < 3) m_linearLimits.m_maxMotorForce[index] = force; else m_angularLimits[index - 3].m_maxMotorForce = force; @@ -1070,19 +1127,22 @@ void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force) void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff) { btAssert((index >= 0) && (index < 6)); - if (index<3) + if (index < 3) m_linearLimits.m_enableSpring[index] = onOff; else - m_angularLimits[index - 3] .m_enableSpring = onOff; + m_angularLimits[index - 3].m_enableSpring = onOff; } void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded) { btAssert((index >= 0) && (index < 6)); - if (index<3) { + if (index < 3) + { m_linearLimits.m_springStiffness[index] = stiffness; m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded; - } else { + } + else + { m_angularLimits[index - 3].m_springStiffness = stiffness; m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded; } @@ -1091,10 +1151,13 @@ void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded) { btAssert((index >= 0) && (index < 6)); - if (index<3) { + if (index < 3) + { m_linearLimits.m_springDamping[index] = damping; m_linearLimits.m_springDampingLimited[index] = limitIfNeeded; - } else { + } + else + { m_angularLimits[index - 3].m_springDamping = damping; m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded; } @@ -1104,9 +1167,9 @@ void btGeneric6DofSpring2Constraint::setEquilibriumPoint() { calculateTransforms(); int i; - for( i = 0; i < 3; i++) + for (i = 0; i < 3; i++) m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i]; } @@ -1114,35 +1177,38 @@ void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index) { btAssert((index >= 0) && (index < 6)); calculateTransforms(); - if (index<3) + if (index < 3) m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; else - m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3]; + 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) + if (index < 3) m_linearLimits.m_equilibriumPoint[index] = val; else - m_angularLimits[index - 3] .m_equilibriumPoint = val; + 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) { + if (m_loLimit > m_hiLimit) + { m_currentLimit = 0; m_currentLimitError = btScalar(0.f); } - else if(m_loLimit == m_hiLimit) { + else if (m_loLimit == m_hiLimit) + { m_currentLimitError = test_value - m_loLimit; m_currentLimit = 3; - } else { + } + else + { m_currentLimitError = test_value - m_loLimit; m_currentLimitErrorHi = test_value - m_hiLimit; m_currentLimit = 4; @@ -1155,18 +1221,20 @@ void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_va { btScalar loLimit = m_lowerLimit[limitIndex]; btScalar hiLimit = m_upperLimit[limitIndex]; - if(loLimit > hiLimit) { + if (loLimit > hiLimit) + { m_currentLimitError[limitIndex] = 0; m_currentLimit[limitIndex] = 0; } - else if(loLimit == hiLimit) { + else if (loLimit == hiLimit) + { m_currentLimitError[limitIndex] = test_value - loLimit; m_currentLimit[limitIndex] = 3; - } else { + } + else + { m_currentLimitError[limitIndex] = test_value - loLimit; m_currentLimitErrorHi[limitIndex] = test_value - hiLimit; m_currentLimit[limitIndex] = 4; } } - - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h index 66d176958..00e24364e 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h @@ -37,7 +37,6 @@ email: projectileman@yahoo.com http://gimpact.sf.net */ - #ifndef BT_GENERIC_6DOF_CONSTRAINT2_H #define BT_GENERIC_6DOF_CONSTRAINT2_H @@ -47,18 +46,17 @@ http://gimpact.sf.net class btRigidBody; - #ifdef BT_USE_DOUBLE_PRECISION -#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2 -#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2" +#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2 +#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2" #else -#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData -#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData" -#endif //BT_USE_DOUBLE_PRECISION +#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData +#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData" +#endif //BT_USE_DOUBLE_PRECISION enum RotateOrder { - RO_XYZ=0, + RO_XYZ = 0, RO_XZY, RO_YXZ, RO_YZX, @@ -69,9 +67,9 @@ enum RotateOrder class btRotationalLimitMotor2 { public: -// upper < lower means free -// upper == lower means locked -// upper > lower means limited + // upper < lower means free + // upper == lower means locked + // upper > lower means limited btScalar m_loLimit; btScalar m_hiLimit; btScalar m_bounce; @@ -79,95 +77,92 @@ public: btScalar m_stopCFM; btScalar m_motorERP; btScalar m_motorCFM; - bool m_enableMotor; + bool m_enableMotor; btScalar m_targetVelocity; btScalar m_maxMotorForce; - bool m_servoMotor; + bool m_servoMotor; btScalar m_servoTarget; - bool m_enableSpring; + bool m_enableSpring; btScalar m_springStiffness; - bool m_springStiffnessLimited; + bool m_springStiffnessLimited; btScalar m_springDamping; - bool m_springDampingLimited; + bool m_springDampingLimited; btScalar m_equilibriumPoint; btScalar m_currentLimitError; btScalar m_currentLimitErrorHi; btScalar m_currentPosition; - int m_currentLimit; + 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_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_springDamping = 0; + m_springDampingLimited = false; + m_equilibriumPoint = 0; - m_currentLimitError = 0; + m_currentLimitError = 0; m_currentLimitErrorHi = 0; - m_currentPosition = 0; - m_currentLimit = 0; + m_currentPosition = 0; + m_currentLimit = 0; } - btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot) + 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_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_springDamping = limot.m_springDamping; + m_springDampingLimited = limot.m_springDampingLimited; + m_equilibriumPoint = limot.m_equilibriumPoint; - m_currentLimitError = limot.m_currentLimitError; + m_currentLimitError = limot.m_currentLimitError; m_currentLimitErrorHi = limot.m_currentLimitErrorHi; - m_currentPosition = limot.m_currentPosition; - m_currentLimit = limot.m_currentLimit; + m_currentPosition = limot.m_currentPosition; + m_currentLimit = limot.m_currentLimit; } - bool isLimited() { - if(m_loLimit > m_hiLimit) return false; + 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 + // upper < lower means free + // upper == lower means locked + // upper > lower means limited btVector3 m_lowerLimit; btVector3 m_upperLimit; btVector3 m_bounce; @@ -175,14 +170,14 @@ public: btVector3 m_stopCFM; btVector3 m_motorERP; btVector3 m_motorCFM; - bool m_enableMotor[3]; - bool m_servoMotor[3]; - bool m_enableSpring[3]; + bool m_enableMotor[3]; + bool m_servoMotor[3]; + bool m_enableSpring[3]; btVector3 m_servoTarget; btVector3 m_springStiffness; - bool m_springStiffnessLimited[3]; + bool m_springStiffnessLimited[3]; btVector3 m_springDamping; - bool m_springDampingLimited[3]; + bool m_springDampingLimited[3]; btVector3 m_equilibriumPoint; btVector3 m_targetVelocity; btVector3 m_maxMotorForce; @@ -190,69 +185,69 @@ public: btVector3 m_currentLimitError; btVector3 m_currentLimitErrorHi; btVector3 m_currentLinearDiff; - int m_currentLimit[3]; + 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_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 ); + 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++) + 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_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; + 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 ) + 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_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; + m_currentLinearDiff = other.m_currentLinearDiff; - for(int i=0; i < 3; i++) + 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_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_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]; + m_currentLimit[i] = other.m_currentLimit[i]; } } @@ -269,15 +264,14 @@ 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 + BT_6DOF_FLAGS_ERP_MOTO2 = 8, }; -#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis +#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis - -ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) +btGeneric6DofSpring2Constraint : public btTypedConstraint { protected: - btTransform m_frameInA; btTransform m_frameInB; @@ -290,45 +284,43 @@ protected: 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; - 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&) + btGeneric6DofSpring2Constraint& operator=(const 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); + 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); + 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); + 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 void getInfo1(btConstraintInfo1 * info); + virtual void getInfo2(btConstraintInfo2 * info); virtual int calculateSerializeBufferSize() const; virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; @@ -336,19 +328,19 @@ public: 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(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; } + 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& getCalculatedTransformB() const { return m_calculatedTransformB; } - const btTransform & getFrameOffsetA() const { return m_frameInA; } - const btTransform & getFrameOffsetB() const { return m_frameInB; } + const btTransform& getFrameOffsetA() const { return m_frameInA; } + const btTransform& getFrameOffsetB() const { return m_frameInB; } - btTransform & getFrameOffsetA() { return m_frameInA; } - btTransform & getFrameOffsetB() { 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]; } @@ -359,58 +351,58 @@ public: // 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 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 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 getLinearUpperLimit(btVector3 & linearUpper) { linearUpper = m_linearLimits.m_upperLimit; } void setAngularLowerLimit(const btVector3& angularLower) { - for(int i = 0; i < 3; i++) + 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++) + for (int i = 0; i < 3; i++) m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]); } - void getAngularLowerLimit(btVector3& angularLower) + void getAngularLowerLimit(btVector3 & angularLower) { - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) angularLower[i] = m_angularLimits[i].m_loLimit; } - void getAngularLowerLimitReversed(btVector3& angularLower) + void getAngularLowerLimitReversed(btVector3 & angularLower) { - for(int i = 0; i < 3; i++) + 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++) + 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++) + for (int i = 0; i < 3; i++) m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]); } - void getAngularUpperLimit(btVector3& angularUpper) + void getAngularUpperLimit(btVector3 & angularUpper) { - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) angularUpper[i] = m_angularLimits[i].m_hiLimit; } - void getAngularUpperLimitReversed(btVector3& angularUpper) + void getAngularUpperLimitReversed(btVector3 & angularUpper) { - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) angularUpper[i] = -m_angularLimits[i].m_loLimit; } @@ -418,7 +410,7 @@ public: void setLimit(int axis, btScalar lo, btScalar hi) { - if(axis<3) + if (axis < 3) { m_linearLimits.m_lowerLimit[axis] = lo; m_linearLimits.m_upperLimit[axis] = hi; @@ -427,14 +419,14 @@ public: { lo = btNormalizeAngle(lo); hi = btNormalizeAngle(hi); - m_angularLimits[axis-3].m_loLimit = lo; - m_angularLimits[axis-3].m_hiLimit = 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) + if (axis < 3) { m_linearLimits.m_lowerLimit[axis] = lo; m_linearLimits.m_upperLimit[axis] = hi; @@ -443,54 +435,53 @@ public: { lo = btNormalizeAngle(lo); hi = btNormalizeAngle(hi); - m_angularLimits[axis-3].m_hiLimit = -lo; - m_angularLimits[axis-3].m_loLimit = -hi; + m_angularLimits[axis - 3].m_hiLimit = -lo; + m_angularLimits[axis - 3].m_loLimit = -hi; } } bool isLimited(int limitIndex) { - if(limitIndex<3) + if (limitIndex < 3) { return m_linearLimits.isLimited(limitIndex); } - return m_angularLimits[limitIndex-3].isLimited(); + 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 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 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 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). + //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); -}; + 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 { @@ -511,12 +502,12 @@ struct btGeneric6DofSpring2ConstraintData 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]; + 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; @@ -531,13 +522,13 @@ struct btGeneric6DofSpring2ConstraintData 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]; + char m_angularEnableMotor[4]; + char m_angularServoMotor[4]; + char m_angularEnableSpring[4]; + char m_angularSpringStiffnessLimited[4]; + char m_angularSpringDampingLimited[4]; - int m_rotateOrder; + int m_rotateOrder; }; struct btGeneric6DofSpring2ConstraintDoubleData2 @@ -559,12 +550,12 @@ struct btGeneric6DofSpring2ConstraintDoubleData2 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]; + 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; @@ -579,13 +570,13 @@ struct btGeneric6DofSpring2ConstraintDoubleData2 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]; + char m_angularEnableMotor[4]; + char m_angularServoMotor[4]; + char m_angularEnableSpring[4]; + char m_angularSpringStiffnessLimited[4]; + char m_angularSpringDampingLimited[4]; - int m_rotateOrder; + int m_rotateOrder; }; SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const @@ -596,70 +587,70 @@ SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSi SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const { btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer; - btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); + 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++) + 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_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_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++) + 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; + 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++) + 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_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; @@ -672,8 +663,4 @@ SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* da return btGeneric6DofSpring2ConstraintDataName; } - - - - -#endif //BT_GENERIC_6DOF_CONSTRAINT_H +#endif //BT_GENERIC_6DOF_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp index 6f765884e..8baf52bcd 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -17,26 +17,23 @@ subject to the following restrictions: #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" - -btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) +btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) { - init(); + init(); } - btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) - : btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB) + : btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB) { - init(); + init(); } - void btGeneric6DofSpringConstraint::init() { m_objectType = D6_SPRING_CONSTRAINT_TYPE; - for(int i = 0; i < 6; i++) + for (int i = 0; i < 6; i++) { m_springEnabled[i] = false; m_equilibriumPoint[i] = btScalar(0.f); @@ -45,12 +42,11 @@ void btGeneric6DofSpringConstraint::init() } } - void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) { btAssert((index >= 0) && (index < 6)); m_springEnabled[index] = onOff; - if(index < 3) + if (index < 3) { m_linearLimits.m_enableMotor[index] = onOff; } @@ -60,44 +56,38 @@ void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) } } - - void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) { btAssert((index >= 0) && (index < 6)); m_springStiffness[index] = stiffness; } - void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping) { btAssert((index >= 0) && (index < 6)); m_springDamping[index] = damping; } - void btGeneric6DofSpringConstraint::setEquilibriumPoint() { calculateTransforms(); int i; - for( i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; } - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i]; } } - - void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index) { btAssert((index >= 0) && (index < 6)); calculateTransforms(); - if(index < 3) + if (index < 3) { m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; } @@ -113,15 +103,14 @@ void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val) m_equilibriumPoint[index] = val; } - void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info) { // it is assumed that calculateTransforms() have been called before this call int i; //btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity(); - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { - if(m_springEnabled[i]) + if (m_springEnabled[i]) { // get current position of constraint btScalar currPos = m_calculatedLinearDiff[i]; @@ -130,28 +119,27 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf // spring force is (delta * m_stiffness) according to Hooke's Law btScalar force = delta * m_springStiffness[i]; btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations); - m_linearLimits.m_targetVelocity[i] = velFactor * force; - m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; + m_linearLimits.m_targetVelocity[i] = velFactor * force; + m_linearLimits.m_maxMotorForce[i] = btFabs(force); } } - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { - if(m_springEnabled[i + 3]) + if (m_springEnabled[i + 3]) { // get current position of constraint btScalar currPos = m_calculatedAxisAngleDiff[i]; // calculate difference - btScalar delta = currPos - m_equilibriumPoint[i+3]; + btScalar delta = currPos - m_equilibriumPoint[i + 3]; // spring force is (-delta * m_stiffness) according to Hooke's Law - btScalar force = -delta * m_springStiffness[i+3]; - btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations); + 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); } } } - void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) { // this will be called by constraint solver at the constraint setup stage @@ -161,25 +149,21 @@ void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) btGeneric6DofConstraint::getInfo2(info); } - -void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2) +void btGeneric6DofSpringConstraint::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 + 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]); + 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(); + calculateTransforms(); } - - - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h index dac59c688..02b9d4d05 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -16,20 +16,17 @@ subject to the following restrictions: #ifndef BT_GENERIC_6DOF_SPRING_CONSTRAINT_H #define BT_GENERIC_6DOF_SPRING_CONSTRAINT_H - #include "LinearMath/btVector3.h" #include "btTypedConstraint.h" #include "btGeneric6DofConstraint.h" #ifdef BT_USE_DOUBLE_PRECISION -#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2 -#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2" +#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2 +#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2" #else -#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintData -#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintData" -#endif //BT_USE_DOUBLE_PRECISION - - +#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintData +#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintData" +#endif //BT_USE_DOUBLE_PRECISION /// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF @@ -41,101 +38,98 @@ subject to the following restrictions: /// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] ) /// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] ) -ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpringConstraint : public btGeneric6DofConstraint +ATTRIBUTE_ALIGNED16(class) +btGeneric6DofSpringConstraint : public btGeneric6DofConstraint { protected: - bool m_springEnabled[6]; - btScalar m_equilibriumPoint[6]; - btScalar m_springStiffness[6]; - btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping) + bool m_springEnabled[6]; + btScalar m_equilibriumPoint[6]; + btScalar m_springStiffness[6]; + btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping) void init(); - void internalUpdateSprings(btConstraintInfo2* info); -public: - + void internalUpdateSprings(btConstraintInfo2 * info); + +public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); + + btGeneric6DofSpringConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA); + btGeneric6DofSpringConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); void enableSpring(int index, bool onOff); void setStiffness(int index, btScalar stiffness); void setDamping(int index, btScalar damping); - void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF + 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); bool isSpringEnabled(int index) const { - return m_springEnabled[index]; + return m_springEnabled[index]; } btScalar getStiffness(int index) const { - return m_springStiffness[index]; + return m_springStiffness[index]; } btScalar getDamping(int index) const { - return m_springDamping[index]; + return m_springDamping[index]; } btScalar getEquilibriumPoint(int index) const { - return m_equilibriumPoint[index]; + return m_equilibriumPoint[index]; } - virtual void setAxis( const btVector3& axis1, const btVector3& axis2); + virtual void setAxis(const btVector3& axis1, const btVector3& axis2); - virtual void getInfo2 (btConstraintInfo2* info); + virtual void getInfo2(btConstraintInfo2 * info); - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; - struct btGeneric6DofSpringConstraintData { - btGeneric6DofConstraintData m_6dofData; - - int m_springEnabled[6]; - float m_equilibriumPoint[6]; - float m_springStiffness[6]; - float m_springDamping[6]; + btGeneric6DofConstraintData m_6dofData; + + int m_springEnabled[6]; + float m_equilibriumPoint[6]; + float m_springStiffness[6]; + float m_springDamping[6]; }; struct btGeneric6DofSpringConstraintDoubleData2 { - btGeneric6DofConstraintDoubleData2 m_6dofData; - - int m_springEnabled[6]; - double m_equilibriumPoint[6]; - double m_springStiffness[6]; - double m_springDamping[6]; + btGeneric6DofConstraintDoubleData2 m_6dofData; + + int m_springEnabled[6]; + double m_equilibriumPoint[6]; + double m_springStiffness[6]; + double m_springDamping[6]; }; - -SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const { return sizeof(btGeneric6DofSpringConstraintData2); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer; - btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer); + btGeneric6DofConstraint::serialize(&dof->m_6dofData, serializer); int i; - for (i=0;i<6;i++) + for (i = 0; i < 6; i++) { dof->m_equilibriumPoint[i] = m_equilibriumPoint[i]; dof->m_springDamping[i] = m_springDamping[i]; - dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0; + dof->m_springEnabled[i] = m_springEnabled[i] ? 1 : 0; dof->m_springStiffness[i] = m_springStiffness[i]; } return btGeneric6DofSpringConstraintDataName; } -#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H - +#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp index 4be2aabe4..6507e1a0a 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp @@ -13,54 +13,49 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btHinge2Constraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" - - // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) -: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(),RO_XYZ), - m_anchor(anchor), - m_axis1(axis1), - m_axis2(axis2) + : btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), RO_XYZ), + m_anchor(anchor), + m_axis1(axis1), + m_axis2(axis2) { // build frame basis // 6DOF constraint uses Euler angles and to define limits // it is assumed that rotational order is : // Z - first, allowed limits are (-PI,PI); - // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number + // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number // used to prevent constraint from instability on poles; // new position of X, allowed limits are (-PI,PI); // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs // Build the frame in world coordinate system first btVector3 zAxis = axis1.normalize(); btVector3 xAxis = axis2.normalize(); - btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system + btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system btTransform frameInW; frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); + frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); frameInW.setOrigin(anchor); // now get constraint frame in local coordinate systems m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; // sei limits setLinearLowerLimit(btVector3(0.f, 0.f, -1.f)); - setLinearUpperLimit(btVector3(0.f, 0.f, 1.f)); + setLinearUpperLimit(btVector3(0.f, 0.f, 1.f)); // like front wheels of a car - setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f)); - setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); + setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f)); + setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); // enable suspension enableSpring(2, true); setStiffness(2, SIMD_PI * SIMD_PI * 4.f); setDamping(2, 0.01f); setEquilibriumPoint(); } - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h index 06a8e3ecd..95f604a89 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h @@ -16,32 +16,30 @@ subject to the following restrictions: #ifndef BT_HINGE2_CONSTRAINT_H #define BT_HINGE2_CONSTRAINT_H - - #include "LinearMath/btVector3.h" #include "btTypedConstraint.h" #include "btGeneric6DofSpring2Constraint.h" - - // Constraint similar to ODE Hinge2 Joint // has 3 degrees of frredom: // 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2) // 1 translational (along axis Z) with suspension spring -ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpring2Constraint +ATTRIBUTE_ALIGNED16(class) +btHinge2Constraint : public btGeneric6DofSpring2Constraint { protected: - btVector3 m_anchor; - btVector3 m_axis1; - btVector3 m_axis2; + btVector3 m_anchor; + btVector3 m_axis1; + btVector3 m_axis2; + public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - + BT_DECLARE_ALIGNED_ALLOCATOR(); + // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 - btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2); + btHinge2Constraint(btRigidBody & rbA, btRigidBody & rbB, btVector3 & anchor, btVector3 & axis1, btVector3 & axis2); // access const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } @@ -51,10 +49,7 @@ public: btScalar getAngle2() { return getAngle(0); } // limits void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); } - void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); } + void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3(1.f, 0.f, ang1min)); } }; - - -#endif // BT_HINGE2_CONSTRAINT_H - +#endif // BT_HINGE2_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 7e5e6f9e5..aa6f69000 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btHingeConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" @@ -21,8 +20,6 @@ subject to the following restrictions: #include #include "btSolverBody.h" - - //#define HINGE_USE_OBSOLETE_SOLVER false #define HINGE_USE_OBSOLETE_SOLVER false @@ -30,59 +27,60 @@ subject to the following restrictions: #ifndef __SPU__ - - - - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, - const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) - :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB, + const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA) + : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB), #ifdef _BT_USE_CENTER_LIMIT_ - m_limit(), + m_limit(), #endif - m_angularOnly(false), - m_enableAngularMotor(false), - m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), - m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), - m_useReferenceFrameA(useReferenceFrameA), - m_flags(0), - m_normalCFM(0), - m_normalERP(0), - m_stopCFM(0), - m_stopERP(0) + m_angularOnly(false), + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), + m_useReferenceFrameA(useReferenceFrameA), + m_flags(0), + m_normalCFM(0), + m_normalERP(0), + m_stopCFM(0), + m_stopERP(0) { m_rbAFrame.getOrigin() = pivotInA; - + // since no frame is given, assume this to be zero angle and just pick rb transform axis btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); btVector3 rbAxisA2; btScalar projection = axisInA.dot(rbAxisA1); - if (projection >= 1.0f - SIMD_EPSILON) { + if (projection >= 1.0f - SIMD_EPSILON) + { rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - } else if (projection <= -1.0f + SIMD_EPSILON) { + } + else if (projection <= -1.0f + SIMD_EPSILON) + { rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); - rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - } else { + rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); + } + else + { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); } - m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), - rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), - rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); + m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(), + rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(), + rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ()); + + btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB); + btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1); + btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); - btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - m_rbBFrame.getOrigin() = pivotInB; - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); - -#ifndef _BT_USE_CENTER_LIMIT_ + m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(), + rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(), + rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ()); + +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -94,47 +92,44 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } - - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA) + : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), #ifdef _BT_USE_CENTER_LIMIT_ -m_limit(), + m_limit(), #endif -m_angularOnly(false), m_enableAngularMotor(false), -m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), -m_useReferenceFrameA(useReferenceFrameA), -m_flags(0), -m_normalCFM(0), -m_normalERP(0), -m_stopCFM(0), -m_stopERP(0) + m_angularOnly(false), + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), + m_useReferenceFrameA(useReferenceFrameA), + m_flags(0), + m_normalCFM(0), + m_normalERP(0), + m_stopCFM(0), + m_stopERP(0) { - // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; - m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), - rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), - rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); + m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(), + rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(), + rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ()); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); + btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB); + btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); - -#ifndef _BT_USE_CENTER_LIMIT_ + m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(), + rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(), + rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ()); + +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -146,26 +141,24 @@ m_stopERP(0) m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } - - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, - const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, + const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) + : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame), #ifdef _BT_USE_CENTER_LIMIT_ -m_limit(), + m_limit(), #endif -m_angularOnly(false), -m_enableAngularMotor(false), -m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), -m_useReferenceFrameA(useReferenceFrameA), -m_flags(0), -m_normalCFM(0), -m_normalERP(0), -m_stopCFM(0), -m_stopERP(0) + m_angularOnly(false), + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), + m_useReferenceFrameA(useReferenceFrameA), + m_flags(0), + m_normalCFM(0), + m_normalERP(0), + m_stopCFM(0), + m_stopERP(0) { -#ifndef _BT_USE_CENTER_LIMIT_ +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -175,30 +168,28 @@ m_stopERP(0) m_solveLimit = false; #endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); -} - - +} btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), + : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_rbBFrame(rbAFrame), #ifdef _BT_USE_CENTER_LIMIT_ -m_limit(), + m_limit(), #endif -m_angularOnly(false), -m_enableAngularMotor(false), -m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), -m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), -m_useReferenceFrameA(useReferenceFrameA), -m_flags(0), -m_normalCFM(0), -m_normalERP(0), -m_stopCFM(0), -m_stopERP(0) + m_angularOnly(false), + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), + m_useReferenceFrameA(useReferenceFrameA), + m_flags(0), + m_normalCFM(0), + m_normalERP(0), + m_stopCFM(0), + m_stopERP(0) { ///not providing rigidbody B means implicitly using worldspace for body B m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); -#ifndef _BT_USE_CENTER_LIMIT_ +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -210,9 +201,7 @@ m_stopERP(0) m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } - - -void btHingeConstraint::buildJacobian() +void btHingeConstraint::buildJacobian() { if (m_useSolveConstraintObsolete) { @@ -221,8 +210,8 @@ void btHingeConstraint::buildJacobian() if (!m_angularOnly) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin(); btVector3 relPos = pivotBInW - pivotAInW; btVector3 normal[3]; @@ -232,23 +221,23 @@ void btHingeConstraint::buildJacobian() } else { - normal[0].setValue(btScalar(1.0),0,0); + normal[0].setValue(btScalar(1.0), 0, 0); } btPlaneSpace1(normal[0], normal[1], normal[2]); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normal[i], + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); } } @@ -258,60 +247,55 @@ void btHingeConstraint::buildJacobian() //this is unused for now, it's a todo btVector3 jointAxis0local; btVector3 jointAxis1local; - - btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); + + btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2), jointAxis0local, jointAxis1local); btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - - new (&m_jacAng[0]) btJacobianEntry(jointAxis0, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - new (&m_jacAng[1]) btJacobianEntry(jointAxis1, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + new (&m_jacAng[0]) btJacobianEntry(jointAxis0, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); - new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + new (&m_jacAng[1]) btJacobianEntry(jointAxis1, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); - // clear accumulator - m_accLimitImpulse = btScalar(0.); + new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); - // test angular limit - testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + // clear accumulator + m_accLimitImpulse = btScalar(0.); + + // test angular limit + testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); //Compute K = J*W*J' for hinge axis - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + - getRigidBodyB().computeAngularImpulseDenominator(axisA)); - + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + + getRigidBodyB().computeAngularImpulseDenominator(axisA)); } } - -#endif //__SPU__ - +#endif //__SPU__ static inline btScalar btNormalizeAnglePositive(btScalar angle) { - return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI)); + return btFmod(btFmod(angle, btScalar(2.0 * SIMD_PI)) + btScalar(2.0 * SIMD_PI), btScalar(2.0 * SIMD_PI)); } - - static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle) { btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) - - btNormalizeAnglePositive(accAngle))); + btNormalizeAnglePositive(accAngle))); return result; } @@ -320,41 +304,36 @@ static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle) btScalar tol(0.3); btScalar result = btShortestAngularDistance(accAngle, curAngle); - if (btFabs(result) > tol) + if (btFabs(result) > tol) return curAngle; - else + else return accAngle + result; return curAngle; } - btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle() { btScalar hingeAngle = getHingeAngle(); - m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle); + m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, hingeAngle); return m_accumulatedAngle; } -void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle) +void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle) { - m_accumulatedAngle = accAngle; + m_accumulatedAngle = accAngle; } void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info) { //update m_accumulatedAngle btScalar curHingeAngle = getHingeAngle(); - m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle); + m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, curHingeAngle); btHingeConstraint::getInfo1(info); - } - void btHingeConstraint::getInfo1(btConstraintInfo1* info) { - - if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; @@ -362,17 +341,16 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info) } else { - info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular - info->nub = 1; + info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular + info->nub = 1; //always add the row, to avoid computation (data is not available yet) //prepare constraint - testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - if(getSolveLimit() || getEnableAngularMotor()) + testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); + if (getSolveLimit() || getEnableAngularMotor()) { - info->m_numConstraintRows++; // limit 3rd anguar as well - info->nub--; + info->m_numConstraintRows++; // limit 3rd anguar as well + info->nub--; } - } } @@ -386,41 +364,38 @@ void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) else { //always add the 'limit' row, to avoid computation (data is not available yet) - info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular - info->nub = 0; + info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular + info->nub = 0; } } -void btHingeConstraint::getInfo2 (btConstraintInfo2* info) +void btHingeConstraint::getInfo2(btConstraintInfo2* info) { - if(m_useOffsetForConstraintFrame) + if (m_useOffsetForConstraintFrame) { - getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); + getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity()); } else { - getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); + getInfo2Internal(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity()); } } - -void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) +void btHingeConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB) { ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now - testLimit(transA,transB); + testLimit(transA, transB); - getInfo2Internal(info,transA,transB,angVelA,angVelB); + getInfo2Internal(info, transA, transB, angVelA, angVelB); } - -void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) +void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB) { - btAssert(!m_useSolveConstraintObsolete); int i, skip = info->rowskip; // transforms in world space - btTransform trA = transA*m_rbAFrame; - btTransform trB = transB*m_rbBFrame; + btTransform trA = transA * m_rbAFrame; + btTransform trB = transB * m_rbBFrame; // pivot point btVector3 pivotAInW = trA.getOrigin(); btVector3 pivotBInW = trB.getOrigin(); @@ -448,7 +423,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf info->m_constraintError[i*skip]=0.f; } } -#endif //#if 0 +#endif //#if 0 // linear (all fixed) if (!m_angularOnly) @@ -460,10 +435,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf info->m_J2linearAxis[0] = -1; info->m_J2linearAxis[skip + 1] = -1; info->m_J2linearAxis[2 * skip + 2] = -1; - } - - - + } btVector3 a1 = pivotAInW - transA.getOrigin(); { @@ -471,22 +443,22 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2); } btVector3 a2 = pivotBInW - transB.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + a2.getSkewSymmetricMatrix(angular0, angular1, angular2); } // linear RHS btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp; - btScalar k = info->fps * normalErp; + btScalar k = info->fps * normalErp; if (!m_angularOnly) { - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); } @@ -504,9 +476,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf // get 2 orthos to hinge axis (X, Y) btVector3 p = trA.getBasis().getColumn(0); btVector3 q = trA.getBasis().getColumn(1); - // set the two hinge angular rows - int s3 = 3 * info->rowskip; - int s4 = 4 * info->rowskip; + // set the two hinge angular rows + int s3 = 3 * info->rowskip; + int s4 = 4 * info->rowskip; info->m_J1angularAxis[s3 + 0] = p[0]; info->m_J1angularAxis[s3 + 1] = p[1]; @@ -521,181 +493,172 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf info->m_J2angularAxis[s4 + 0] = -q[0]; info->m_J2angularAxis[s4 + 1] = -q[1]; info->m_J2angularAxis[s4 + 2] = -q[2]; - // compute the right hand side of the constraint equation. set relative - // body velocities along p and q to bring the hinge back into alignment. - // if ax1,ax2 are the unit length hinge axes as computed from body1 and - // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). - // if `theta' is the angle between ax1 and ax2, we need an angular velocity - // along u to cover angle erp*theta in one step : - // |angular_velocity| = angle/time = erp*theta / stepsize - // = (erp*fps) * theta - // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| - // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) - // ...as ax1 and ax2 are unit length. if theta is smallish, - // theta ~= sin(theta), so - // angular_velocity = (erp*fps) * (ax1 x ax2) - // ax1 x ax2 is in the plane space of ax1, so we project the angular - // velocity to p and q to find the right hand side. - btVector3 ax2 = trB.getBasis().getColumn(2); + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1,ax2 are the unit length hinge axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + btVector3 ax2 = trB.getBasis().getColumn(2); btVector3 u = ax1.cross(ax2); info->m_constraintError[s3] = k * u.dot(p); info->m_constraintError[s4] = k * u.dot(q); // check angular limits - int nrow = 4; // last filled row + int nrow = 4; // last filled row int srow; btScalar limit_err = btScalar(0.0); int limit = 0; - if(getSolveLimit()) + if (getSolveLimit()) { -#ifdef _BT_USE_CENTER_LIMIT_ - limit_err = m_limit.getCorrection() * m_referenceSign; +#ifdef _BT_USE_CENTER_LIMIT_ + limit_err = m_limit.getCorrection() * m_referenceSign; #else - limit_err = m_correction * m_referenceSign; + limit_err = m_correction * m_referenceSign; #endif - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - + limit = (limit_err > btScalar(0.0)) ? 1 : 2; } // if the hinge has joint limits or motor, add in the extra row bool powered = getEnableAngularMotor(); - if(limit || powered) + if (limit || powered) { nrow++; srow = nrow * info->rowskip; - info->m_J1angularAxis[srow+0] = ax1[0]; - info->m_J1angularAxis[srow+1] = ax1[1]; - info->m_J1angularAxis[srow+2] = ax1[2]; + info->m_J1angularAxis[srow + 0] = ax1[0]; + info->m_J1angularAxis[srow + 1] = ax1[1]; + info->m_J1angularAxis[srow + 2] = ax1[2]; - info->m_J2angularAxis[srow+0] = -ax1[0]; - info->m_J2angularAxis[srow+1] = -ax1[1]; - info->m_J2angularAxis[srow+2] = -ax1[2]; + info->m_J2angularAxis[srow + 0] = -ax1[0]; + info->m_J2angularAxis[srow + 1] = -ax1[1]; + info->m_J2angularAxis[srow + 2] = -ax1[2]; btScalar lostop = getLowerLimit(); btScalar histop = getUpperLimit(); - if(limit && (lostop == histop)) + if (limit && (lostop == histop)) { // the joint motor is ineffective powered = false; } info->m_constraintError[srow] = btScalar(0.0f); btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; - if(powered) + if (powered) { - if(m_flags & BT_HINGE_FLAGS_CFM_NORM) + if (m_flags & BT_HINGE_FLAGS_CFM_NORM) { info->cfm[srow] = m_normalCFM; } btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; - info->m_lowerLimit[srow] = - m_maxMotorImpulse; - info->m_upperLimit[srow] = m_maxMotorImpulse; + info->m_lowerLimit[srow] = -m_maxMotorImpulse; + info->m_upperLimit[srow] = m_maxMotorImpulse; } - if(limit) + if (limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_HINGE_FLAGS_CFM_STOP) + if (m_flags & BT_HINGE_FLAGS_CFM_STOP) { info->cfm[srow] = m_stopCFM; } - if(lostop == histop) + if (lostop == histop) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } - else if(limit == 1) - { // low limit + else if (limit == 1) + { // low limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } - else - { // high limit + else + { // high limit info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ btScalar bounce = m_limit.getRelaxationFactor(); #else btScalar bounce = m_relaxationFactor; #endif - if(bounce > btScalar(0.0)) + if (bounce > btScalar(0.0)) { btScalar vel = angVelA.dot(ax1); vel -= angVelB.dot(ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) + if (limit == 1) + { // low limit + if (vel < 0) { btScalar newc = -bounce * vel; - if(newc > info->m_constraintError[srow]) + if (newc > info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } else - { // high limit - all those computations are reversed - if(vel > 0) + { // high limit - all those computations are reversed + if (vel > 0) { btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) + if (newc < info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } } -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ info->m_constraintError[srow] *= m_limit.getBiasFactor(); #else info->m_constraintError[srow] *= m_biasFactor; #endif - } // if(limit) - } // if angular limit or powered + } // if(limit) + } // if angular limit or powered } - -void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) +void btHingeConstraint::setFrames(const btTransform& frameA, const btTransform& frameB) { m_rbAFrame = frameA; m_rbBFrame = frameB; buildJacobian(); } - -void btHingeConstraint::updateRHS(btScalar timeStep) +void btHingeConstraint::updateRHS(btScalar timeStep) { (void)timeStep; - } - - - btScalar btHingeConstraint::getHingeAngle() { - return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + return getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); } -btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) +btScalar btHingeConstraint::getHingeAngle(const btTransform& transA, const btTransform& transB) { - const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); - const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); + const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); + const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1); -// btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); + // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); return m_referenceSign * angle; } - - -void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) +void btHingeConstraint::testLimit(const btTransform& transA, const btTransform& transB) { // Compute limit information - m_hingeAngle = getHingeAngle(transA,transB); -#ifdef _BT_USE_CENTER_LIMIT_ + m_hingeAngle = getHingeAngle(transA, transB); +#ifdef _BT_USE_CENTER_LIMIT_ m_limit.test(m_hingeAngle); #else m_correction = btScalar(0.); @@ -709,7 +672,7 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t m_correction = (m_lowerLimit - m_hingeAngle); m_limitSign = 1.0f; m_solveLimit = true; - } + } else if (m_hingeAngle >= m_upperLimit) { m_correction = m_upperLimit - m_hingeAngle; @@ -721,7 +684,6 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t return; } - static btVector3 vHinge(0, 0, btScalar(1)); void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) @@ -731,14 +693,15 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) qConstraint.normalize(); // extract "pure" hinge component - btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); + btVector3 vNoHinge = quatRotate(qConstraint, vHinge); + vNoHinge.normalize(); btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); btQuaternion qHinge = qNoHinge.inverse() * qConstraint; qHinge.normalize(); // compute angular target, clamped to limits btScalar targetAngle = qHinge.getAngle(); - if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. + if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. { qHinge = -(qHinge); targetAngle = qHinge.getAngle(); @@ -751,7 +714,7 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) { -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ m_limit.fit(targetAngle); #else if (m_lowerLimit < m_upperLimit) @@ -763,20 +726,18 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) } #endif // compute angular velocity - btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); btScalar dAngle = targetAngle - curAngle; m_motorTargetVelocity = dAngle / dt; } - - -void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) +void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB) { btAssert(!m_useSolveConstraintObsolete); int i, s = info->rowskip; // transforms in world space - btTransform trA = transA*m_rbAFrame; - btTransform trB = transB*m_rbBFrame; + btTransform trA = transA * m_rbAFrame; + btTransform trB = transB * m_rbBFrame; // pivot point // btVector3 pivotAInW = trA.getOrigin(); // btVector3 pivotBInW = trB.getOrigin(); @@ -789,11 +750,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); btScalar miS = miA + miB; btScalar factA, factB; - if(miS > btScalar(0.f)) + if (miS > btScalar(0.f)) { factA = miB / miS; } - else + else { factA = btScalar(0.5f); } @@ -803,15 +764,21 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info btVector3 ax1A = trA.getBasis().getColumn(2); btVector3 ax1B = trB.getBasis().getColumn(2); btVector3 ax1 = ax1A * factA + ax1B * factB; + if (ax1.length2() SIMD_EPSILON) + if (len2 > SIMD_EPSILON) { p /= btSqrt(len2); } @@ -843,44 +810,44 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info // fill three rows tmpA = relA.cross(p); tmpB = relB.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s0 + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s0 + i] = -tmpB[i]; tmpA = relA.cross(q); tmpB = relB.cross(q); - if(hasStaticBody && getSolveLimit()) - { // to make constraint between static and dynamic objects more rigid + if (hasStaticBody && getSolveLimit()) + { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } - for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s1 + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s1 + i] = -tmpB[i]; tmpA = relA.cross(ax1); tmpB = relB.cross(ax1); - if(hasStaticBody) - { // to make constraint between static and dynamic objects more rigid + if (hasStaticBody) + { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation tmpB *= factB; tmpA *= factA; } - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = -tmpB[i]; - btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp; + btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp; btScalar k = info->fps * normalErp; if (!m_angularOnly) { - for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s0 + i] = p[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s1 + i] = q[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = ax1[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s0 + i] = -p[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s1 + i] = -q[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -ax1[i]; + + // compute three elements of right hand side - // compute three elements of right hand side - btScalar rhs = k * p.dot(ofs); info->m_constraintError[s0] = rhs; rhs = k * q.dot(ofs); @@ -925,146 +892,144 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. - k = info->fps * normalErp;//?? + k = info->fps * normalErp; //?? btVector3 u = ax1A.cross(ax1B); info->m_constraintError[s3] = k * u.dot(p); info->m_constraintError[s4] = k * u.dot(q); #endif // check angular limits - nrow = 4; // last filled row + nrow = 4; // last filled row int srow; btScalar limit_err = btScalar(0.0); int limit = 0; - if(getSolveLimit()) + if (getSolveLimit()) { -#ifdef _BT_USE_CENTER_LIMIT_ - limit_err = m_limit.getCorrection() * m_referenceSign; +#ifdef _BT_USE_CENTER_LIMIT_ + limit_err = m_limit.getCorrection() * m_referenceSign; #else - limit_err = m_correction * m_referenceSign; + limit_err = m_correction * m_referenceSign; #endif - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - + limit = (limit_err > btScalar(0.0)) ? 1 : 2; } // if the hinge has joint limits or motor, add in the extra row bool powered = getEnableAngularMotor(); - if(limit || powered) + if (limit || powered) { nrow++; srow = nrow * info->rowskip; - info->m_J1angularAxis[srow+0] = ax1[0]; - info->m_J1angularAxis[srow+1] = ax1[1]; - info->m_J1angularAxis[srow+2] = ax1[2]; + info->m_J1angularAxis[srow + 0] = ax1[0]; + info->m_J1angularAxis[srow + 1] = ax1[1]; + info->m_J1angularAxis[srow + 2] = ax1[2]; - info->m_J2angularAxis[srow+0] = -ax1[0]; - info->m_J2angularAxis[srow+1] = -ax1[1]; - info->m_J2angularAxis[srow+2] = -ax1[2]; + info->m_J2angularAxis[srow + 0] = -ax1[0]; + info->m_J2angularAxis[srow + 1] = -ax1[1]; + info->m_J2angularAxis[srow + 2] = -ax1[2]; btScalar lostop = getLowerLimit(); btScalar histop = getUpperLimit(); - if(limit && (lostop == histop)) + if (limit && (lostop == histop)) { // the joint motor is ineffective powered = false; } info->m_constraintError[srow] = btScalar(0.0f); btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; - if(powered) + if (powered) { - if(m_flags & BT_HINGE_FLAGS_CFM_NORM) + if (m_flags & BT_HINGE_FLAGS_CFM_NORM) { info->cfm[srow] = m_normalCFM; } btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; - info->m_lowerLimit[srow] = - m_maxMotorImpulse; - info->m_upperLimit[srow] = m_maxMotorImpulse; + info->m_lowerLimit[srow] = -m_maxMotorImpulse; + info->m_upperLimit[srow] = m_maxMotorImpulse; } - if(limit) + if (limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_HINGE_FLAGS_CFM_STOP) + if (m_flags & BT_HINGE_FLAGS_CFM_STOP) { info->cfm[srow] = m_stopCFM; } - if(lostop == histop) + if (lostop == histop) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } - else if(limit == 1) - { // low limit + else if (limit == 1) + { // low limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } - else - { // high limit + else + { // high limit info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ btScalar bounce = m_limit.getRelaxationFactor(); #else btScalar bounce = m_relaxationFactor; #endif - if(bounce > btScalar(0.0)) + if (bounce > btScalar(0.0)) { btScalar vel = angVelA.dot(ax1); vel -= angVelB.dot(ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) + if (limit == 1) + { // low limit + if (vel < 0) { btScalar newc = -bounce * vel; - if(newc > info->m_constraintError[srow]) + if (newc > info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } else - { // high limit - all those computations are reversed - if(vel > 0) + { // high limit - all those computations are reversed + if (vel > 0) { btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) + if (newc < info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } } -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ info->m_constraintError[srow] *= m_limit.getBiasFactor(); #else info->m_constraintError[srow] *= m_biasFactor; #endif - } // if(limit) - } // if angular limit or powered + } // if(limit) + } // if angular limit or powered } - -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///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 btHingeConstraint::setParam(int num, btScalar value, int axis) { - if((axis == -1) || (axis == 5)) + if ((axis == -1) || (axis == 5)) { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : + switch (num) + { + case BT_CONSTRAINT_STOP_ERP: m_stopERP = value; m_flags |= BT_HINGE_FLAGS_ERP_STOP; break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: m_stopCFM = value; m_flags |= BT_HINGE_FLAGS_CFM_STOP; break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: m_normalCFM = value; m_flags |= BT_HINGE_FLAGS_CFM_NORM; break; @@ -1072,7 +1037,7 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis) m_normalERP = value; m_flags |= BT_HINGE_FLAGS_ERP_NORM; break; - default : + default: btAssertConstrParams(0); } } @@ -1083,22 +1048,22 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis) } ///return the local value of parameter -btScalar btHingeConstraint::getParam(int num, int axis) const +btScalar btHingeConstraint::getParam(int num, int axis) const { btScalar retVal = 0; - if((axis == -1) || (axis == 5)) + if ((axis == -1) || (axis == 5)) { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : + switch (num) + { + case BT_CONSTRAINT_STOP_ERP: btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP); retVal = m_stopERP; break; - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_STOP_CFM: btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); retVal = m_stopCFM; break; - case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_CFM: btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); retVal = m_normalCFM; break; @@ -1106,7 +1071,7 @@ btScalar btHingeConstraint::getParam(int num, int axis) const btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM); retVal = m_normalERP; break; - default : + default: btAssertConstrParams(0); } } @@ -1116,5 +1081,3 @@ btScalar btHingeConstraint::getParam(int num, int axis) const } return retVal; } - - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 3c3df24db..c7509e30a 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -20,7 +20,6 @@ subject to the following restrictions: #define _BT_USE_CENTER_LIMIT_ 1 - #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" #include "btTypedConstraint.h" @@ -28,14 +27,12 @@ subject to the following restrictions: class btRigidBody; #ifdef BT_USE_DOUBLE_PRECISION -#define btHingeConstraintData btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version -#define btHingeConstraintDataName "btHingeConstraintDoubleData2" +#define btHingeConstraintData btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version +#define btHingeConstraintDataName "btHingeConstraintDoubleData2" #else -#define btHingeConstraintData btHingeConstraintFloatData -#define btHingeConstraintDataName "btHingeConstraintFloatData" -#endif //BT_USE_DOUBLE_PRECISION - - +#define btHingeConstraintData btHingeConstraintFloatData +#define btHingeConstraintDataName "btHingeConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION enum btHingeFlags { @@ -45,89 +42,83 @@ enum btHingeFlags BT_HINGE_FLAGS_ERP_NORM = 8 }; - /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space /// axis defines the orientation of the hinge axis -ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) +btHingeConstraint : public btTypedConstraint { #ifdef IN_PARALLELL_SOLVER public: #endif - btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor + btJacobianEntry m_jac[3]; //3 orthogonal linear constraints + btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis. btTransform m_rbBFrame; - btScalar m_motorTargetVelocity; - btScalar m_maxMotorImpulse; + btScalar m_motorTargetVelocity; + btScalar m_maxMotorImpulse; - -#ifdef _BT_USE_CENTER_LIMIT_ - btAngularLimit m_limit; +#ifdef _BT_USE_CENTER_LIMIT_ + btAngularLimit m_limit; #else - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_limitSign; - btScalar m_correction; + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_limitSign; + btScalar m_correction; - btScalar m_limitSoftness; - btScalar m_biasFactor; - btScalar m_relaxationFactor; + btScalar m_limitSoftness; + btScalar m_biasFactor; + btScalar m_relaxationFactor; - bool m_solveLimit; + bool m_solveLimit; #endif - btScalar m_kHinge; + btScalar m_kHinge; + btScalar m_accLimitImpulse; + btScalar m_hingeAngle; + btScalar m_referenceSign; - btScalar m_accLimitImpulse; - btScalar m_hingeAngle; - btScalar m_referenceSign; + bool m_angularOnly; + bool m_enableAngularMotor; + bool m_useSolveConstraintObsolete; + bool m_useOffsetForConstraintFrame; + bool m_useReferenceFrameA; - bool m_angularOnly; - bool m_enableAngularMotor; - bool m_useSolveConstraintObsolete; - bool m_useOffsetForConstraintFrame; - bool m_useReferenceFrameA; + btScalar m_accMotorImpulse; - btScalar m_accMotorImpulse; + int m_flags; + btScalar m_normalCFM; + btScalar m_normalERP; + btScalar m_stopCFM; + btScalar m_stopERP; - int m_flags; - btScalar m_normalCFM; - btScalar m_normalERP; - btScalar m_stopCFM; - btScalar m_stopERP; - - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false); - btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false); - - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false); + btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB, const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA = false); - btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false); + btHingeConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false); + btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false); - virtual void buildJacobian(); + btHingeConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false); - virtual void getInfo1 (btConstraintInfo1* info); + virtual void buildJacobian(); - void getInfo1NonVirtual(btConstraintInfo1* info); + virtual void getInfo1(btConstraintInfo1 * info); - virtual void getInfo2 (btConstraintInfo2* info); + void getInfo1NonVirtual(btConstraintInfo1 * info); - void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); + virtual void getInfo2(btConstraintInfo2 * info); - void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); - void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); - + void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB); - void updateRHS(btScalar timeStep); + void getInfo2Internal(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB); + void getInfo2InternalUsingFrameOffset(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB); + + void updateRHS(btScalar timeStep); const btRigidBody& getRigidBodyA() const { @@ -138,19 +129,19 @@ public: return m_rbB; } - btRigidBody& getRigidBodyA() - { - return m_rbA; - } + btRigidBody& getRigidBodyA() + { + return m_rbA; + } - btRigidBody& getRigidBodyB() - { - return m_rbB; + btRigidBody& getRigidBodyB() + { + return m_rbB; } btTransform& getFrameOffsetA() { - return m_rbAFrame; + return m_rbAFrame; } btTransform& getFrameOffsetB() @@ -159,15 +150,15 @@ public: } void setFrames(const btTransform& frameA, const btTransform& frameB); - - void setAngularOnly(bool angularOnly) + + void setAngularOnly(bool angularOnly) { m_angularOnly = angularOnly; } - void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse) + void enableAngularMotor(bool enableMotor, btScalar targetVelocity, btScalar maxMotorImpulse) { - m_enableAngularMotor = enableMotor; + m_enableAngularMotor = enableMotor; m_motorTargetVelocity = targetVelocity; m_maxMotorImpulse = maxMotorImpulse; } @@ -175,29 +166,28 @@ public: // extra motor API, including ability to set a target rotation (as opposed to angular velocity) // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to // maintain a given angular target. - void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; } + void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; } void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; } void setMotorTargetVelocity(btScalar motorTargetVelocity) { m_motorTargetVelocity = motorTargetVelocity; } - void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B. + void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B. void setMotorTarget(btScalar targetAngle, btScalar dt); - - void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) + void setLimit(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor); #else m_lowerLimit = btNormalizeAngle(low); m_upperLimit = btNormalizeAngle(high); - m_limitSoftness = _softness; + m_limitSoftness = _softness; m_biasFactor = _biasFactor; m_relaxationFactor = _relaxationFactor; #endif } - + btScalar getLimitSoftness() const { -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ return m_limit.getSoftness(); #else return m_limitSoftness; @@ -206,7 +196,7 @@ public: btScalar getLimitBiasFactor() const { -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ return m_limit.getBiasFactor(); #else return m_biasFactor; @@ -215,112 +205,110 @@ public: btScalar getLimitRelaxationFactor() const { -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ return m_limit.getRelaxationFactor(); #else return m_relaxationFactor; #endif } - void setAxis(btVector3& axisInA) + void setAxis(btVector3 & axisInA) { btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); btVector3 pivotInA = m_rbAFrame.getOrigin(); -// m_rbAFrame.getOrigin() = pivotInA; - m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), - rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), - rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); + // m_rbAFrame.getOrigin() = pivotInA; + m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(), + rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(), + rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ()); btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA; - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); + btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB); + btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA)); - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); + m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(), + rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(), + rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ()); m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis(); - } - bool hasLimit() const { -#ifdef _BT_USE_CENTER_LIMIT_ - return m_limit.getHalfRange() > 0; -#else - return m_lowerLimit <= m_upperLimit; -#endif - } - - btScalar getLowerLimit() const + bool hasLimit() const { -#ifdef _BT_USE_CENTER_LIMIT_ - return m_limit.getLow(); +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getHalfRange() > 0; #else - return m_lowerLimit; + return m_lowerLimit <= m_upperLimit; #endif } - btScalar getUpperLimit() const + btScalar getLowerLimit() const { -#ifdef _BT_USE_CENTER_LIMIT_ - return m_limit.getHigh(); -#else - return m_upperLimit; +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getLow(); +#else + return m_lowerLimit; #endif } + btScalar getUpperLimit() const + { +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getHigh(); +#else + return m_upperLimit; +#endif + } ///The getHingeAngle gives the hinge angle in range [-PI,PI] btScalar getHingeAngle(); - btScalar getHingeAngle(const btTransform& transA,const btTransform& transB); + btScalar getHingeAngle(const btTransform& transA, const btTransform& transB); - void testLimit(const btTransform& transA,const btTransform& transB); + void testLimit(const btTransform& transA, const btTransform& transB); - - const btTransform& getAFrame() const { return m_rbAFrame; }; + const btTransform& getAFrame() const { return m_rbAFrame; }; const btTransform& getBFrame() const { return m_rbBFrame; }; - btTransform& getAFrame() { return m_rbAFrame; }; + btTransform& getAFrame() { return m_rbAFrame; }; btTransform& getBFrame() { return m_rbBFrame; }; inline int getSolveLimit() { -#ifdef _BT_USE_CENTER_LIMIT_ - return m_limit.isLimit(); +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.isLimit(); #else - return m_solveLimit; + return m_solveLimit; #endif } inline btScalar getLimitSign() { -#ifdef _BT_USE_CENTER_LIMIT_ - return m_limit.getSign(); +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getSign(); #else return m_limitSign; #endif } - inline bool getAngularOnly() - { - return m_angularOnly; + inline bool getAngularOnly() + { + return m_angularOnly; } - inline bool getEnableAngularMotor() - { - return m_enableAngularMotor; + inline bool getEnableAngularMotor() + { + return m_enableAngularMotor; } - inline btScalar getMotorTargetVelocity() - { - return m_motorTargetVelocity; + inline btScalar getMotorTargetVelocity() + { + return m_motorTargetVelocity; } - inline btScalar getMaxMotorImpulse() - { - return m_maxMotorImpulse; + inline btScalar getMaxMotorImpulse() + { + return m_maxMotorImpulse; } // access for UseFrameOffset bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } @@ -329,143 +317,132 @@ public: bool getUseReferenceFrameA() const { return m_useReferenceFrameA; } void setUseReferenceFrameA(bool useReferenceFrameA) { m_useReferenceFrameA = useReferenceFrameA; } - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///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 void setParam(int num, btScalar value, int axis = -1); ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; - - virtual int getFlags() const + virtual btScalar getParam(int num, int axis = -1) const; + + virtual int getFlags() const { - return m_flags; + return m_flags; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; - //only for backward compatibility #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION ///this structure is not used, except for loading pre-2.82 .bullet files -struct btHingeConstraintDoubleData +struct btHingeConstraintDoubleData { - btTypedConstraintData m_typeConstraintData; - btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTypedConstraintData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. btTransformDoubleData 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; + 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; }; -#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION +#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION ///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account -ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint +ATTRIBUTE_ALIGNED16(class) +btHingeAccumulatedAngleConstraint : public btHingeConstraint { protected: - btScalar m_accumulatedAngle; + btScalar m_accumulatedAngle; + public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false) - :btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA ) + + btHingeAccumulatedAngleConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB, const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA = false) + : btHingeConstraint(rbA, rbB, pivotInA, pivotInB, axisInA, axisInB, useReferenceFrameA) { - m_accumulatedAngle=getHingeAngle(); + m_accumulatedAngle = getHingeAngle(); } - btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false) - :btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA) + btHingeAccumulatedAngleConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false) + : btHingeConstraint(rbA, pivotInA, axisInA, useReferenceFrameA) { - m_accumulatedAngle=getHingeAngle(); - } - - btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false) - :btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA ) - { - m_accumulatedAngle=getHingeAngle(); + m_accumulatedAngle = getHingeAngle(); } - btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false) - :btHingeConstraint(rbA,rbAFrame, useReferenceFrameA ) + btHingeAccumulatedAngleConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false) + : btHingeConstraint(rbA, rbB, rbAFrame, rbBFrame, useReferenceFrameA) { - m_accumulatedAngle=getHingeAngle(); + m_accumulatedAngle = getHingeAngle(); + } + + btHingeAccumulatedAngleConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false) + : btHingeConstraint(rbA, rbAFrame, useReferenceFrameA) + { + m_accumulatedAngle = getHingeAngle(); } btScalar getAccumulatedHingeAngle(); - void setAccumulatedHingeAngle(btScalar accAngle); - virtual void getInfo1 (btConstraintInfo1* info); - + void setAccumulatedHingeAngle(btScalar accAngle); + virtual void getInfo1(btConstraintInfo1 * info); }; -struct btHingeConstraintFloatData +struct btHingeConstraintFloatData { - btTypedConstraintData m_typeConstraintData; - btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTypedConstraintData m_typeConstraintData; + btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. btTransformFloatData m_rbBFrame; - int m_useReferenceFrameA; - int m_angularOnly; - - int m_enableAngularMotor; - float m_motorTargetVelocity; - float m_maxMotorImpulse; + int m_useReferenceFrameA; + int m_angularOnly; - float m_lowerLimit; - float m_upperLimit; - float m_limitSoftness; - float m_biasFactor; - float m_relaxationFactor; + int m_enableAngularMotor; + float m_motorTargetVelocity; + float m_maxMotorImpulse; + float m_lowerLimit; + float m_upperLimit; + float m_limitSoftness; + float m_biasFactor; + float m_relaxationFactor; }; - - ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btHingeConstraintDoubleData2 +struct btHingeConstraintDoubleData2 { - btTypedConstraintDoubleData m_typeConstraintData; - btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTypedConstraintDoubleData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. btTransformDoubleData m_rbBFrame; - int m_useReferenceFrameA; - int m_angularOnly; - int m_enableAngularMotor; - double m_motorTargetVelocity; - double m_maxMotorImpulse; - - double m_lowerLimit; - double m_upperLimit; - double m_limitSoftness; - double m_biasFactor; - double m_relaxationFactor; - char m_padding1[4]; + int m_useReferenceFrameA; + int m_angularOnly; + int m_enableAngularMotor; + double m_motorTargetVelocity; + double m_maxMotorImpulse; + double m_lowerLimit; + double m_upperLimit; + double m_limitSoftness; + double m_biasFactor; + double m_relaxationFactor; + char m_padding1[4]; }; - - - -SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const { return sizeof(btHingeConstraintData); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer; - btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer); + btTypedConstraint::serialize(&hingeData->m_typeConstraintData, serializer); m_rbAFrame.serialize(hingeData->m_rbAFrame); m_rbBFrame.serialize(hingeData->m_rbBFrame); @@ -475,7 +452,7 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse); hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity); hingeData->m_useReferenceFrameA = m_useReferenceFrameA; -#ifdef _BT_USE_CENTER_LIMIT_ +#ifdef _BT_USE_CENTER_LIMIT_ hingeData->m_lowerLimit = float(m_limit.getLow()); hingeData->m_upperLimit = float(m_limit.getHigh()); hingeData->m_limitSoftness = float(m_limit.getSoftness()); @@ -500,4 +477,4 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS return btHingeConstraintDataName; } -#endif //BT_HINGECONSTRAINT_H +#endif //BT_HINGECONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index 125580d19..438456fe5 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -18,7 +18,6 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" - //notes: // Another memory optimization would be to store m_1MinvJt in the remaining 3 w components // which makes the btJacobianEntry memory layout 16 bytes @@ -27,25 +26,26 @@ subject to the following restrictions: /// 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 -ATTRIBUTE_ALIGNED16(class) btJacobianEntry +ATTRIBUTE_ALIGNED16(class) +btJacobianEntry { public: - btJacobianEntry() {}; + btJacobianEntry(){}; //constraint between two different rigidbodies btJacobianEntry( const btMatrix3x3& world2A, const btMatrix3x3& world2B, - const btVector3& rel_pos1,const btVector3& rel_pos2, + const btVector3& rel_pos1, const btVector3& rel_pos2, const btVector3& jointAxis, - const btVector3& inertiaInvA, + const btVector3& inertiaInvA, const btScalar massInvA, const btVector3& inertiaInvB, const btScalar massInvB) - :m_linearJointAxis(jointAxis) + : 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_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); @@ -54,33 +54,31 @@ public: //angular constraint between two different rigidbodies btJacobianEntry(const btVector3& jointAxis, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - const btVector3& inertiaInvA, - const btVector3& inertiaInvB) - :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + const btVector3& inertiaInvA, + const btVector3& inertiaInvB) + : m_linearJointAxis(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))) { - m_aJ= world2A*jointAxis; - m_bJ = world2B*-jointAxis; - m_0MinvJt = inertiaInvA * m_aJ; + 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); + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); btAssert(m_Adiag > btScalar(0.0)); } //angular constraint between two different rigidbodies btJacobianEntry(const btVector3& axisInA, - const btVector3& axisInB, - const btVector3& inertiaInvA, - const btVector3& inertiaInvB) - : m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) - , m_aJ(axisInA) - , m_bJ(-axisInB) + const btVector3& axisInB, + const btVector3& inertiaInvA, + const btVector3& inertiaInvB) + : m_linearJointAxis(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))), m_aJ(axisInA), m_bJ(-axisInB) { - m_0MinvJt = inertiaInvA * m_aJ; + m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); btAssert(m_Adiag > btScalar(0.0)); } @@ -88,25 +86,25 @@ public: //constraint on one rigidbody btJacobianEntry( const btMatrix3x3& world2A, - const btVector3& rel_pos1,const btVector3& rel_pos2, + const btVector3& rel_pos1, const btVector3& rel_pos2, const btVector3& jointAxis, - const btVector3& inertiaInvA, + const btVector3& inertiaInvA, const btScalar massInvA) - :m_linearJointAxis(jointAxis) + : m_linearJointAxis(jointAxis) { - m_aJ= world2A*(rel_pos1.cross(jointAxis)); - m_bJ = world2A*(rel_pos2.cross(-jointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); + m_aJ = world2A * (rel_pos1.cross(jointAxis)); + m_bJ = world2A * (rel_pos2.cross(-jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = btVector3(btScalar(0.), btScalar(0.), btScalar(0.)); m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); btAssert(m_Adiag > btScalar(0.0)); } - btScalar getDiagonal() const { return m_Adiag; } + btScalar getDiagonal() const { return m_Adiag; } // for two constraints on the same rigidbody (for example vehicle friction) - btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const + btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const { const btJacobianEntry& jacA = *this; btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); @@ -114,42 +112,39 @@ public: return lin + ang; } - - // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) - btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const + btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA, const btScalar massInvB) const { const btJacobianEntry& jacA = *this; btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; - btVector3 lin0 = massInvA * lin ; + btVector3 lin0 = massInvA * lin; btVector3 lin1 = massInvB * lin; - btVector3 sum = ang0+ang1+lin0+lin1; - return sum[0]+sum[1]+sum[2]; + btVector3 sum = ang0 + ang1 + lin0 + lin1; + return sum[0] + sum[1] + sum[2]; } - btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) + btScalar getRelativeVelocity(const btVector3& linvelA, const btVector3& angvelA, const btVector3& linvelB, const btVector3& angvelB) { btVector3 linrel = linvelA - linvelB; - btVector3 angvela = angvelA * m_aJ; - btVector3 angvelb = angvelB * m_bJ; + btVector3 angvela = angvelA * m_aJ; + btVector3 angvelb = angvelB * m_bJ; linrel *= m_linearJointAxis; angvela += angvelb; angvela += linrel; - btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2]; + btScalar rel_vel2 = angvela[0] + angvela[1] + angvela[2]; return rel_vel2 + SIMD_EPSILON; } -//private: + //private: - btVector3 m_linearJointAxis; - btVector3 m_aJ; - btVector3 m_bJ; - btVector3 m_0MinvJt; - btVector3 m_1MinvJt; + btVector3 m_linearJointAxis; + btVector3 m_aJ; + btVector3 m_bJ; + btVector3 m_0MinvJt; + btVector3 m_1MinvJt; //Optimization: can be stored in the w/last component of one of the vectors - btScalar m_Adiag; - + btScalar m_Adiag; }; -#endif //BT_JACOBIAN_ENTRY_H +#endif //BT_JACOBIAN_ENTRY_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp index f3979be35..ccf891604 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp @@ -15,14 +15,9 @@ subject to the following restrictions: #include "btNNCGConstraintSolver.h" - - - - - -btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); + btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size()); m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size()); @@ -37,38 +32,39 @@ btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject* return val; } -btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/) +btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/) { - int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { - if (1) // uncomment this for a bit less random ((iteration & 7) == 0) + if (1) // uncomment this for a bit less random ((iteration & 7) == 0) { - - for (int j=0; j0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; - if (beta>1) + btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; + if (beta > 1) { - for (int j=0;jisEnabled()) { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + 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); + constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep); } } @@ -147,203 +141,206 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) { int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; + int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1; - for (int c=0;cbtScalar(0)) + if (totalImpulse > btScalar(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[c*multiplier] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[c*multiplier] = 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[c * multiplier] = deltaf; + deltaflengthsqr += deltaf * deltaf; + } + else + { + m_deltafCF[c * multiplier] = 0; } } if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]]; - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; - - if (totalImpulse>btScalar(0)) + if (totalImpulse > btScalar(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[c*multiplier+1] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[c*multiplier+1] = 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[c * multiplier + 1] = deltaf; + deltaflengthsqr += deltaf * deltaf; + } + else + { + m_deltafCF[c * multiplier + 1] = 0; } } } } - } - else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + else //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;jbtScalar(0)) + if (totalImpulse > btScalar(0)) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + 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); + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); m_deltafCF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { + deltaflengthsqr += deltaf * deltaf; + } + else + { m_deltafCF[j] = 0; } } } - { + { int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (int j=0;jbtScalar(0)) + if (totalImpulse > btScalar(0)) { - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + 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); + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); m_deltafCRF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { + deltaflengthsqr += deltaf * deltaf; + } + else + { m_deltafCRF[j] = 0; } } - } - + } } - - - } - - - - if (!m_onlyForNoneContact) + if (!m_onlyForNoneContact) { - if (iteration==0) + if (iteration == 0) { - for (int j=0;j0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; - if (beta>1) { - for (int j=0;j 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; + if (beta > 1) + { + for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = 0; + for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++) m_pC[j] = 0; + for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++) m_pCF[j] = 0; + for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = 0; + } + else + { + for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) { btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; - if (iteration < constraint.m_overrideNumSolverIterations) { + if (iteration < constraint.m_overrideNumSolverIterations) + { btScalar additionaldeltaimpulse = beta * m_pNC[j]; constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j]; btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; const btSolverConstraint& c = constraint; - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); + body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse); + body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse); } } - for (int j=0;j m_pNC; // p for None Contact constraints - btAlignedObjectArray m_pC; // p for Contact constraints - btAlignedObjectArray m_pCF; // p for ContactFriction constraints - btAlignedObjectArray m_pCRF; // p for ContactRollingFriction constraints + btAlignedObjectArray m_pNC; // p for None Contact constraints + btAlignedObjectArray m_pC; // p for Contact constraints + btAlignedObjectArray m_pCF; // p for ContactFriction constraints + btAlignedObjectArray m_pCRF; // p for ContactRollingFriction constraints //These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration. - btAlignedObjectArray m_deltafNC; // deltaf for NoneContact constraints - btAlignedObjectArray m_deltafC; // deltaf for Contact constraints - btAlignedObjectArray m_deltafCF; // deltaf for ContactFriction constraints - btAlignedObjectArray m_deltafCRF; // deltaf for ContactRollingFriction constraints + btAlignedObjectArray m_deltafNC; // deltaf for NoneContact constraints + btAlignedObjectArray m_deltafC; // deltaf for Contact constraints + btAlignedObjectArray m_deltafCF; // deltaf for ContactFriction constraints + btAlignedObjectArray m_deltafCRF; // deltaf for ContactRollingFriction constraints - protected: + 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); - 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); - - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {} @@ -57,8 +54,4 @@ public: bool m_onlyForNoneContact; }; - - - -#endif //BT_NNCG_CONSTRAINT_SOLVER_H - +#endif //BT_NNCG_CONSTRAINT_SOLVER_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index 3c0430b90..ad399dc57 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -13,217 +13,193 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btPoint2PointConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include - - - - -btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), -m_flags(0), -m_useSolveConstraintObsolete(false) +btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB) + : btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0), m_useSolveConstraintObsolete(false) { - } - -btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), -m_flags(0), -m_useSolveConstraintObsolete(false) +btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, const btVector3& pivotInA) + : btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA), m_pivotInA(pivotInA), m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_flags(0), m_useSolveConstraintObsolete(false) { - } -void btPoint2PointConstraint::buildJacobian() +void btPoint2PointConstraint::buildJacobian() { - ///we need it for both methods { m_appliedImpulse = btScalar(0.); - btVector3 normal(0,0,0); + btVector3 normal(0, 0, 0); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { normal[i] = 1; new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), - normal, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - normal[i] = 0; + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform() * m_pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform() * m_pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + normal[i] = 0; } } - - } -void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) +void btPoint2PointConstraint::getInfo1(btConstraintInfo1* info) { getInfo1NonVirtual(info); } -void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info) +void btPoint2PointConstraint::getInfo1NonVirtual(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; info->nub = 0; - } else + } + else { info->m_numConstraintRows = 3; info->nub = 3; } } - - - -void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) +void btPoint2PointConstraint::getInfo2(btConstraintInfo2* info) { - getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); } -void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans) +void btPoint2PointConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans) { btAssert(!m_useSolveConstraintObsolete); - //retrieve matrices + //retrieve matrices // anchor points in global coordinates with respect to body PORs. - - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - btVector3 a1 = body0_trans.getBasis()*getPivotInA(); + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip + 1] = 1; + info->m_J1linearAxis[2 * info->rowskip + 2] = 1; + + btVector3 a1 = body0_trans.getBasis() * getPivotInA(); { btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip); btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2); } - + info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; - - btVector3 a2 = body1_trans.getBasis()*getPivotInB(); - + info->m_J2linearAxis[info->rowskip + 1] = -1; + info->m_J2linearAxis[2 * info->rowskip + 2] = -1; + + btVector3 a2 = body1_trans.getBasis() * getPivotInB(); + { - // btVector3 a2n = -a2; + // btVector3 a2n = -a2; btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip); + a2.getSkewSymmetricMatrix(angular0, angular1, angular2); } - - - // set right hand side + // set right hand side btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp; - btScalar 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 & BT_P2P_FLAGS_CFM) + btScalar k = info->fps * currERP; + int j; + for (j = 0; j < 3; 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 & BT_P2P_FLAGS_CFM) + { + for (j = 0; j < 3; j++) { - info->cfm[j*info->rowskip] = m_cfm; + info->cfm[j * info->rowskip] = m_cfm; } } - btScalar impulseClamp = m_setting.m_impulseClamp;// - for (j=0; j<3; j++) - { + btScalar impulseClamp = m_setting.m_impulseClamp; // + for (j = 0; j < 3; j++) + { if (m_setting.m_impulseClamp > 0) { - info->m_lowerLimit[j*info->rowskip] = -impulseClamp; - info->m_upperLimit[j*info->rowskip] = impulseClamp; + info->m_lowerLimit[j * info->rowskip] = -impulseClamp; + info->m_upperLimit[j * info->rowskip] = impulseClamp; } } info->m_damping = m_setting.m_damping; - } - - -void btPoint2PointConstraint::updateRHS(btScalar timeStep) +void btPoint2PointConstraint::updateRHS(btScalar timeStep) { (void)timeStep; - } -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///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 btPoint2PointConstraint::setParam(int num, btScalar value, int axis) { - if(axis != -1) + if (axis != -1) { btAssertConstrParams(0); } else { - switch(num) + switch (num) { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - m_erp = value; + case BT_CONSTRAINT_ERP: + case BT_CONSTRAINT_STOP_ERP: + m_erp = value; m_flags |= BT_P2P_FLAGS_ERP; break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - m_cfm = value; + case BT_CONSTRAINT_CFM: + case BT_CONSTRAINT_STOP_CFM: + m_cfm = value; m_flags |= BT_P2P_FLAGS_CFM; break; - default: + default: btAssertConstrParams(0); } } } ///return the local value of parameter -btScalar btPoint2PointConstraint::getParam(int num, int axis) const +btScalar btPoint2PointConstraint::getParam(int num, int axis) const { btScalar retVal(SIMD_INFINITY); - if(axis != -1) + if (axis != -1) { btAssertConstrParams(0); } else { - switch(num) + switch (num) { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : + case BT_CONSTRAINT_ERP: + case BT_CONSTRAINT_STOP_ERP: btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP); - retVal = m_erp; + retVal = m_erp; break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : + case BT_CONSTRAINT_CFM: + case BT_CONSTRAINT_STOP_CFM: btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM); - retVal = m_cfm; + retVal = m_cfm; break; - default: + default: btAssertConstrParams(0); } } return retVal; } - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index 8fa03d719..4717e1980 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -22,26 +22,24 @@ subject to the following restrictions: class btRigidBody; - #ifdef BT_USE_DOUBLE_PRECISION -#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2 -#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2" +#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2 +#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2" #else -#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData -#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData" -#endif //BT_USE_DOUBLE_PRECISION +#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData +#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION -struct btConstraintSetting +struct btConstraintSetting { - btConstraintSetting() : - m_tau(btScalar(0.3)), - m_damping(btScalar(1.)), - m_impulseClamp(btScalar(0.)) + btConstraintSetting() : m_tau(btScalar(0.3)), + m_damping(btScalar(1.)), + m_impulseClamp(btScalar(0.)) { } - btScalar m_tau; - btScalar m_damping; - btScalar m_impulseClamp; + btScalar m_tau; + btScalar m_damping; + btScalar m_impulseClamp; }; enum btPoint2PointFlags @@ -51,52 +49,51 @@ enum btPoint2PointFlags }; /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space -ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) +btPoint2PointConstraint : public btTypedConstraint { #ifdef IN_PARALLELL_SOLVER public: #endif - btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - - btVector3 m_pivotInA; - btVector3 m_pivotInB; - - int m_flags; - btScalar m_erp; - btScalar m_cfm; - -public: + btJacobianEntry m_jac[3]; //3 orthogonal linear constraints + btVector3 m_pivotInA; + btVector3 m_pivotInB; + + int m_flags; + btScalar m_erp; + btScalar m_cfm; + +public: BT_DECLARE_ALIGNED_ALLOCATOR(); ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; + bool m_useSolveConstraintObsolete; - btConstraintSetting m_setting; + btConstraintSetting m_setting; - btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB); + btPoint2PointConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB); - btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA); + btPoint2PointConstraint(btRigidBody & rbA, const btVector3& pivotInA); + virtual void buildJacobian(); - virtual void buildJacobian(); + virtual void getInfo1(btConstraintInfo1 * info); - virtual void getInfo1 (btConstraintInfo1* info); + void getInfo1NonVirtual(btConstraintInfo1 * info); - void getInfo1NonVirtual (btConstraintInfo1* info); + virtual void getInfo2(btConstraintInfo2 * info); - virtual void getInfo2 (btConstraintInfo2* info); + void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& body0_trans, const btTransform& body1_trans); - void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans); + void updateRHS(btScalar timeStep); - void updateRHS(btScalar timeStep); - - void setPivotA(const btVector3& pivotA) + void setPivotA(const btVector3& pivotA) { m_pivotInA = pivotA; } - void setPivotB(const btVector3& pivotB) + void setPivotB(const btVector3& pivotB) { m_pivotInB = pivotB; } @@ -111,70 +108,66 @@ public: return m_pivotInB; } - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///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 void setParam(int num, btScalar value, int axis = -1); ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; - - virtual int getFlags() const - { - return m_flags; - } + virtual btScalar getParam(int num, int axis = -1) const; - virtual int calculateSerializeBufferSize() const; + virtual int getFlags() const + { + return m_flags; + } + + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btPoint2PointConstraintFloatData +struct btPoint2PointConstraintFloatData { - btTypedConstraintData m_typeConstraintData; - btVector3FloatData m_pivotInA; - btVector3FloatData m_pivotInB; + btTypedConstraintData m_typeConstraintData; + btVector3FloatData m_pivotInA; + btVector3FloatData m_pivotInB; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btPoint2PointConstraintDoubleData2 +struct btPoint2PointConstraintDoubleData2 { - btTypedConstraintDoubleData m_typeConstraintData; - btVector3DoubleData m_pivotInA; - btVector3DoubleData m_pivotInB; + btTypedConstraintDoubleData m_typeConstraintData; + btVector3DoubleData m_pivotInA; + btVector3DoubleData m_pivotInB; }; #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ///this structure is not used, except for loading pre-2.82 .bullet files ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btPoint2PointConstraintDoubleData +struct btPoint2PointConstraintDoubleData { - btTypedConstraintData m_typeConstraintData; - btVector3DoubleData m_pivotInA; - btVector3DoubleData m_pivotInB; + btTypedConstraintData m_typeConstraintData; + btVector3DoubleData m_pivotInA; + btVector3DoubleData m_pivotInB; }; -#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION +#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION - -SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const { return sizeof(btPoint2PointConstraintData2); - } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer; - btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer); + btTypedConstraint::serialize(&p2pData->m_typeConstraintData, serializer); m_pivotInA.serialize(p2pData->m_pivotInA); m_pivotInB.serialize(p2pData->m_pivotInB); return btPoint2PointConstraintDataName; } -#endif //BT_POINT2POINTCONSTRAINT_H +#endif //BT_POINT2POINTCONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index b0d57a3e8..d3b71e458 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -33,24 +33,24 @@ subject to the following restrictions: //#include "btSolverBody.h" //#include "btSolverConstraint.h" #include "LinearMath/btAlignedObjectArray.h" -#include //for memset +#include //for memset -int gNumSplitImpulseRecoveries = 0; +int gNumSplitImpulseRecoveries = 0; #include "BulletDynamics/Dynamics/btRigidBody.h" //#define VERBOSE_RESIDUAL_PRINTF 1 ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check. -static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { - btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity()); // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; if (sum < c.m_lowerLimit) @@ -68,21 +68,20 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolver c.m_appliedImpulse = sum; } - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - return deltaImpulse; + return deltaImpulse * (1. / c.m_jacDiagABInv); } - -static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { - btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity()); - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; if (sum < c.m_lowerLimit) { @@ -93,70 +92,67 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSol { c.m_appliedImpulse = sum; } - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - return deltaImpulse; + return deltaImpulse * (1. / c.m_jacDiagABInv); } - - #ifdef USE_SIMD #include - -#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) -static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) +#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e)) +static inline __m128 btSimdDot3(__m128 vec0, __m128 vec1) { - __m128 result = _mm_mul_ps( vec0, vec1); - return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) ); + __m128 result = _mm_mul_ps(vec0, vec1); + return _mm_add_ps(btVecSplat(result, 0), _mm_add_ps(btVecSplat(result, 1), btVecSplat(result, 2))); } -#if defined (BT_ALLOW_SSE4) +#if defined(BT_ALLOW_SSE4) #include -#define USE_FMA 1 -#define USE_FMA3_INSTEAD_FMA4 1 -#define USE_SSE4_DOT 1 +#define USE_FMA 1 +#define USE_FMA3_INSTEAD_FMA4 1 +#define USE_SSE4_DOT 1 -#define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f) -#define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f)) +#define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f) +#define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f)) #if USE_SSE4_DOT -#define DOT_PRODUCT(a, b) SSE4_DP(a, b) +#define DOT_PRODUCT(a, b) SSE4_DP(a, b) #else -#define DOT_PRODUCT(a, b) btSimdDot3(a, b) +#define DOT_PRODUCT(a, b) btSimdDot3(a, b) #endif #if USE_FMA #if USE_FMA3_INSTEAD_FMA4 // a*b + c -#define FMADD(a, b, c) _mm_fmadd_ps(a, b, c) +#define FMADD(a, b, c) _mm_fmadd_ps(a, b, c) // -(a*b) + c -#define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c) -#else // USE_FMA3 +#define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c) +#else // USE_FMA3 // a*b + c -#define FMADD(a, b, c) _mm_macc_ps(a, b, c) +#define FMADD(a, b, c) _mm_macc_ps(a, b, c) // -(a*b) + c -#define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c) +#define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c) #endif -#else // USE_FMA +#else // USE_FMA // c + a*b -#define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b)) +#define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b)) // c - a*b -#define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b)) +#define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b)) #endif #endif // Project Gauss Seidel or the equivalent Sequential Impulse -static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { __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 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); btSimdScalar 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(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); @@ -169,54 +165,52 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, __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_contactNormal1.mVec128, body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.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_add_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)); - return deltaImpulse; + bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); + return deltaImpulse.m_floats[0] / c.m_jacDiagABInv; } - // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 -static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { -#if defined (BT_ALLOW_SSE4) - __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); - __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); - const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); - const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit); - const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); - deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); - deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); - tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum - const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit); - const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp); - deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower); - c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower); - body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128); - body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); - body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); - body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); - return deltaImpulse; +#if defined(BT_ALLOW_SSE4) + __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); + __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm); + const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); + const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit); + const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); + deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); + deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); + tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum + const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit); + const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp); + deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower); + c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower); + bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128); + bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128); + bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128); + bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128); + btSimdScalar deltaImp = deltaImpulse; + return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv); #else - return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c); + return gResolveSingleConstraintRowGeneric_sse2(bodyA, bodyB, c); #endif } - - -static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { __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 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); btSimdScalar 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(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); @@ -226,103 +220,98 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bod __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_contactNormal1.mVec128, body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.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_add_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)); - return deltaImpulse; + bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); + return deltaImpulse.m_floats[0] / c.m_jacDiagABInv; } - // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 -static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { #ifdef BT_ALLOW_SSE4 - __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); - __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); - const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); - const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); - deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); - deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); - tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); - const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit); - deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask); - c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask); - body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128); - body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); - body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); - body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); - return deltaImpulse; + __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); + __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm); + const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); + const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); + deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); + deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); + tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); + const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit); + deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask); + c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask); + bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128); + bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128); + bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128); + bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128); + btSimdScalar deltaImp = deltaImpulse; + return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv); #else - return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c); -#endif //BT_ALLOW_SSE4 + return gResolveSingleConstraintRowLowerLimit_sse2(bodyA, bodyB, c); +#endif //BT_ALLOW_SSE4 } +#endif //USE_SIMD -#endif //USE_SIMD - - - -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { - return m_resolveSingleConstraintRowGeneric(body1, body2, c); + return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c); } // Project Gauss Seidel or the equivalent Sequential Impulse -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { - return m_resolveSingleConstraintRowGeneric(body1, body2, c); + return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c); } -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { - return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); + return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c); } - -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { - return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); + return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c); } - -static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& c) +static btScalar gResolveSplitPenetrationImpulse_scalar_reference( + btSolverBody& bodyA, + btSolverBody& bodyB, + const btSolverConstraint& c) { btScalar deltaImpulse = 0.f; - if (c.m_rhsPenetration) - { - gNumSplitImpulseRecoveries++; - deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); + if (c.m_rhsPenetration) + { + gNumSplitImpulseRecoveries++; + deltaImpulse = c.m_rhsPenetration - btScalar(c.m_appliedPushImpulse) * c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity()); - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(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_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - } - return deltaImpulse; + deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; + const btScalar sum = btScalar(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; + } + bodyA.internalApplyPushImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyB.internalApplyPushImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + } + return deltaImpulse * (1. / c.m_jacDiagABInv); } -static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) @@ -331,112 +320,126 @@ static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btS gNumSplitImpulseRecoveries++; __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(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128); + __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(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetTurnVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetTurnVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); + btSimdScalar resultLowerLess, resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum, upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp); + deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse)); + c.m_appliedPushImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum)); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.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_add_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)); - return deltaImpulse; + bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); + btSimdScalar deltaImp = deltaImpulse; + return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv); #else - return gResolveSplitPenetrationImpulse_scalar_reference(body1,body2,c); + return gResolveSplitPenetrationImpulse_scalar_reference(bodyA, bodyB, c); #endif } - btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() { - m_btSeed2 = 0; - m_cachedSolverMode = 0; - setupSolverFunctions( false ); + m_btSeed2 = 0; + m_cachedSolverMode = 0; + setupSolverFunctions(false); } -void btSequentialImpulseConstraintSolver::setupSolverFunctions( bool useSimd ) +void btSequentialImpulseConstraintSolver::setupSolverFunctions(bool useSimd) { - m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference; - m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference; - m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference; + m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference; + m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference; + m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference; - if ( useSimd ) - { + if (useSimd) + { #ifdef USE_SIMD - m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2; - m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2; - m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2; + m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2; + m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2; + m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2; #ifdef BT_ALLOW_SSE4 - int cpuFeatures = btCpuFeatureUtility::getCpuFeatures(); - if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1)) - { - m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3; - m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; - } -#endif//BT_ALLOW_SSE4 -#endif //USE_SIMD - } + int cpuFeatures = btCpuFeatureUtility::getCpuFeatures(); + if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1)) + { + m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3; + m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; + } +#endif //BT_ALLOW_SSE4 +#endif //USE_SIMD + } } - btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() - { - } +btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() +{ +} - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric() - { - return gResolveSingleConstraintRowGeneric_scalar_reference; - } +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric() +{ + return gResolveSingleConstraintRowGeneric_scalar_reference; +} + +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit() +{ + return gResolveSingleConstraintRowLowerLimit_scalar_reference; +} + +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarSplitPenetrationImpulseGeneric() +{ + return gResolveSplitPenetrationImpulse_scalar_reference; +} + +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2SplitPenetrationImpulseGeneric() +{ + return gResolveSplitPenetrationImpulse_sse2; +} - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit() - { - return gResolveSingleConstraintRowLowerLimit_scalar_reference; - } #ifdef USE_SIMD - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric() - { - return gResolveSingleConstraintRowGeneric_sse2; - } - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit() - { - return gResolveSingleConstraintRowLowerLimit_sse2; - } +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric() +{ + return gResolveSingleConstraintRowGeneric_sse2; +} +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit() +{ + return gResolveSingleConstraintRowLowerLimit_sse2; +} #ifdef BT_ALLOW_SSE4 - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric() - { - return gResolveSingleConstraintRowGeneric_sse4_1_fma3; - } - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit() - { - return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; - } -#endif //BT_ALLOW_SSE4 -#endif //USE_SIMD +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric() +{ + return gResolveSingleConstraintRowGeneric_sse4_1_fma3; +} +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit() +{ + return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; +} +#endif //BT_ALLOW_SSE4 +#endif //USE_SIMD unsigned long btSequentialImpulseConstraintSolver::btRand2() { - m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; + m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff; return m_btSeed2; } - - +unsigned long btSequentialImpulseConstraintSolver::btRand2a(unsigned long& seed) +{ + seed = (1664525L * seed + 1013904223L) & 0xffffffff; + return seed; +} //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) -int btSequentialImpulseConstraintSolver::btRandInt2 (int n) +int btSequentialImpulseConstraintSolver::btRandInt2(int n) { // seems good; xor-fold and modulus const unsigned long un = static_cast(n); @@ -444,15 +447,20 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) // 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) { + if (un <= 0x00010000UL) + { r ^= (r >> 16); - if (un <= 0x00000100UL) { + if (un <= 0x00000100UL) + { r ^= (r >> 8); - if (un <= 0x00000010UL) { + if (un <= 0x00000010UL) + { r ^= (r >> 4); - if (un <= 0x00000004UL) { + if (un <= 0x00000004UL) + { r ^= (r >> 2); - if (un <= 0x00000002UL) { + if (un <= 0x00000002UL) + { r ^= (r >> 1); } } @@ -460,70 +468,62 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) } } - return (int) (r % un); + return (int)(r % un); } - - -void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep) +int btSequentialImpulseConstraintSolver::btRandInt2a(int n, unsigned long& seed) { + // seems good; xor-fold and modulus + const unsigned long un = static_cast(n); + unsigned long r = btSequentialImpulseConstraintSolver::btRand2a(seed); - btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; - - solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetDeltaAngularVelocity().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) + // 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) { - solverBody->m_worldTransform = rb->getWorldTransform(); - solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor()); - solverBody->m_originalBody = rb; - solverBody->m_angularFactor = rb->getAngularFactor(); - solverBody->m_linearFactor = rb->getLinearFactor(); - solverBody->m_linearVelocity = rb->getLinearVelocity(); - solverBody->m_angularVelocity = rb->getAngularVelocity(); - solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep; - solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ; - - } else - { - solverBody->m_worldTransform.setIdentity(); - solverBody->internalSetInvMass(btVector3(0,0,0)); - solverBody->m_originalBody = 0; - 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); - solverBody->m_externalForceImpulse.setValue(0,0,0); - solverBody->m_externalTorqueImpulse.setValue(0,0,0); + 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 btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep) +{ + btSISolverSingleIterationData::initSolverBody(solverBody, collisionObject, timeStep); +} - - - - -btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold) +btScalar btSequentialImpulseConstraintSolver::restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold) { //printf("rel_vel =%f\n", rel_vel); - if (btFabs(rel_vel)hasAnisotropicFriction(frictionMode)) { // transform to local coordinates @@ -534,21 +534,15 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb // ... and transform it back to global coordinates frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; } - } - - - -void btSequentialImpulseConstraintSolver::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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +void btSequentialImpulseConstraintSolver::setupFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { + btSolverBody& solverBodyA = tmpSolverBodyPool[solverBodyIdA]; + btSolverBody& solverBodyB = tmpSolverBodyPool[solverBodyIdB]; - - btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; - - btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; + btRigidBody* body0 = tmpSolverBodyPool[solverBodyIdA].m_originalBody; + btRigidBody* bodyA = tmpSolverBodyPool[solverBodyIdB].m_originalBody; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -564,21 +558,23 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_contactNormal1 = normalAxis; btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor(); - }else + solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor(); + } + else { solverConstraint.m_contactNormal1.setZero(); solverConstraint.m_relpos1CrossNormal.setZero(); - solverConstraint.m_angularComponentA .setZero(); + solverConstraint.m_angularComponentA.setZero(); } - if (body1) + if (bodyA) { solverConstraint.m_contactNormal2 = -normalAxis; btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor(); - } else + solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor(); + } + else { solverConstraint.m_contactNormal2.setZero(); solverConstraint.m_relpos2CrossNormal.setZero(); @@ -591,32 +587,28 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr btScalar denom1 = 0.f; if (body0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); denom0 = body0->getInvMass() + normalAxis.dot(vec); } - if (body1) + if (bodyA) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = body1->getInvMass() + normalAxis.dot(vec); + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = bodyA->getInvMass() + normalAxis.dot(vec); } - btScalar denom = relaxation/(denom0+denom1); + btScalar denom = relaxation / (denom0 + denom1); solverConstraint.m_jacDiagABInv = denom; } { - - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : btVector3(0, 0, 0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0)); - rel_vel = vel1Dotn+vel2Dotn; + rel_vel = vel1Dotn + vel2Dotn; -// btScalar positionalError = 0.f; + // btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel; + btScalar velocityError = desiredVelocity - rel_vel; btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; btScalar penetrationImpulse = btScalar(0); @@ -624,8 +616,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) { btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis); - btScalar positionalError = -distance * infoGlobal.m_frictionERP/infoGlobal.m_timeStep; - penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar positionalError = -distance * infoGlobal.m_frictionERP / infoGlobal.m_timeStep; + penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; } solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; @@ -633,42 +625,56 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; - } } -btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +void btSequentialImpulseConstraintSolver::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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { - btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); + btSequentialImpulseConstraintSolver::setupFrictionConstraintInternal(m_tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); +} + +btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, 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, btScalar cfmSlip) +{ + btSolverConstraint& solverConstraint = tmpSolverContactFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); + setupFrictionConstraintInternal(tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); return solverConstraint; } -void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, - btScalar desiredVelocity, btScalar cfmSlip) + +btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); + return solverConstraint; +} + + +void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity, btScalar cfmSlip) { - btVector3 normalAxis(0,0,0); - + btVector3 normalAxis(0, 0, 0); solverConstraint.m_contactNormal1 = normalAxis; solverConstraint.m_contactNormal2 = -normalAxis; - btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; + btSolverBody& solverBodyA = tmpSolverBodyPool[solverBodyIdA]; + btSolverBody& solverBodyB = tmpSolverBodyPool[solverBodyIdB]; - btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; + btRigidBody* body0 = tmpSolverBodyPool[solverBodyIdA].m_originalBody; + btRigidBody* bodyA = tmpSolverBodyPool[solverBodyIdB].m_originalBody; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_friction = combinedTorsionalFriction; - solverConstraint.m_originalContactPoint = 0; + solverConstraint.m_friction = combinedTorsionalFriction; + solverConstraint.m_originalContactPoint = 0; solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; @@ -676,134 +682,149 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo { btVector3 ftorqueAxis1 = -normalAxis1; solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor() : btVector3(0, 0, 0); } { btVector3 ftorqueAxis1 = normalAxis1; solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor() : btVector3(0, 0, 0); } - { - btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); - btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); + btVector3 iMJaA = body0 ? body0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0); + btVector3 iMJaB = bodyA ? bodyA->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0); btScalar sum = 0; sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; + solverConstraint.m_jacDiagABInv = btScalar(1.) / sum; } { - - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : btVector3(0, 0, 0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0)); - rel_vel = vel1Dotn+vel2Dotn; + rel_vel = vel1Dotn + vel2Dotn; -// btScalar positionalError = 0.f; + // btScalar positionalError = 0.f; - btSimdScalar velocityError = desiredVelocity - rel_vel; - btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); + btSimdScalar velocityError = desiredVelocity - rel_vel; + btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; - } } +void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity, btScalar cfmSlip) - - - - - -btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { - btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); + setupTorsionalFrictionConstraintInternal(m_tmpSolverBodyPool, solverConstraint, normalAxis1, solverBodyIdA, solverBodyIdB, + cp, combinedTorsionalFriction, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, + desiredVelocity, cfmSlip); + +} + +btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +{ + btSolverConstraint& solverConstraint = tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); + setupTorsionalFrictionConstraintInternal(tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } -int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep) +btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +{ + btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); + return solverConstraint; +} + +int btSISolverSingleIterationData::getOrInitSolverBody(btCollisionObject & body, btScalar timeStep) { #if BT_THREADSAFE - int solverBodyId = -1; - if ( !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 ) - { - if ( btRigidBody* rb = btRigidBody::upcast( &body ) ) - { - solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody( &solverBody, &body, timeStep ); - body.setCompanionId( solverBodyId ); - } - } - } - else if (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 (uniqueId >= 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 - btRigidBody* rb = btRigidBody::upcast( &body ); - solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody( &solverBody, &body, timeStep ); - m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId; - } - } - else - { - // 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 < m_tmpSolverBodyPool.size() ); + 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) + { + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody, &body, timeStep); + body.setCompanionId(solverBodyId); + } + } + 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 (uniqueId >= 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 +#else // BT_THREADSAFE - int solverBodyIdA = -1; + int solverBodyIdA = -1; if (body.getCompanionId() >= 0) { //body has already been converted solverBodyIdA = body.getCompanionId(); - btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); - } else + btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); + } + else { btRigidBody* rb = btRigidBody::upcast(&body); //convert both active and kinematic objects (for their velocity) @@ -811,249 +832,481 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& { solverBodyIdA = m_tmpSolverBodyPool.size(); btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,&body,timeStep); + initSolverBody(&solverBody, &body, timeStep); body.setCompanionId(solverBodyIdA); - } else + } + else { - - if (m_fixedBodyId<0) + if (m_fixedBodyId < 0) { m_fixedBodyId = m_tmpSolverBodyPool.size(); btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&fixedBody,0,timeStep); + initSolverBody(&fixedBody, 0, timeStep); } return m_fixedBodyId; -// return 0;//assume first one is a fixed solver body + // return 0;//assume first one is a fixed solver body } } return solverBodyIdA; -#endif // BT_THREADSAFE +#endif // BT_THREADSAFE +} +void btSISolverSingleIterationData::initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep) +{ + btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0; + solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetDeltaAngularVelocity().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 = rb->getWorldTransform(); + solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor()); + solverBody->m_originalBody = rb; + solverBody->m_angularFactor = rb->getAngularFactor(); + solverBody->m_linearFactor = rb->getLinearFactor(); + solverBody->m_linearVelocity = rb->getLinearVelocity(); + solverBody->m_angularVelocity = rb->getAngularVelocity(); + solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep; + solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep; + } + else + { + solverBody->m_worldTransform.setIdentity(); + solverBody->internalSetInvMass(btVector3(0, 0, 0)); + solverBody->m_originalBody = 0; + 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); + solverBody->m_externalForceImpulse.setValue(0, 0, 0); + solverBody->m_externalTorqueImpulse.setValue(0, 0, 0); + } +} + +int btSISolverSingleIterationData::getSolverBody(btCollisionObject& body) const +{ +#if BT_THREADSAFE + 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(); + btAssert(solverBodyId >= 0); + } + 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 (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size()) + { + m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID); + } + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId]; + btAssert(solverBodyId != INVALID_SOLVER_BODY_ID); + } + 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()); + } + btAssert(m_fixedBodyId >= 0); + solverBodyId = m_fixedBodyId; + } + btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size()); + return solverBodyId; +#else // BT_THREADSAFE + int solverBodyIdA = -1; + + if (body.getCompanionId() >= 0) + { + //body has already been converted + solverBodyIdA = body.getCompanionId(); + btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); + } + else + { + btRigidBody* rb = btRigidBody::upcast(&body); + //convert both active and kinematic objects (for their velocity) + if (rb && (rb->getInvMass() || rb->isKinematicObject())) + { + btAssert(0); + } + else + { + if (m_fixedBodyId < 0) + { + btAssert(0); + } + return m_fixedBodyId; + // return 0;//assume first one is a fixed solver body + } + } + + return solverBodyIdA; +#endif // BT_THREADSAFE +} + +int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body, btScalar timeStep) +{ +#if BT_THREADSAFE + 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) + { + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody, &body, timeStep); + body.setCompanionId(solverBodyId); + } + } + 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 (uniqueId >= 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; + + if (body.getCompanionId() >= 0) + { + //body has already been converted + solverBodyIdA = body.getCompanionId(); + btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); + } + else + { + btRigidBody* rb = btRigidBody::upcast(&body); + //convert both active and kinematic objects (for their velocity) + if (rb && (rb->getInvMass() || rb->isKinematicObject())) + { + solverBodyIdA = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody, &body, timeStep); + body.setCompanionId(solverBodyIdA); + } + else + { + if (m_fixedBodyId < 0) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody, 0, timeStep); + } + return m_fixedBodyId; + // return 0;//assume first one is a fixed solver body + } + } + + return solverBodyIdA; +#endif // BT_THREADSAFE } #include -void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - const btVector3& rel_pos1, const btVector3& rel_pos2) + +void btSequentialImpulseConstraintSolver::setupContactConstraintInternal(btSISolverSingleIterationData& siData, + btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + const btVector3& rel_pos1, const btVector3& rel_pos2) { + // const btVector3& pos1 = cp.getPositionWorldOnA(); + // const btVector3& pos2 = cp.getPositionWorldOnB(); - // const btVector3& pos1 = cp.getPositionWorldOnA(); - // const btVector3& pos2 = cp.getPositionWorldOnB(); + btSolverBody* bodyA = &siData.m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyB = &siData.m_tmpSolverBodyPool[solverBodyIdB]; - btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + btRigidBody* rb0 = bodyA->m_originalBody; + btRigidBody* rb1 = bodyB->m_originalBody; - btRigidBody* rb0 = bodyA->m_originalBody; - btRigidBody* rb1 = bodyB->m_originalBody; + // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); -// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); -// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + relaxation = infoGlobal.m_sor; + btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep; - relaxation = infoGlobal.m_sor; - btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; + //cfm = 1 / ( dt * kp + kd ) + //erp = dt * kp / ( dt * kp + kd ) - //cfm = 1 / ( dt * kp + kd ) - //erp = dt * kp / ( dt * kp + kd ) - - btScalar cfm = infoGlobal.m_globalCfm; - btScalar erp = infoGlobal.m_erp2; + 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; - + 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; + } + } - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + cfm *= invTimeStep; - { + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); + + { #ifdef COMPUTE_IMPULSE_DENOM - btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); + btScalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB); + btScalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB); #else - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } - if (rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } -#endif //COMPUTE_IMPULSE_DENOM + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + if (rb0) + { + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); + } + if (rb1) + { + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); + } +#endif //COMPUTE_IMPULSE_DENOM - btScalar denom = relaxation/(denom0+denom1+cfm); - solverConstraint.m_jacDiagABInv = denom; - } + btScalar denom = relaxation / (denom0 + denom1 + cfm); + solverConstraint.m_jacDiagABInv = denom; + } - if (rb0) - { - solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - } else - { - solverConstraint.m_contactNormal1.setZero(); - solverConstraint.m_relpos1CrossNormal.setZero(); - } - if (rb1) - { - solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB; - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - }else - { - solverConstraint.m_contactNormal2.setZero(); - solverConstraint.m_relpos2CrossNormal.setZero(); - } + if (rb0) + { + solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + } + else + { + solverConstraint.m_contactNormal1.setZero(); + solverConstraint.m_relpos1CrossNormal.setZero(); + } + if (rb1) + { + solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB; + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + } + else + { + solverConstraint.m_contactNormal2.setZero(); + solverConstraint.m_relpos2CrossNormal.setZero(); + } - btScalar restitution = 0.f; - btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; + btScalar restitution = 0.f; + btScalar penetration = cp.getDistance() + infoGlobal.m_linearSlop; - { - btVector3 vel1,vel2; - - vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - - // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - btVector3 vel = vel1 - vel2; - btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); - - - - solverConstraint.m_friction = cp.m_combinedFriction; - - - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - }; - } - - - ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - 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; - - { - - btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0); - btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0); - btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0); - btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0); - - - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) - + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) - + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB); - btScalar rel_vel = vel1Dotn+vel2Dotn; - - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; - - - - if (penetration>0) - { - positionalError = 0; - - velocityError -= penetration *invTimeStep; - } else - { - positionalError = -penetration * erp*invTimeStep; - - } - - 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_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv; - 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 = cfm*solverConstraint.m_jacDiagABInv; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } + { + btVector3 vel1, vel2; + vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0, 0, 0); + vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0); + // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); + btVector3 vel = vel1 - vel2; + btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); + + solverConstraint.m_friction = cp.m_combinedFriction; + + restitution = btSequentialImpulseConstraintSolver::restitutionCurveInternal(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + }; + } + + ///warm starting (or zero if disabled) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); + } + else + { + solverConstraint.m_appliedImpulse = 0.f; + } + + solverConstraint.m_appliedPushImpulse = 0.f; + + { + btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse : btVector3(0, 0, 0); + btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse : btVector3(0, 0, 0); + btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse : btVector3(0, 0, 0); + btVector3 externalTorqueImpulseB = bodyB->m_originalBody ? bodyB->m_externalTorqueImpulse : btVector3(0, 0, 0); + + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity + externalTorqueImpulseA); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity + externalTorqueImpulseB); + btScalar rel_vel = vel1Dotn + vel2Dotn; + + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel; // * damping; + + if (penetration > 0) + { + positionalError = 0; + + velocityError -= penetration * invTimeStep; + } + else + { + positionalError = -penetration * erp * invTimeStep; + } + + 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_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv; + 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 = cfm * solverConstraint.m_jacDiagABInv; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } +} + +void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + const btVector3& rel_pos1, const btVector3& rel_pos2) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations + ); + setupContactConstraintInternal(siData, solverConstraint, + solverBodyIdA, solverBodyIdB, + cp, infoGlobal, + relaxation, + rel_pos1, rel_pos2); } - -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, + btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) { - - btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + btSolverBody* bodyA = &tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyB = &tmpSolverBodyPool[solverBodyIdB]; btRigidBody* rb0 = bodyA->m_originalBody; btRigidBody* rb1 = bodyB->m_originalBody; { - btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + btSolverConstraint& frictionConstraint1 = tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; if (rb0) - bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1 * rb0->getInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse); if (rb1) - bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); - } else + bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2 * rb1->getInvMass(), -frictionConstraint1.m_angularComponentB, -(btScalar)frictionConstraint1.m_appliedImpulse); + } + else { frictionConstraint1.m_appliedImpulse = 0.f; } @@ -1061,50 +1314,55 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolver if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + btSolverConstraint& frictionConstraint2 = tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; + frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; if (rb0) - bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1 * rb0->getInvMass(), frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse); if (rb1) - bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); - } else + bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2 * rb1->getInvMass(), -frictionConstraint2.m_angularComponentB, -(btScalar)frictionConstraint2.m_appliedImpulse); + } + else { frictionConstraint2.m_appliedImpulse = 0.f; } } } - - - -void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) { - btCollisionObject* colObj0=0,*colObj1=0; + setFrictionConstraintImpulseInternal(m_tmpSolverBodyPool, m_tmpSolverContactFrictionConstraintPool, + solverConstraint, + solverBodyIdA, solverBodyIdB, + cp, infoGlobal); + +} +void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) +{ + btCollisionObject *colObj0 = 0, *colObj1 = 0; colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); - -// btRigidBody* bodyA = btRigidBody::upcast(colObj0); -// btRigidBody* bodyB = btRigidBody::upcast(colObj1); - - btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + int solverBodyIdA = siData.getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = siData.getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + // btRigidBody* bodyA = btRigidBody::upcast(colObj0); + // btRigidBody* bodyB = btRigidBody::upcast(colObj1); + btSolverBody* solverBodyA = &siData.m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* solverBodyB = &siData.m_tmpSolverBodyPool[solverBodyIdB]; ///avoid collision response between two static objects if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero()))) return; - int rollingFriction=1; - for (int j=0;jgetNumContacts();j++) + int rollingFriction = 1; + for (int j = 0; j < manifold->getNumContacts(); j++) { - btManifoldPoint& cp = manifold->getContactPoint(j); if (cp.getDistance() <= manifold->getContactProcessingThreshold()) @@ -1113,9 +1371,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btVector3 rel_pos2; btScalar relaxation; - - int frictionIndex = m_tmpSolverContactConstraintPool.size(); - btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); + int frictionIndex = siData.m_tmpSolverContactConstraintPool.size(); + btSolverConstraint& solverConstraint = siData.m_tmpSolverContactConstraintPool.expandNonInitializing(); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -1129,43 +1386,48 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btVector3 vel1; btVector3 vel2; - - solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1); - solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 ); - btVector3 vel = vel1 - vel2; + solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1); + solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2); + + btVector3 vel = vel1 - vel2; btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); - setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); - - - + setupContactConstraintInternal(siData, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); /////setup the friction constraints - solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); + solverConstraint.m_frictionIndex = siData.m_tmpSolverContactFrictionConstraintPool.size(); - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) + if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0)) { - { - 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); + + btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, + siData.m_tmpSolverContactRollingFrictionConstraintPool, + 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) - addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp, - cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (axis1.length()>0.001) - addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp, - cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + 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) + { + btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, + siData.m_tmpSolverContactRollingFrictionConstraintPool, axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, + cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + } + if (axis1.length() > 0.001) + { + btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, + siData.m_tmpSolverContactRollingFrictionConstraintPool, axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, + cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + } } } @@ -1180,114 +1442,437 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m ///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 + ///and use contactPoint.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ///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_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) + + 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); - 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)) - { - 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); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,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); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal); + 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); + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + 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, infoGlobal); + 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); + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, 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); + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + 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); + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + 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_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; + cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; } } - - } else + } + else { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + 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, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); - + { + btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + 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); - - - - + btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal( + siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, + solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); } } } -void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + btSequentialImpulseConstraintSolver::convertContactInternal(siData, manifold, infoGlobal); +} + +void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { int i; btPersistentManifold* manifold = 0; -// btCollisionObject* colObj0=0,*colObj1=0; + // btCollisionObject* colObj0=0,*colObj1=0; - - for (i=0;i& tmpSolverBodyPool, + int& maxOverrideNumSolverIterations, + 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 = &tmpSolverBodyPool[solverBodyIdA]; + const btSolverBody* bodyBPtr = &tmpSolverBodyPool[solverBodyIdB]; + + int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; + if (overrideNumSolverIterations > maxOverrideNumSolverIterations) + maxOverrideNumSolverIterations = overrideNumSolverIterations; + + for (int j = 0; j < info1.m_numConstraintRows; j++) + { + memset(¤tConstraintRow[j], 0, sizeof(btSolverConstraint)); + currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY; + currentConstraintRow[j].m_upperLimit = SIMD_INFINITY; + currentConstraintRow[j].m_appliedImpulse = 0.f; + currentConstraintRow[j].m_appliedPushImpulse = 0.f; + currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; + currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; + currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; + } + + // these vectors are already cleared in initSolverBody, no need to redundantly clear again + btAssert(bodyAPtr->getDeltaLinearVelocity().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 < info1.m_numConstraintRows; j++) + { + btSolverConstraint& solverConstraint = currentConstraintRow[j]; + + if (solverConstraint.m_upperLimit >= 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::convertJoint(btSolverConstraint* currentConstraintRow, + btTypedConstraint* constraint, + const btTypedConstraint::btConstraintInfo1& info1, + int solverBodyIdA, + int solverBodyIdB, + const btContactSolverInfo& infoGlobal) +{ +} + +void btSequentialImpulseConstraintSolver::convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("convertJoints"); + for (int j = 0; j < numConstraints; j++) + { + btTypedConstraint* constraint = constraints[j]; + constraint->buildJacobian(); + constraint->internalSetAppliedImpulse(0.0f); + } + + int totalNumRows = 0; + + siData.m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); + //calculate the total number of contraint rows + for (int i = 0; i < numConstraints; i++) + { + btTypedConstraint::btConstraintInfo1& info1 = siData.m_tmpConstraintSizesPool[i]; + btJointFeedback* fb = constraints[i]->getJointFeedback(); + 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; + } + siData.m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); + + ///setup the btSolverConstraints + int currentRow = 0; + + for (int i = 0; i < numConstraints; i++) + { + const btTypedConstraint::btConstraintInfo1& info1 = siData.m_tmpConstraintSizesPool[i]; + + if (info1.m_numConstraintRows) + { + btAssert(currentRow < totalNumRows); + + btSolverConstraint* currentConstraintRow = &siData.m_tmpSolverNonContactConstraintPool[currentRow]; + btTypedConstraint* constraint = constraints[i]; + btRigidBody& rbA = constraint->getRigidBodyA(); + btRigidBody& rbB = constraint->getRigidBodyB(); + + int solverBodyIdA = siData.getOrInitSolverBody(rbA, infoGlobal.m_timeStep); + int solverBodyIdB = siData.getOrInitSolverBody(rbB, infoGlobal.m_timeStep); + + convertJointInternal(siData.m_tmpSolverBodyPool, siData.m_maxOverrideNumSolverIterations, + currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); + } + currentRow += info1.m_numConstraintRows; + } +} + +void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + convertJointsInternal(siData, constraints, numConstraints, infoGlobal); +} + + +void btSequentialImpulseConstraintSolver::convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("convertBodies"); + for (int i = 0; i < numBodies; i++) + { + bodies[i]->setCompanionId(-1); + } +#if BT_THREADSAFE + siData.m_kinematicBodyUniqueIdToSolverBodyTable.resize(0); +#endif // BT_THREADSAFE + + siData.m_tmpSolverBodyPool.reserve(numBodies + 1); + siData.m_tmpSolverBodyPool.resize(0); + + //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + //initSolverBody(&fixedBody,0); + + for (int i = 0; i < numBodies; i++) + { + int bodyId = siData.getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); + + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body && body->getInvMass()) + { + btSolverBody& solverBody = siData.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; + } + } + } +} + + +void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + convertBodiesInternal(siData, bodies, numBodies, infoGlobal); +} + +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; - } + // 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 - //make sure that dynamic bodies exist for all (enabled) constraints - for (int i=0;iisEnabled()) { if (!constraint->getRigidBodyA().isStaticOrKinematicObject()) { - bool found=false; - for (int b=0;bgetRigidBodyA()==bodies[b]) + if (&constraint->getRigidBodyA() == bodies[b]) { found = true; break; @@ -1297,10 +1882,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } if (!constraint->getRigidBodyB().isStaticOrKinematicObject()) { - bool found=false; - for (int b=0;bgetRigidBodyB()==bodies[b]) + if (&constraint->getRigidBodyB() == bodies[b]) { found = true; break; @@ -1310,290 +1895,46 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } } - //make sure that dynamic bodies exist for all contact manifolds - for (int i=0;igetBody0()->isStaticOrKinematicObject()) - { - bool found=false; - for (int b=0;bgetBody0()==bodies[b]) - { - found = true; - break; - } - } - btAssert(found); - } - if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject()) - { - bool found=false; - for (int b=0;bgetBody1()==bodies[b]) - { - found = true; - break; - } - } - btAssert(found); - } - } -#endif //BT_ADDITIONAL_DEBUG - - - for (int i = 0; i < numBodies; i++) + //make sure that dynamic bodies exist for all contact manifolds + for (int i = 0; i < numManifolds; i++) { - bodies[i]->setCompanionId(-1); + if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject()) + { + bool found = false; + for (int b = 0; b < numBodies; b++) + { + if (manifoldPtr[i]->getBody0() == bodies[b]) + { + found = true; + break; + } + } + btAssert(found); + } + if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject()) + { + bool found = false; + for (int b = 0; b < numBodies; b++) + { + if (manifoldPtr[i]->getBody1() == bodies[b]) + { + found = true; + break; + } + } + btAssert(found); + } } -#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); +#endif //BT_ADDITIONAL_DEBUG //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; - - } - - - } - } - - 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); - 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; - - - } - } - } - currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; - } - } - - convertContacts(manifoldPtr,numManifolds,infoGlobal); - - } - -// btContactSolverInfo info = infoGlobal; + convertContacts(manifoldPtr, numManifolds, infoGlobal); + // btContactSolverInfo info = infoGlobal; int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); @@ -1602,372 +1943,462 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2); + m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2); else m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); { int i; - for (i=0;iisEnabled()) { - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); - leastSquaresResidual += residual*residual; + int bodyAid = siData.getSolverBody(constraints[j]->getRigidBodyA()); + int bodyBid = siData.getSolverBody(constraints[j]->getRigidBodyB()); + btSolverBody& bodyA = siData.m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = siData.m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep); } } - if (iteration< infoGlobal.m_numIterations) + ///solve all contact constraints + if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) { - for (int j=0;jisEnabled()) + btScalar totalImpulse = 0; + { - 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); + const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[c]]; + btScalar residual = siData.m_resolveSingleConstraintRowLowerLimit(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + + totalImpulse = solveManifold.m_appliedImpulse; } - } - - ///solve all contact constraints - if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; - - for (int c=0;c btScalar(0)) { + solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]]; - - if (totalImpulse>btScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual += residual*residual; - } + btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } + } - if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) + { + btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[c * multiplier + 1]]; + + if (totalImpulse > btScalar(0)) { + solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; - - if (totalImpulse>btScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual += residual*residual; - } + btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } } - } - else//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;jbtScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual += residual*residual; - } - } - } - - - 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 residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - leastSquaresResidual += residual*residual; - } - } - - } + else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + { + //solve the friction constraints after all contact constraints, don't interleave them + int numPoolConstraints = siData.m_tmpSolverContactConstraintPool.size(); + int j; + + for (j = 0; j < numPoolConstraints; j++) + { + const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[j]]; + btScalar residual = siData.m_resolveSingleConstraintRowLowerLimit(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + + ///solve all friction constraints + + int numFrictionPoolConstraints = siData.m_tmpSolverContactFrictionConstraintPool.size(); + for (j = 0; j < numFrictionPoolConstraints; j++) + { + btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = siData.m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + if (totalImpulse > btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; + + btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + } + } + + int numRollingFrictionPoolConstraints = siData.m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (int j = 0; j < numRollingFrictionPoolConstraints; j++) + { + btSolverConstraint& rollingFrictionConstraint = siData.m_tmpSolverContactRollingFrictionConstraintPool[j]; + btScalar totalImpulse = siData.m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; + if (totalImpulse > btScalar(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 residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], siData.m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + } + } return leastSquaresResidual; } -void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/) { + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + btScalar leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, + iteration, constraints, numConstraints, infoGlobal); + return leastSquaresResidual; +} + +void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + solveGroupCacheFriendlySplitImpulseIterationsInternal(siData, + bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + +} +void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, 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) { { - for ( iteration = 0;iteration=(infoGlobal.m_numIterations-1)) + if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration); + printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration); #endif break; } } - } + } } } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { BT_PROFILE("solveGroupCacheFriendlyIterations"); { ///this is a special step to resolve penetrations (just for contacts) - solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - for ( int iteration = 0 ; iteration< maxIterations ; iteration++) - //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) + for (int iteration = 0; iteration < maxIterations; iteration++) + //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) { - m_leastSquaresResidual = 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))) + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n",m_leastSquaresResidual,iteration); + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); #endif + m_analyticsData.m_numSolverCalls++; + m_analyticsData.m_numIterationsUsed = iteration+1; + m_analyticsData.m_islandId = -2; + if (numBodies>0) + m_analyticsData.m_islandId = bodies[0]->getCompanionId(); + m_analyticsData.m_numBodies = numBodies; + m_analyticsData.m_numContactManifolds = numManifolds; + m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; break; } } - } return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int i,j; - - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + for (int j = iBegin; j < iEnd; j++) { - for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; + const btSolverConstraint& solveManifold = tmpSolverContactConstraintPool[j]; + btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint; + btAssert(pt); + pt->m_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 & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; - } - //do a callback here? + // printf("pt->m_appliedImpulseLateral1 = %f\n", f); + pt->m_appliedImpulseLateral1 = tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + pt->m_appliedImpulseLateral2 = tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse; } + //do a callback here? } +} - numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); - for (j=0;jgetJointFeedback(); if (fb) { - fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ - + fb->m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr->getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep; + fb->m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr->getRigidBodyB().getLinearFactor() / infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr->getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr->getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; /*RGM ???? */ } constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); - if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + if (btFabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold()) { constr->setEnabled(false); } } +} - - - for ( i=0;i& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) +{ + for (int i = iBegin; i < iEnd; i++) { - btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; + btRigidBody* body = tmpSolverBodyPool[i].m_originalBody; if (body) { if (infoGlobal.m_splitImpulse) - m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); + tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); else - m_tmpSolverBodyPool[i].writebackVelocity(); + tmpSolverBodyPool[i].writebackVelocity(); - m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( - m_tmpSolverBodyPool[i].m_linearVelocity+ - m_tmpSolverBodyPool[i].m_externalForceImpulse); + tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( + tmpSolverBodyPool[i].m_linearVelocity + + tmpSolverBodyPool[i].m_externalForceImpulse); - m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( - m_tmpSolverBodyPool[i].m_angularVelocity+ - m_tmpSolverBodyPool[i].m_externalTorqueImpulse); + tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( + tmpSolverBodyPool[i].m_angularVelocity + + tmpSolverBodyPool[i].m_externalTorqueImpulse); if (infoGlobal.m_splitImpulse) - m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform); + tmpSolverBodyPool[i].m_originalBody->setWorldTransform(tmpSolverBodyPool[i].m_worldTransform); - m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1); + tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1); } } +} - m_tmpSolverContactConstraintPool.resizeNoInitialize(0); - m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("solveGroupCacheFriendlyFinish"); - m_tmpSolverBodyPool.resizeNoInitialize(0); + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + writeBackContactsInternal(siData.m_tmpSolverContactConstraintPool, siData.m_tmpSolverContactFrictionConstraintPool, 0, siData.m_tmpSolverContactConstraintPool.size(), infoGlobal); + } + + writeBackJointsInternal(siData.m_tmpSolverNonContactConstraintPool, 0, siData.m_tmpSolverNonContactConstraintPool.size(), infoGlobal); + writeBackBodiesInternal(siData.m_tmpSolverBodyPool, 0, siData.m_tmpSolverBodyPool.size(), infoGlobal); + + siData.m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + siData.m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + siData.m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + siData.m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + + siData.m_tmpSolverBodyPool.resizeNoInitialize(0); return 0.f; } +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, infoGlobal); +} /// btSequentialImpulseConstraintSolver Sequentially applies impulses -btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/) +btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btDispatcher* /*dispatcher*/) { - BT_PROFILE("solveGroup"); //you need to provide at least some bodies - solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); + solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); + solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); return 0.f; } -void btSequentialImpulseConstraintSolver::reset() +void btSequentialImpulseConstraintSolver::reset() { m_btSeed2 = 0; -} +} \ No newline at end of file diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 16c7eb74c..2b88e25be 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/Engine/lib/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. @@ -27,141 +27,265 @@ class btCollisionObject; #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" -typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); +typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); + +struct btSISolverSingleIterationData +{ + btAlignedObjectArray& m_tmpSolverBodyPool; + btConstraintArray& m_tmpSolverContactConstraintPool; + btConstraintArray& m_tmpSolverNonContactConstraintPool; + btConstraintArray& m_tmpSolverContactFrictionConstraintPool; + btConstraintArray& m_tmpSolverContactRollingFrictionConstraintPool; + + btAlignedObjectArray& m_orderTmpConstraintPool; + btAlignedObjectArray& m_orderNonContactConstraintPool; + btAlignedObjectArray& m_orderFrictionConstraintPool; + btAlignedObjectArray& m_tmpConstraintSizesPool; + unsigned long& m_seed; + + btSingleConstraintRowSolver& m_resolveSingleConstraintRowGeneric; + btSingleConstraintRowSolver& m_resolveSingleConstraintRowLowerLimit; + btSingleConstraintRowSolver& m_resolveSplitPenetrationImpulse; + btAlignedObjectArray& m_kinematicBodyUniqueIdToSolverBodyTable; + int& m_fixedBodyId; + int& m_maxOverrideNumSolverIterations; + int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep); + static void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep); + int getSolverBody(btCollisionObject& body) const; + + + btSISolverSingleIterationData(btAlignedObjectArray& tmpSolverBodyPool, + btConstraintArray& tmpSolverContactConstraintPool, + btConstraintArray& tmpSolverNonContactConstraintPool, + btConstraintArray& tmpSolverContactFrictionConstraintPool, + btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, + btAlignedObjectArray& orderTmpConstraintPool, + btAlignedObjectArray& orderNonContactConstraintPool, + btAlignedObjectArray& orderFrictionConstraintPool, + btAlignedObjectArray& tmpConstraintSizesPool, + btSingleConstraintRowSolver& resolveSingleConstraintRowGeneric, + btSingleConstraintRowSolver& resolveSingleConstraintRowLowerLimit, + btSingleConstraintRowSolver& resolveSplitPenetrationImpulse, + btAlignedObjectArray& kinematicBodyUniqueIdToSolverBodyTable, + unsigned long& seed, + int& fixedBodyId, + int& maxOverrideNumSolverIterations + ) + :m_tmpSolverBodyPool(tmpSolverBodyPool), + m_tmpSolverContactConstraintPool(tmpSolverContactConstraintPool), + m_tmpSolverNonContactConstraintPool(tmpSolverNonContactConstraintPool), + m_tmpSolverContactFrictionConstraintPool(tmpSolverContactFrictionConstraintPool), + m_tmpSolverContactRollingFrictionConstraintPool(tmpSolverContactRollingFrictionConstraintPool), + m_orderTmpConstraintPool(orderTmpConstraintPool), + m_orderNonContactConstraintPool(orderNonContactConstraintPool), + m_orderFrictionConstraintPool(orderFrictionConstraintPool), + m_tmpConstraintSizesPool(tmpConstraintSizesPool), + m_seed(seed), + m_resolveSingleConstraintRowGeneric(resolveSingleConstraintRowGeneric), + m_resolveSingleConstraintRowLowerLimit(resolveSingleConstraintRowLowerLimit), + m_resolveSplitPenetrationImpulse(resolveSplitPenetrationImpulse), + m_kinematicBodyUniqueIdToSolverBodyTable(kinematicBodyUniqueIdToSolverBodyTable), + m_fixedBodyId(fixedBodyId), + m_maxOverrideNumSolverIterations(maxOverrideNumSolverIterations) + { + } +}; + +struct btSolverAnalyticsData +{ + btSolverAnalyticsData() + { + m_numSolverCalls = 0; + m_numIterationsUsed = -1; + m_remainingLeastSquaresResidual = -1; + m_islandId = -2; + } + int m_islandId; + int m_numBodies; + int m_numContactManifolds; + int m_numSolverCalls; + int m_numIterationsUsed; + double m_remainingLeastSquaresResidual; +}; ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. -ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver +ATTRIBUTE_ALIGNED16(class) +btSequentialImpulseConstraintSolver : public btConstraintSolver { -protected: - btAlignedObjectArray m_tmpSolverBodyPool; - btConstraintArray m_tmpSolverContactConstraintPool; - btConstraintArray m_tmpSolverNonContactConstraintPool; - btConstraintArray m_tmpSolverContactFrictionConstraintPool; - btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; + - btAlignedObjectArray m_orderTmpConstraintPool; - btAlignedObjectArray m_orderNonContactConstraintPool; - btAlignedObjectArray m_orderFrictionConstraintPool; +protected: + btAlignedObjectArray m_tmpSolverBodyPool; + btConstraintArray m_tmpSolverContactConstraintPool; + btConstraintArray m_tmpSolverNonContactConstraintPool; + btConstraintArray m_tmpSolverContactFrictionConstraintPool; + btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; + + btAlignedObjectArray m_orderTmpConstraintPool; + btAlignedObjectArray m_orderNonContactConstraintPool; + btAlignedObjectArray m_orderFrictionConstraintPool; btAlignedObjectArray m_tmpConstraintSizesPool; - int m_maxOverrideNumSolverIterations; + 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 + // 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 ); + btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse; + int m_cachedSolverMode; // used to check if SOLVER_SIMD flag has been changed + void setupSolverFunctions(bool useSimd); - btScalar m_leastSquaresResidual; + 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, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity=0., btScalar cfmSlip=0.); + 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, + const btContactSolverInfo& infoGlobal, + btScalar desiredVelocity = 0., btScalar cfmSlip = 0.); - 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.); + 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, 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); + 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, - const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); + 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); + static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode); - void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); + 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; + unsigned long m_btSeed2; - btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold); - virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); + virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); - void convertContact(btPersistentManifold* manifold,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); - btSimdScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) - { - return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); - } + virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); - btSimdScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) - { - return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); - } + btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint) + { + return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint); + } + + btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint) + { + return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint); + } //internal method - int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep); - void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep); - - btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - 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: - - - 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); - - 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); + int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep); + void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep); + btScalar resolveSingleConstraintRowGeneric(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowLowerLimit(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint); + btScalar resolveSplitPenetrationImpulse(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint) + { + return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint); + } public: + 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); + + + 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); + +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); - + virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + + static btScalar solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal); + static void convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); + static void convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal); + static void convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); + static void setupContactConstraintInternal(btSISolverSingleIterationData& siData, btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, + const btVector3& rel_pos1, const btVector3& rel_pos2); + static btScalar restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold); + static btSolverConstraint& addTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.); + static void setupTorsionalFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity, btScalar cfmSlip); + static void setupFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip); + static btSolverConstraint& addFrictionConstraintInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, 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.); + static void setFrictionConstraintImpulseInternal(btAlignedObjectArray& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, + + btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); + static void convertJointInternal(btAlignedObjectArray& tmpSolverBodyPool, + int& maxOverrideNumSolverIterations, + btSolverConstraint* currentConstraintRow, + btTypedConstraint* constraint, + const btTypedConstraint::btConstraintInfo1& info1, + int solverBodyIdA, + int solverBodyIdB, + const btContactSolverInfo& infoGlobal); + + static btScalar solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); + + static void writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + + static void writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + static void writeBackBodiesInternal(btAlignedObjectArray& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + static void solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + + ///clear internal cached data and reset random seed - virtual void reset(); - + virtual void reset(); + unsigned long btRand2(); + int btRandInt2(int n); - int btRandInt2 (int n); + static unsigned long btRand2a(unsigned long& seed); + static int btRandInt2a(int n, unsigned long& seed); - void setRandSeed(unsigned long seed) + void setRandSeed(unsigned long seed) { m_btSeed2 = seed; } - unsigned long getRandSeed() const + unsigned long getRandSeed() const { return m_btSeed2; } - - virtual btConstraintSolverType getSolverType() const + virtual btConstraintSolverType getSolverType() const { return BT_SEQUENTIAL_IMPULSE_SOLVER; } - btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric() + btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric() { return m_resolveSingleConstraintRowGeneric; } @@ -169,7 +293,7 @@ public: { m_resolveSingleConstraintRowGeneric = rowSolver; } - btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit() + btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit() { return m_resolveSingleConstraintRowLowerLimit; } @@ -178,19 +302,22 @@ public: m_resolveSingleConstraintRowLowerLimit = rowSolver; } + + ///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4 - btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric(); - btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric(); - btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric(); + static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric(); + static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric(); + static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric(); ///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4 - btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit(); - btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit(); - btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit(); + static btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit(); + static btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit(); + static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit(); + + static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric(); + static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric(); + + btSolverAnalyticsData m_analyticsData; }; - - - -#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H - +#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp new file mode 100644 index 000000000..2718da4a5 --- /dev/null +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp @@ -0,0 +1,1554 @@ +/* +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; i < numConstraints; i++) + { + btTypedConstraint* constraint = constraints[i]; + + JointParams& params = jointParamsArray[i]; + const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + + if (info1.m_numConstraintRows) + { + params.m_solverConstraint = totalNumRows; + params.m_solverBodyA = getOrInitSolverBody(constraint->getRigidBodyA(), 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; j < numConstraints; j++) + { + if (constraints[j]->isEnabled()) + { + 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/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h new file mode 100644 index 000000000..1861ddd7d --- /dev/null +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h @@ -0,0 +1,150 @@ +/* +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/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index d63cef031..cac5302a7 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -18,8 +18,6 @@ Added by Roman Ponomarev (rponom@gmail.com) April 04, 2008 */ - - #include "btSliderConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" @@ -29,10 +27,10 @@ April 04, 2008 void btSliderConstraint::initParams() { - m_lowerLinLimit = btScalar(1.0); - m_upperLinLimit = btScalar(-1.0); - m_lowerAngLimit = btScalar(0.); - m_upperAngLimit = btScalar(0.); + m_lowerLinLimit = btScalar(1.0); + m_upperLinLimit = btScalar(-1.0); + m_lowerAngLimit = btScalar(0.); + m_upperAngLimit = btScalar(0.); m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; m_dampingDirLin = btScalar(0.); @@ -59,13 +57,13 @@ void btSliderConstraint::initParams() m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM; m_poweredLinMotor = false; - m_targetLinMotorVelocity = btScalar(0.); - m_maxLinMotorForce = btScalar(0.); + m_targetLinMotorVelocity = btScalar(0.); + m_maxLinMotorForce = btScalar(0.); m_accumulatedLinMotorImpulse = btScalar(0.0); m_poweredAngMotor = false; - m_targetAngMotorVelocity = btScalar(0.); - m_maxAngMotorForce = btScalar(0.); + m_targetAngMotorVelocity = btScalar(0.); + m_maxAngMotorForce = btScalar(0.); m_accumulatedAngMotorImpulse = btScalar(0.0); m_flags = 0; @@ -73,43 +71,32 @@ void btSliderConstraint::initParams() m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME; - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); } - - - - btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB), - m_useSolveConstraintObsolete(false), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB), + m_useSolveConstraintObsolete(false), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) { initParams(); } - - btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB), - m_useSolveConstraintObsolete(false), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB), + m_useSolveConstraintObsolete(false), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; -// m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); + // m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); initParams(); } - - - - - void btSliderConstraint::getInfo1(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) @@ -119,46 +106,39 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info) } else { - info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular - info->nub = 2; + info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular + info->nub = 2; //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); testAngLimits(); testLinLimits(); - if(getSolveLinLimit() || getPoweredLinMotor()) + if (getSolveLinLimit() || getPoweredLinMotor()) { - info->m_numConstraintRows++; // limit 3rd linear as well - info->nub--; + info->m_numConstraintRows++; // limit 3rd linear as well + info->nub--; } - if(getSolveAngLimit() || getPoweredAngMotor()) + if (getSolveAngLimit() || getPoweredAngMotor()) { - info->m_numConstraintRows++; // limit 3rd angular as well - info->nub--; + info->m_numConstraintRows++; // limit 3rd angular as well + info->nub--; } } } void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info) { - - info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used) - info->nub = 0; + info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used) + info->nub = 0; } void btSliderConstraint::getInfo2(btConstraintInfo2* info) { - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass()); + getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(), m_rbB.getLinearVelocity(), m_rbA.getInvMass(), m_rbB.getInvMass()); } - - - - - - -void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) +void btSliderConstraint::calculateTransforms(const btTransform& transA, const btTransform& transB) { - if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) + if (m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) { m_calculatedTransformA = transA * m_frameInA; m_calculatedTransformB = transB * m_frameInB; @@ -170,8 +150,8 @@ void btSliderConstraint::calculateTransforms(const btTransform& transA,const btT } m_realPivotAInW = m_calculatedTransformA.getOrigin(); m_realPivotBInW = m_calculatedTransformB.getOrigin(); - m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) + m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X + if (m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) { m_delta = m_realPivotBInW - m_realPivotAInW; } @@ -180,30 +160,28 @@ void btSliderConstraint::calculateTransforms(const btTransform& transA,const btT m_delta = m_realPivotAInW - m_realPivotBInW; } m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - btVector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { + btVector3 normalWorld; + int i; + //linear part + for (i = 0; i < 3; i++) + { normalWorld = m_calculatedTransformA.getBasis().getColumn(i); m_depth[i] = m_delta.dot(normalWorld); - } + } } - - void btSliderConstraint::testLinLimits(void) { m_solveLinLim = false; m_linPos = m_depth[0]; - if(m_lowerLinLimit <= m_upperLinLimit) + if (m_lowerLinLimit <= m_upperLinLimit) { - if(m_depth[0] > m_upperLinLimit) + if (m_depth[0] > m_upperLinLimit) { m_depth[0] -= m_upperLinLimit; m_solveLinLim = true; } - else if(m_depth[0] < m_lowerLinLimit) + else if (m_depth[0] < m_lowerLinLimit) { m_depth[0] -= m_lowerLinLimit; m_solveLinLim = true; @@ -219,27 +197,25 @@ void btSliderConstraint::testLinLimits(void) } } - - void btSliderConstraint::testAngLimits(void) { m_angDepth = btScalar(0.); m_solveAngLim = false; - if(m_lowerAngLimit <= m_upperAngLimit) + if (m_lowerAngLimit <= m_upperAngLimit) { const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); -// btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); - btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0)); + // btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0)); rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit); m_angPos = rot; - if(rot < m_lowerAngLimit) + if (rot < m_lowerAngLimit) { m_angDepth = rot - m_lowerAngLimit; m_solveAngLim = true; - } - else if(rot > m_upperAngLimit) + } + else if (rot > m_upperAngLimit) { m_angDepth = rot - m_upperAngLimit; m_solveAngLim = true; @@ -255,8 +231,6 @@ btVector3 btSliderConstraint::getAncorInA(void) return ancorInA; } - - btVector3 btSliderConstraint::getAncorInB(void) { btVector3 ancorInB; @@ -264,17 +238,16 @@ btVector3 btSliderConstraint::getAncorInB(void) return ancorInB; } - -void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass ) +void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, btScalar rbAinvMass, btScalar rbBinvMass) { const btTransform& trA = getCalculatedTransformA(); const btTransform& trB = getCalculatedTransformB(); - + btAssert(!m_useSolveConstraintObsolete); int i, s = info->rowskip; - + btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); - + // difference between frames in WCS btVector3 ofs = trB.getOrigin() - trA.getOrigin(); // now get weight factors depending on masses @@ -283,11 +256,11 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); btScalar miS = miA + miB; btScalar factA, factB; - if(miS > btScalar(0.f)) + if (miS > btScalar(0.f)) { factA = miB / miS; } - else + else { factA = btScalar(0.5f); } @@ -295,17 +268,17 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra btVector3 ax1, p, q; btVector3 ax1A = trA.getBasis().getColumn(0); btVector3 ax1B = trB.getBasis().getColumn(0); - if(m_useOffsetForConstraintFrame) + if (m_useOffsetForConstraintFrame) { // get the desired direction of slider axis // as weighted sum of X-orthos of frameA and frameB in WCS ax1 = ax1A * factA + ax1B * factB; ax1.normalize(); // construct two orthos to slider axis - btPlaneSpace1 (ax1, p, q); + btPlaneSpace1(ax1, p, q); } else - { // old way - use frameA + { // old way - use frameA ax1 = trA.getBasis().getColumn(0); // get 2 orthos to slider axis (Y, Z) p = trA.getBasis().getColumn(1); @@ -322,16 +295,16 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra info->m_J1angularAxis[0] = p[0]; info->m_J1angularAxis[1] = p[1]; info->m_J1angularAxis[2] = p[2]; - info->m_J1angularAxis[s+0] = q[0]; - info->m_J1angularAxis[s+1] = q[1]; - info->m_J1angularAxis[s+2] = q[2]; + info->m_J1angularAxis[s + 0] = q[0]; + info->m_J1angularAxis[s + 1] = q[1]; + info->m_J1angularAxis[s + 2] = q[2]; info->m_J2angularAxis[0] = -p[0]; info->m_J2angularAxis[1] = -p[1]; info->m_J2angularAxis[2] = -p[2]; - info->m_J2angularAxis[s+0] = -q[0]; - info->m_J2angularAxis[s+1] = -q[1]; - info->m_J2angularAxis[s+2] = -q[2]; + info->m_J2angularAxis[s + 0] = -q[0]; + info->m_J2angularAxis[s + 1] = -q[1]; + info->m_J2angularAxis[s + 2] = -q[2]; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the slider back into alignment. // if ax1A,ax1B are the unit length slider axes as computed from bodyA and @@ -347,25 +320,25 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. -// btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); + // btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp; btScalar k = info->fps * currERP; btVector3 u = ax1A.cross(ax1B); info->m_constraintError[0] = k * u.dot(p); info->m_constraintError[s] = k * u.dot(q); - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG) + if (m_flags & BT_SLIDER_FLAGS_CFM_ORTANG) { info->cfm[0] = m_cfmOrthoAng; info->cfm[s] = m_cfmOrthoAng; } - int nrow = 1; // last filled row + int nrow = 1; // last filled row int srow; btScalar limit_err; int limit; - // next two rows. + // next two rows. // we want: velA + wA x relA == velB + wB x relB ... but this would // result in three equations, so we project along two orthos to the slider axis @@ -375,8 +348,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra int s2 = nrow * s; nrow++; int s3 = nrow * s; - btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0); - if(m_useOffsetForConstraintFrame) + btVector3 tmpA(0, 0, 0), tmpB(0, 0, 0), relA(0, 0, 0), relB(0, 0, 0), c(0, 0, 0); + if (m_useOffsetForConstraintFrame) { // get vector from bodyB to frameB in WCS relB = trB.getOrigin() - bodyB_trans.getOrigin(); @@ -398,7 +371,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra // now choose average ortho to slider axis p = orthoB * factA + orthoA * factB; btScalar len2 = p.length2(); - if(len2 > SIMD_EPSILON) + if (len2 > SIMD_EPSILON) { p /= btSqrt(len2); } @@ -411,38 +384,38 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra // fill two rows tmpA = relA.cross(p); tmpB = relB.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = -tmpB[i]; tmpA = relA.cross(q); tmpB = relB.cross(q); - if(hasStaticBody && getSolveAngLimit()) - { // to make constraint between static and dynamic objects more rigid + if (hasStaticBody && getSolveAngLimit()) + { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } - for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s3 + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s3 + i] = -tmpB[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = p[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s3 + i] = q[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -p[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s3 + i] = -q[i]; } else - { // old way - maybe incorrect if bodies are not on the slider axis + { // old way - maybe incorrect if bodies are not on the slider axis // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); btVector3 tmp = c.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = factA * tmp[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = factB * tmp[i]; tmp = c.cross(q); - for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; + for (i = 0; i < 3; i++) info->m_J1angularAxis[s3 + i] = factA * tmp[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[s3 + i] = factB * tmp[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = p[i]; + for (i = 0; i < 3; i++) info->m_J1linearAxis[s3 + i] = q[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -p[i]; + for (i = 0; i < 3; i++) info->m_J2linearAxis[s3 + i] = -q[i]; } // compute two elements of right hand side @@ -454,19 +427,18 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra info->m_constraintError[s2] = rhs; rhs = k * q.dot(ofs); info->m_constraintError[s3] = rhs; - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) + if (m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) { info->cfm[s2] = m_cfmOrthoLin; info->cfm[s3] = m_cfmOrthoLin; } - // check linear limits limit_err = btScalar(0.0); limit = 0; - if(getSolveLinLimit()) + if (getSolveLinLimit()) { - limit_err = getLinDepth() * signFact; + limit_err = getLinDepth() * signFact; limit = (limit_err > btScalar(0.0)) ? 2 : 1; } bool powered = getPoweredLinMotor(); @@ -475,12 +447,12 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra { nrow++; srow = nrow * info->rowskip; - info->m_J1linearAxis[srow+0] = ax1[0]; - info->m_J1linearAxis[srow+1] = ax1[1]; - info->m_J1linearAxis[srow+2] = ax1[2]; - info->m_J2linearAxis[srow+0] = -ax1[0]; - info->m_J2linearAxis[srow+1] = -ax1[1]; - info->m_J2linearAxis[srow+2] = -ax1[2]; + info->m_J1linearAxis[srow + 0] = ax1[0]; + info->m_J1linearAxis[srow + 1] = ax1[1]; + info->m_J1linearAxis[srow + 2] = ax1[2]; + info->m_J2linearAxis[srow + 0] = -ax1[0]; + info->m_J2linearAxis[srow + 1] = -ax1[1]; + info->m_J2linearAxis[srow + 2] = -ax1[2]; // linear torque decoupling step: // // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies @@ -488,36 +460,36 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra // constraint force is applied at must lie along the same ax1 axis. // a torque couple will result in limited slider-jointed free // bodies from gaining angular momentum. - if(m_useOffsetForConstraintFrame) + if (m_useOffsetForConstraintFrame) { // this is needed only when bodyA and bodyB are both dynamic. - if(!hasStaticBody) + if (!hasStaticBody) { tmpA = relA.cross(ax1); tmpB = relB.cross(ax1); - info->m_J1angularAxis[srow+0] = tmpA[0]; - info->m_J1angularAxis[srow+1] = tmpA[1]; - info->m_J1angularAxis[srow+2] = tmpA[2]; - info->m_J2angularAxis[srow+0] = -tmpB[0]; - info->m_J2angularAxis[srow+1] = -tmpB[1]; - info->m_J2angularAxis[srow+2] = -tmpB[2]; + info->m_J1angularAxis[srow + 0] = tmpA[0]; + info->m_J1angularAxis[srow + 1] = tmpA[1]; + info->m_J1angularAxis[srow + 2] = tmpA[2]; + info->m_J2angularAxis[srow + 0] = -tmpB[0]; + info->m_J2angularAxis[srow + 1] = -tmpB[1]; + info->m_J2angularAxis[srow + 2] = -tmpB[2]; } } else - { // The old way. May be incorrect if bodies are not on the slider axis - btVector3 ltd; // Linear Torque Decoupling vector (a torque) + { // The old way. May be incorrect if bodies are not on the slider axis + btVector3 ltd; // Linear Torque Decoupling vector (a torque) ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = factA*ltd[0]; - info->m_J1angularAxis[srow+1] = factA*ltd[1]; - info->m_J1angularAxis[srow+2] = factA*ltd[2]; - info->m_J2angularAxis[srow+0] = factB*ltd[0]; - info->m_J2angularAxis[srow+1] = factB*ltd[1]; - info->m_J2angularAxis[srow+2] = factB*ltd[2]; + info->m_J1angularAxis[srow + 0] = factA * ltd[0]; + info->m_J1angularAxis[srow + 1] = factA * ltd[1]; + info->m_J1angularAxis[srow + 2] = factA * ltd[2]; + info->m_J2angularAxis[srow + 0] = factB * ltd[0]; + info->m_J2angularAxis[srow + 1] = factB * ltd[1]; + info->m_J2angularAxis[srow + 2] = factB * ltd[2]; } // right-hand part btScalar lostop = getLowerLinLimit(); btScalar histop = getUpperLinLimit(); - if(limit && (lostop == histop)) + if (limit && (lostop == histop)) { // the joint motor is ineffective powered = false; } @@ -525,9 +497,9 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra info->m_lowerLimit[srow] = 0.; info->m_upperLimit[srow] = 0.; currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp; - if(powered) + if (powered) { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) + if (m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) { info->cfm[srow] = m_cfmDirLin; } @@ -537,41 +509,41 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps; info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps; } - if(limit) + if (limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) + if (m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) { info->cfm[srow] = m_cfmLimLin; } - if(lostop == histop) - { // limited low and high simultaneously + if (lostop == histop) + { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } - else if(limit == 1) - { // low limit + else if (limit == 1) + { // low limit info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } - else - { // high limit + else + { // high limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); - if(bounce > btScalar(0.0)) + if (bounce > btScalar(0.0)) { btScalar vel = linVelA.dot(ax1); vel -= linVelB.dot(ax1); vel *= signFact; // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) + if (limit == 1) + { // low limit + if (vel < 0) { btScalar newc = -bounce * vel; if (newc > info->m_constraintError[srow]) @@ -581,11 +553,11 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra } } else - { // high limit - all those computations are reversed - if(vel > 0) + { // high limit - all those computations are reversed + if (vel > 0) { btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) + if (newc < info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } @@ -593,40 +565,40 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra } } info->m_constraintError[srow] *= getSoftnessLimLin(); - } // if(limit) - } // if linear limit + } // if(limit) + } // if linear limit // check angular limits limit_err = btScalar(0.0); limit = 0; - if(getSolveAngLimit()) + if (getSolveAngLimit()) { limit_err = getAngDepth(); limit = (limit_err > btScalar(0.0)) ? 1 : 2; } // if the slider has joint limits, add in the extra row powered = getPoweredAngMotor(); - if(limit || powered) + if (limit || powered) { nrow++; srow = nrow * info->rowskip; - info->m_J1angularAxis[srow+0] = ax1[0]; - info->m_J1angularAxis[srow+1] = ax1[1]; - info->m_J1angularAxis[srow+2] = ax1[2]; + info->m_J1angularAxis[srow + 0] = ax1[0]; + info->m_J1angularAxis[srow + 1] = ax1[1]; + info->m_J1angularAxis[srow + 2] = ax1[2]; - info->m_J2angularAxis[srow+0] = -ax1[0]; - info->m_J2angularAxis[srow+1] = -ax1[1]; - info->m_J2angularAxis[srow+2] = -ax1[2]; + info->m_J2angularAxis[srow + 0] = -ax1[0]; + info->m_J2angularAxis[srow + 1] = -ax1[1]; + info->m_J2angularAxis[srow + 2] = -ax1[2]; btScalar lostop = getLowerAngLimit(); btScalar histop = getUpperAngLimit(); - if(limit && (lostop == histop)) + if (limit && (lostop == histop)) { // the joint motor is ineffective powered = false; } currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp; - if(powered) + if (powered) { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) + if (m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) { info->cfm[srow] = m_cfmDirAng; } @@ -635,55 +607,55 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps; info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps; } - if(limit) + if (limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) + if (m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) { info->cfm[srow] = m_cfmLimAng; } - if(lostop == histop) + if (lostop == histop) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } - else if(limit == 1) - { // low limit + else if (limit == 1) + { // low limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } - else - { // high limit + else + { // high limit info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng()); - if(bounce > btScalar(0.0)) + if (bounce > btScalar(0.0)) { btScalar vel = m_rbA.getAngularVelocity().dot(ax1); vel -= m_rbB.getAngularVelocity().dot(ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) + if (limit == 1) + { // low limit + if (vel < 0) { btScalar newc = -bounce * vel; - if(newc > info->m_constraintError[srow]) + if (newc > info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } else - { // high limit - all those computations are reversed - if(vel > 0) + { // high limit - all those computations are reversed + if (vel > 0) { btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) + if (newc < info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } @@ -691,165 +663,161 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra } } info->m_constraintError[srow] *= getSoftnessLimAng(); - } // if(limit) - } // if angular limit or powered + } // if(limit) + } // if angular limit or powered } - -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///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 btSliderConstraint::setParam(int num, btScalar value, int axis) { - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : - if(axis < 1) - { - m_softnessLimLin = value; - m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN; - } - else if(axis < 3) - { - m_softnessOrthoLin = value; - m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN; - } - else if(axis == 3) - { - m_softnessLimAng = value; - m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG; - } - else if(axis < 6) - { - m_softnessOrthoAng = value; - m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - if(axis < 1) - { - m_cfmDirLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN; - } - else if(axis == 3) - { - m_cfmDirAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_STOP_CFM : - if(axis < 1) - { - m_cfmLimLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN; - } - else if(axis < 3) - { - m_cfmOrthoLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN; - } - else if(axis == 3) - { - m_cfmLimAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG; - } - else if(axis < 6) - { - m_cfmOrthoAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG; - } - else - { - btAssertConstrParams(0); - } - break; + case BT_CONSTRAINT_STOP_ERP: + if (axis < 1) + { + m_softnessLimLin = value; + m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN; + } + else if (axis < 3) + { + m_softnessOrthoLin = value; + m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN; + } + else if (axis == 3) + { + m_softnessLimAng = value; + m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG; + } + else if (axis < 6) + { + m_softnessOrthoAng = value; + m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_CFM: + if (axis < 1) + { + m_cfmDirLin = value; + m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN; + } + else if (axis == 3) + { + m_cfmDirAng = value; + m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_STOP_CFM: + if (axis < 1) + { + m_cfmLimLin = value; + m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN; + } + else if (axis < 3) + { + m_cfmOrthoLin = value; + m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN; + } + else if (axis == 3) + { + m_cfmLimAng = value; + m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG; + } + else if (axis < 6) + { + m_cfmOrthoAng = value; + m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG; + } + else + { + btAssertConstrParams(0); + } + break; } } ///return the local value of parameter -btScalar btSliderConstraint::getParam(int num, int axis) const +btScalar btSliderConstraint::getParam(int num, int axis) const { btScalar retVal(SIMD_INFINITY); - switch(num) + switch (num) { - case BT_CONSTRAINT_STOP_ERP : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN); - retVal = m_softnessLimLin; - } - else if(axis < 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN); - retVal = m_softnessOrthoLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG); - retVal = m_softnessLimAng; - } - else if(axis < 6) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG); - retVal = m_softnessOrthoAng; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN); - retVal = m_cfmDirLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG); - retVal = m_cfmDirAng; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_STOP_CFM : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN); - retVal = m_cfmLimLin; - } - else if(axis < 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN); - retVal = m_cfmOrthoLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG); - retVal = m_cfmLimAng; - } - else if(axis < 6) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG); - retVal = m_cfmOrthoAng; - } - else - { - btAssertConstrParams(0); - } - break; + case BT_CONSTRAINT_STOP_ERP: + if (axis < 1) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN); + retVal = m_softnessLimLin; + } + else if (axis < 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN); + retVal = m_softnessOrthoLin; + } + else if (axis == 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG); + retVal = m_softnessLimAng; + } + else if (axis < 6) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG); + retVal = m_softnessOrthoAng; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_CFM: + if (axis < 1) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN); + retVal = m_cfmDirLin; + } + else if (axis == 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG); + retVal = m_cfmDirAng; + } + else + { + btAssertConstrParams(0); + } + break; + case BT_CONSTRAINT_STOP_CFM: + if (axis < 1) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN); + retVal = m_cfmLimLin; + } + else if (axis < 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN); + retVal = m_cfmOrthoLin; + } + else if (axis == 3) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG); + retVal = m_cfmLimAng; + } + else if (axis < 6) + { + btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG); + retVal = m_cfmOrthoAng; + } + else + { + btAssertConstrParams(0); + } + break; } return retVal; } - - - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 1957f08a9..75ca34e97 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -25,31 +25,26 @@ TODO: #ifndef BT_SLIDER_CONSTRAINT_H #define BT_SLIDER_CONSTRAINT_H -#include "LinearMath/btScalar.h"//for BT_USE_DOUBLE_PRECISION +#include "LinearMath/btScalar.h" //for BT_USE_DOUBLE_PRECISION #ifdef BT_USE_DOUBLE_PRECISION -#define btSliderConstraintData2 btSliderConstraintDoubleData -#define btSliderConstraintDataName "btSliderConstraintDoubleData" +#define btSliderConstraintData2 btSliderConstraintDoubleData +#define btSliderConstraintDataName "btSliderConstraintDoubleData" #else -#define btSliderConstraintData2 btSliderConstraintData -#define btSliderConstraintDataName "btSliderConstraintData" -#endif //BT_USE_DOUBLE_PRECISION +#define btSliderConstraintData2 btSliderConstraintData +#define btSliderConstraintDataName "btSliderConstraintData" +#endif //BT_USE_DOUBLE_PRECISION #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" #include "btTypedConstraint.h" - - class btRigidBody; - - -#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) -#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) -#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) -#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f)) - +#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) +#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) +#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) +#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f)) enum btSliderFlags { @@ -67,15 +62,15 @@ enum btSliderFlags BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11) }; - -ATTRIBUTE_ALIGNED16(class) btSliderConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) +btSliderConstraint : public btTypedConstraint { protected: ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; - bool m_useOffsetForConstraintFrame; - btTransform m_frameInA; - btTransform m_frameInB; + bool m_useSolveConstraintObsolete; + bool m_useOffsetForConstraintFrame; + btTransform m_frameInA; + btTransform m_frameInB; // use frameA fo define limits, if true bool m_useLinearReferenceFrameA; // linear limits @@ -119,21 +114,21 @@ protected: btScalar m_restitutionOrthoAng; btScalar m_dampingOrthoAng; btScalar m_cfmOrthoAng; - + // for interlal use bool m_solveLinLim; bool m_solveAngLim; int m_flags; - btJacobianEntry m_jacLin[3]; - btScalar m_jacLinDiagABInv[3]; + btJacobianEntry m_jacLin[3]; + btScalar m_jacLinDiagABInv[3]; - btJacobianEntry m_jacAng[3]; + btJacobianEntry m_jacAng[3]; btScalar m_timeStep; - btTransform m_calculatedTransformA; - btTransform m_calculatedTransformB; + btTransform m_calculatedTransformA; + btTransform m_calculatedTransformB; btVector3 m_sliderAxis; btVector3 m_realPivotAInW; @@ -150,57 +145,57 @@ protected: btScalar m_angDepth; btScalar m_kAngle; - bool m_poweredLinMotor; - btScalar m_targetLinMotorVelocity; - btScalar m_maxLinMotorForce; - btScalar m_accumulatedLinMotorImpulse; - - bool m_poweredAngMotor; - btScalar m_targetAngMotorVelocity; - btScalar m_maxAngMotorForce; - btScalar m_accumulatedAngMotorImpulse; + bool m_poweredLinMotor; + btScalar m_targetLinMotorVelocity; + btScalar m_maxLinMotorForce; + btScalar m_accumulatedLinMotorImpulse; - //------------------------ + bool m_poweredAngMotor; + btScalar m_targetAngMotorVelocity; + btScalar m_maxAngMotorForce; + btScalar m_accumulatedAngMotorImpulse; + + //------------------------ void initParams(); + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + // constructors - btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA); + btSliderConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA); + btSliderConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameA); // overrides - virtual void getInfo1 (btConstraintInfo1* info); + virtual void getInfo1(btConstraintInfo1 * info); - void getInfo1NonVirtual(btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); + void getInfo1NonVirtual(btConstraintInfo1 * info); - void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass); + virtual void getInfo2(btConstraintInfo2 * info); + void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, btScalar rbAinvMass, btScalar rbBinvMass); // access - const btRigidBody& getRigidBodyA() const { return m_rbA; } - const btRigidBody& getRigidBodyB() const { return m_rbB; } - const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } - 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; } - btScalar getLowerLinLimit() { return m_lowerLinLimit; } - void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } - btScalar getUpperLinLimit() { return m_upperLinLimit; } - void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } - btScalar getLowerAngLimit() { return m_lowerAngLimit; } - void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); } - btScalar getUpperAngLimit() { return m_upperAngLimit; } - void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); } + const btRigidBody& getRigidBodyA() const { return m_rbA; } + const btRigidBody& getRigidBodyB() const { return m_rbB; } + const btTransform& getCalculatedTransformA() const { return m_calculatedTransformA; } + 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; } + btScalar getLowerLinLimit() { return m_lowerLinLimit; } + void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } + btScalar getUpperLinLimit() { return m_upperLinLimit; } + void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } + btScalar getLowerAngLimit() { return m_lowerAngLimit; } + void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); } + btScalar getUpperAngLimit() { return m_upperAngLimit; } + void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); } bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } btScalar getSoftnessDirLin() { return m_softnessDirLin; } btScalar getRestitutionDirLin() { return m_restitutionDirLin; } - btScalar getDampingDirLin() { return m_dampingDirLin ; } + btScalar getDampingDirLin() { return m_dampingDirLin; } btScalar getSoftnessDirAng() { return m_softnessDirAng; } btScalar getRestitutionDirAng() { return m_restitutionDirAng; } btScalar getDampingDirAng() { return m_dampingDirAng; } @@ -249,8 +244,6 @@ public: btScalar getLinearPos() const { return m_linPos; } btScalar getAngularPos() const { return m_angPos; } - - // access for ODE solver bool getSolveLinLimit() { return m_solveLinLim; } @@ -258,9 +251,9 @@ public: bool getSolveAngLimit() { return m_solveAngLim; } btScalar getAngDepth() { return m_angDepth; } // shared code used by ODE solver - void calculateTransforms(const btTransform& transA,const btTransform& transB); - void testLinLimits(); - void testAngLimits(); + void calculateTransforms(const btTransform& transA, const btTransform& transB); + void testLinLimits(); + void testAngLimits(); // access for PE Solver btVector3 getAncorInA(); btVector3 getAncorInB(); @@ -268,84 +261,75 @@ public: bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } - void setFrames(const btTransform& frameA, const btTransform& frameB) - { - m_frameInA=frameA; - m_frameInB=frameB; - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + void setFrames(const btTransform& frameA, const btTransform& frameB) + { + m_frameInA = frameA; + m_frameInB = frameB; + calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform()); buildJacobian(); - } + } - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///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 void setParam(int num, btScalar value, int axis = -1); ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; - - virtual int getFlags() const - { + virtual btScalar getParam(int num, int axis = -1) const; + + virtual int getFlags() const + { return m_flags; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; - ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 - struct btSliderConstraintData { - btTypedConstraintData m_typeConstraintData; - btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTypedConstraintData m_typeConstraintData; + btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. btTransformFloatData m_rbBFrame; - - float m_linearUpperLimit; - float m_linearLowerLimit; - float m_angularUpperLimit; - float m_angularLowerLimit; + float m_linearUpperLimit; + float m_linearLowerLimit; - int m_useLinearReferenceFrameA; + float m_angularUpperLimit; + float m_angularLowerLimit; + + int m_useLinearReferenceFrameA; int m_useOffsetForConstraintFrame; - }; - struct btSliderConstraintDoubleData { - btTypedConstraintDoubleData m_typeConstraintData; - btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTypedConstraintDoubleData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. btTransformDoubleData m_rbBFrame; - - double m_linearUpperLimit; - double m_linearLowerLimit; - double m_angularUpperLimit; - double m_angularLowerLimit; + double m_linearUpperLimit; + double m_linearLowerLimit; - int m_useLinearReferenceFrameA; + double m_angularUpperLimit; + double m_angularLowerLimit; + + int m_useLinearReferenceFrameA; int m_useOffsetForConstraintFrame; - }; -SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const { return sizeof(btSliderConstraintData2); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { - - btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer; - btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer); + btSliderConstraintData2* sliderData = (btSliderConstraintData2*)dataBuffer; + btTypedConstraint::serialize(&sliderData->m_typeConstraintData, serializer); m_frameInA.serialize(sliderData->m_rbAFrame); m_frameInB.serialize(sliderData->m_rbBFrame); @@ -362,7 +346,4 @@ SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, bt return btSliderConstraintDataName; } - - -#endif //BT_SLIDER_CONSTRAINT_H - +#endif //BT_SLIDER_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp index 0c7dbd668..1ea20edcb 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @@ -13,43 +13,38 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btSolve2LinearConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" - void btSolve2LinearConstraint::resolveUnilateralPairConstraint( - btRigidBody* body1, - btRigidBody* body2, + btRigidBody* body1, + btRigidBody* body2, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA, const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB, const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1, const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0, btScalar& imp1) { (void)linvelA; (void)linvelB; (void)angvelB; (void)angvelA; - - imp0 = btScalar(0.); imp1 = btScalar(0.); @@ -59,86 +54,76 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( btAssert(len < SIMD_EPSILON); - //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - + btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); - const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); + const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1)); + const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1)); -// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv + // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv btScalar massTerm = btScalar(1.) / (invMassA + invMassB); - // calculate rhs (or error) terms - const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; - const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping; - + const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; + const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping; // dC/dv * dv = -C - + // jacobian * impulse = -error // //impulse = jacobianInverse * -error // inverting 2x2 symmetric system (offdiagonal are equal!) - // + // + btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB); + btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag); - btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); - //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet; //[a b] [d -c] //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) //[jA nD] * [imp0] = [dv0] //[nD jB] [imp1] [dv1] - } - - void btSolve2LinearConstraint::resolveBilateralPairConstraint( - btRigidBody* body1, - btRigidBody* body2, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, + btRigidBody* body1, + btRigidBody* body2, + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA, const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB, const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1, const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0, btScalar& imp1) { - (void)linvelA; (void)linvelB; (void)angvelA; (void)angvelB; - - imp0 = btScalar(0.); imp1 = btScalar(0.); @@ -148,42 +133,40 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( btAssert(len < SIMD_EPSILON); - //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - + btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); - const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); + const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1)); + const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1)); // calculate rhs (or error) terms - const btScalar dv0 = depthA * m_tau - vel0 * m_damping; - const btScalar dv1 = depthB * m_tau - vel1 * m_damping; + const btScalar dv0 = depthA * m_tau - vel0 * m_damping; + const btScalar dv1 = depthB * m_tau - vel1 * m_damping; // dC/dv * dv = -C - + // jacobian * impulse = -error // //impulse = jacobianInverse * -error // inverting 2x2 symmetric system (offdiagonal are equal!) - // + // + btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB); + btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag); - btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); - //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet; //[a b] [d -c] //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) @@ -191,9 +174,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( //[jA nD] * [imp0] = [dv0] //[nD jB] [imp1] [dv1] - if ( imp0 > btScalar(0.0)) + if (imp0 > btScalar(0.0)) { - if ( imp1 > btScalar(0.0) ) + if (imp1 > btScalar(0.0)) { //both positive } @@ -203,9 +186,10 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( // now imp0>0 imp1<0 imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) + if (imp0 > btScalar(0.0)) { - } else + } + else { imp0 = btScalar(0.); } @@ -216,24 +200,25 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( imp0 = btScalar(0.); imp1 = dv1 / jacB.getDiagonal(); - if ( imp1 <= btScalar(0.0) ) + if (imp1 <= btScalar(0.0)) { imp1 = btScalar(0.); // now imp0>0 imp1<0 imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) + if (imp0 > btScalar(0.0)) { - } else + } + else { imp0 = btScalar(0.); } - } else + } + else { } } } - /* void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, const btScalar invMassA, @@ -252,4 +237,3 @@ void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invI } */ - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h index e8bfabf86..fca8ecec8 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h @@ -19,20 +19,16 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btVector3.h" - class btRigidBody; - - /// constraint class used for lateral tyre friction. -class btSolve2LinearConstraint +class btSolve2LinearConstraint { - btScalar m_tau; - btScalar m_damping; + btScalar m_tau; + btScalar m_damping; public: - - btSolve2LinearConstraint(btScalar tau,btScalar damping) + btSolve2LinearConstraint(btScalar tau, btScalar damping) { m_tau = tau; m_damping = damping; @@ -40,52 +36,51 @@ public: // // solve unilateral constraint (equality, direct method) // - void resolveUnilateralPairConstraint( - btRigidBody* body0, + void resolveUnilateralPairConstraint( + btRigidBody* body0, btRigidBody* body1, const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, + const btMatrix3x3& world2B, - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1); + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA, const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB, const btVector3& angvelB, + const btVector3& rel_posA2, + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1, const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0, btScalar& imp1); // // solving 2x2 lcp problem (inequality, direct solution ) // void resolveBilateralPairConstraint( - btRigidBody* body0, - btRigidBody* body1, + btRigidBody* body0, + btRigidBody* body1, const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, + const btMatrix3x3& world2B, - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1); + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA, const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB, const btVector3& angvelB, + const btVector3& rel_posA2, -/* + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1, const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0, btScalar& imp1); + + /* void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, const btScalar invMassA, const btVector3& linvelA,const btVector3& angvelA, @@ -101,7 +96,6 @@ public: btScalar& imp0,btScalar& imp1); */ - }; -#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H +#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h index 27ccefe41..409aa8a08 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef BT_SOLVER_BODY_H #define BT_SOLVER_BODY_H -class btRigidBody; +class btRigidBody; #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" @@ -26,103 +26,99 @@ class btRigidBody; ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision #ifdef BT_USE_SSE #define USE_SIMD 1 -#endif // - +#endif // #ifdef USE_SIMD -struct btSimdScalar +struct btSimdScalar { - SIMD_FORCE_INLINE btSimdScalar() - { - - } - - SIMD_FORCE_INLINE btSimdScalar(float fl) - :m_vec128 (_mm_set1_ps(fl)) + SIMD_FORCE_INLINE btSimdScalar() { } - SIMD_FORCE_INLINE btSimdScalar(__m128 v128) - :m_vec128(v128) + SIMD_FORCE_INLINE btSimdScalar(float fl) + : m_vec128(_mm_set1_ps(fl)) { } - union + + SIMD_FORCE_INLINE btSimdScalar(__m128 v128) + : m_vec128(v128) { - __m128 m_vec128; - float m_floats[4]; - int m_ints[4]; - btScalar m_unusedPadding; + } + union { + __m128 m_vec128; + float m_floats[4]; + int m_ints[4]; + btScalar m_unusedPadding; }; - SIMD_FORCE_INLINE __m128 get128() + SIMD_FORCE_INLINE __m128 get128() { return m_vec128; } - SIMD_FORCE_INLINE const __m128 get128() const + SIMD_FORCE_INLINE const __m128 get128() const { return m_vec128; } - SIMD_FORCE_INLINE void set128(__m128 v128) + SIMD_FORCE_INLINE void set128(__m128 v128) { m_vec128 = v128; } - SIMD_FORCE_INLINE operator __m128() - { - return m_vec128; + SIMD_FORCE_INLINE operator __m128() + { + return m_vec128; } - SIMD_FORCE_INLINE operator const __m128() const - { - return m_vec128; - } - - SIMD_FORCE_INLINE operator float() const - { - return m_floats[0]; + SIMD_FORCE_INLINE operator const __m128() const + { + return m_vec128; } + SIMD_FORCE_INLINE operator float() const + { + return m_floats[0]; + } }; ///@brief Return the elementwise product of two btSimdScalar -SIMD_FORCE_INLINE btSimdScalar -operator*(const btSimdScalar& v1, const btSimdScalar& v2) +SIMD_FORCE_INLINE btSimdScalar +operator*(const btSimdScalar& v1, const btSimdScalar& v2) { - return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); + return btSimdScalar(_mm_mul_ps(v1.get128(), v2.get128())); } ///@brief Return the elementwise product of two btSimdScalar -SIMD_FORCE_INLINE btSimdScalar -operator+(const btSimdScalar& v1, const btSimdScalar& v2) +SIMD_FORCE_INLINE btSimdScalar +operator+(const btSimdScalar& v1, const btSimdScalar& v2) { - return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128())); + return btSimdScalar(_mm_add_ps(v1.get128(), v2.get128())); } - #else #define btSimdScalar btScalar #endif ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -ATTRIBUTE_ALIGNED16 (struct) btSolverBody +ATTRIBUTE_ALIGNED16(struct) +btSolverBody { BT_DECLARE_ALIGNED_ALLOCATOR(); - btTransform m_worldTransform; - btVector3 m_deltaLinearVelocity; - btVector3 m_deltaAngularVelocity; - btVector3 m_angularFactor; - btVector3 m_linearFactor; - btVector3 m_invMass; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; - btVector3 m_linearVelocity; - btVector3 m_angularVelocity; - btVector3 m_externalForceImpulse; - btVector3 m_externalTorqueImpulse; + btTransform m_worldTransform; + btVector3 m_deltaLinearVelocity; + btVector3 m_deltaAngularVelocity; + btVector3 m_angularFactor; + btVector3 m_linearFactor; + btVector3 m_invMass; + btVector3 m_pushVelocity; + btVector3 m_turnVelocity; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btVector3 m_externalForceImpulse; + btVector3 m_externalTorqueImpulse; - btRigidBody* m_originalBody; - void setWorldTransform(const btTransform& worldTransform) + btRigidBody* m_originalBody; + void setWorldTransform(const btTransform& worldTransform) { m_worldTransform = worldTransform; } @@ -131,56 +127,50 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody { return m_worldTransform; } - - - SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity ) const + SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity) const { if (m_originalBody) - velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity+m_externalTorqueImpulse).cross(rel_pos); + velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity + m_externalTorqueImpulse).cross(rel_pos); else - velocity.setValue(0,0,0); + velocity.setValue(0, 0, 0); } - - SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const + SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity) const { if (m_originalBody) - velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos); else - velocity.setValue(0,0,0); + velocity.setValue(0, 0, 0); } - SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const + SIMD_FORCE_INLINE void getAngularVelocity(btVector3 & angVel) const { if (m_originalBody) - angVel =m_angularVelocity+m_deltaAngularVelocity; + angVel = m_angularVelocity + m_deltaAngularVelocity; else - angVel.setValue(0,0,0); + angVel.setValue(0, 0, 0); } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) + SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude) { if (m_originalBody) { - m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor; + m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor); } } - SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent, btScalar impulseMagnitude) { if (m_originalBody) { - m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor; - m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor; + m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor); } } - - const btVector3& getDeltaLinearVelocity() const { return m_deltaLinearVelocity; @@ -191,20 +181,19 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody return m_deltaAngularVelocity; } - const btVector3& getPushVelocity() const + const btVector3& getPushVelocity() const { return m_pushVelocity; } - const btVector3& getTurnVelocity() const + const btVector3& getTurnVelocity() const { return m_turnVelocity; } - //////////////////////////////////////////////// ///some internal methods, don't use them - + btVector3& internalGetDeltaLinearVelocity() { return m_deltaLinearVelocity; @@ -229,7 +218,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody { m_invMass = invMass; } - + btVector3& internalGetPushVelocity() { return m_pushVelocity; @@ -240,67 +229,57 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody return m_turnVelocity; } - SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const + SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity) const { - velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos); } - SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const + SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3 & angVel) const { - angVel = m_angularVelocity+m_deltaAngularVelocity; + angVel = m_angularVelocity + m_deltaAngularVelocity; } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) + SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude) { if (m_originalBody) { - m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } - } - - - - - void writebackVelocity() - { - if (m_originalBody) - { - m_linearVelocity +=m_deltaLinearVelocity; - m_angularVelocity += m_deltaAngularVelocity; - - //m_originalBody->setCompanionId(-1); + m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor; + m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor); } } - - void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp) + void writebackVelocity() { - (void) timeStep; if (m_originalBody) { m_linearVelocity += m_deltaLinearVelocity; m_angularVelocity += m_deltaAngularVelocity; - + + //m_originalBody->setCompanionId(-1); + } + } + + void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp) + { + (void)timeStep; + if (m_originalBody) + { + m_linearVelocity += m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; + //correct the position/orientation based on push/turn recovery btTransform 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) + 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) { - // btQuaternion orn = m_worldTransform.getRotation(); - btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform); + // btQuaternion orn = m_worldTransform.getRotation(); + btTransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform); m_worldTransform = newTransform; } //m_worldTransform.setRotation(orn); //m_originalBody->setCompanionId(-1); } } - - - }; -#endif //BT_SOLVER_BODY_H - - +#endif //BT_SOLVER_BODY_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 5515e6b31..c7938df86 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef BT_SOLVER_CONSTRAINT_H #define BT_SOLVER_CONSTRAINT_H -class btRigidBody; +class btRigidBody; #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" #include "btJacobianEntry.h" @@ -25,56 +25,50 @@ class btRigidBody; //#define NO_FRICTION_TANGENTIALS 1 #include "btSolverBody.h" - ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint +ATTRIBUTE_ALIGNED16(struct) +btSolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal1; + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal1; - btVector3 m_relpos2CrossNormal; - btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always - btVector3 m_angularComponentA; - btVector3 m_angularComponentB; - - mutable btSimdScalar m_appliedPushImpulse; - mutable btSimdScalar m_appliedImpulse; + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; - btScalar m_friction; - btScalar m_jacDiagABInv; - btScalar m_rhs; - btScalar m_cfm; - - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_rhsPenetration; - union - { - void* m_originalContactPoint; - btScalar m_unusedPadding4; - int m_numRowsForNonContactConstraint; + mutable btSimdScalar m_appliedPushImpulse; + mutable btSimdScalar m_appliedImpulse; + + btScalar m_friction; + btScalar m_jacDiagABInv; + btScalar m_rhs; + btScalar m_cfm; + + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_rhsPenetration; + union { + void* m_originalContactPoint; + btScalar m_unusedPadding4; + int m_numRowsForNonContactConstraint; }; - int m_overrideNumSolverIterations; - int m_frictionIndex; + int m_overrideNumSolverIterations; + int m_frictionIndex; int m_solverBodyIdA; int m_solverBodyIdB; - - enum btSolverConstraintType + enum btSolverConstraintType { BT_SOLVER_CONTACT_1D = 0, BT_SOLVER_FRICTION_1D }; }; -typedef btAlignedObjectArray btConstraintArray; - - -#endif //BT_SOLVER_CONSTRAINT_H - - +typedef btAlignedObjectArray btConstraintArray; +#endif //BT_SOLVER_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 9f04f2805..ebe679c44 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -13,69 +13,63 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btTypedConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btSerializer.h" - #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f) btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) -:btTypedObject(type), -m_userConstraintType(-1), -m_userConstraintPtr((void*)-1), -m_breakingImpulseThreshold(SIMD_INFINITY), -m_isEnabled(true), -m_needsFeedback(false), -m_overrideNumSolverIterations(-1), -m_rbA(rbA), -m_rbB(getFixedBody()), -m_appliedImpulse(btScalar(0.)), -m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE), -m_jointFeedback(0) + : btTypedObject(type), + m_userConstraintType(-1), + m_userConstraintPtr((void*)-1), + m_breakingImpulseThreshold(SIMD_INFINITY), + m_isEnabled(true), + m_needsFeedback(false), + m_overrideNumSolverIterations(-1), + m_rbA(rbA), + m_rbB(getFixedBody()), + m_appliedImpulse(btScalar(0.)), + m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE), + m_jointFeedback(0) { } - -btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB) -:btTypedObject(type), -m_userConstraintType(-1), -m_userConstraintPtr((void*)-1), -m_breakingImpulseThreshold(SIMD_INFINITY), -m_isEnabled(true), -m_needsFeedback(false), -m_overrideNumSolverIterations(-1), -m_rbA(rbA), -m_rbB(rbB), -m_appliedImpulse(btScalar(0.)), -m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE), -m_jointFeedback(0) +btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA, btRigidBody& rbB) + : btTypedObject(type), + m_userConstraintType(-1), + m_userConstraintPtr((void*)-1), + m_breakingImpulseThreshold(SIMD_INFINITY), + m_isEnabled(true), + m_needsFeedback(false), + m_overrideNumSolverIterations(-1), + m_rbA(rbA), + m_rbB(rbB), + m_appliedImpulse(btScalar(0.)), + m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE), + m_jointFeedback(0) { } - - - btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact) { - if(lowLim > uppLim) + if (lowLim > uppLim) { return btScalar(1.0f); } - else if(lowLim == uppLim) + else if (lowLim == uppLim) { return btScalar(0.0f); } btScalar lim_fact = btScalar(1.0f); btScalar delta_max = vel / timeFact; - if(delta_max < btScalar(0.0f)) + if (delta_max < btScalar(0.0f)) { - if((pos >= lowLim) && (pos < (lowLim - delta_max))) + if ((pos >= lowLim) && (pos < (lowLim - delta_max))) { lim_fact = (lowLim - pos) / delta_max; } - else if(pos < lowLim) + else if (pos < lowLim) { lim_fact = btScalar(0.0f); } @@ -84,13 +78,13 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal lim_fact = btScalar(1.0f); } } - else if(delta_max > btScalar(0.0f)) + else if (delta_max > btScalar(0.0f)) { - if((pos <= uppLim) && (pos > (uppLim - delta_max))) + if ((pos <= uppLim) && (pos > (uppLim - delta_max))) { lim_fact = (uppLim - pos) / delta_max; } - else if(pos > uppLim) + else if (pos > uppLim) { lim_fact = btScalar(0.0f); } @@ -101,19 +95,19 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal } else { - lim_fact = btScalar(0.0f); + lim_fact = btScalar(0.0f); } return lim_fact; } ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const { - btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer; + btTypedConstraintData2* tcd = (btTypedConstraintData2*)dataBuffer; tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA); tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB); - char* name = (char*) serializer->findNameForPointer(this); + char* name = (char*)serializer->findNameForPointer(this); tcd->m_name = (char*)serializer->getUniquePointer(name); if (tcd->m_name) { @@ -124,10 +118,10 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali tcd->m_needsFeedback = m_needsFeedback; tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations; tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold; - tcd->m_isEnabled = m_isEnabled? 1: 0; - - tcd->m_userConstraintId =m_userConstraintId; - tcd->m_userConstraintType =m_userConstraintType; + tcd->m_isEnabled = m_isEnabled ? 1 : 0; + + tcd->m_userConstraintId = m_userConstraintId; + tcd->m_userConstraintType = m_userConstraintType; tcd->m_appliedImpulse = m_appliedImpulse; tcd->m_dbgDrawSize = m_dbgDrawSize; @@ -135,10 +129,10 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali tcd->m_disableCollisionsBetweenLinkedBodies = false; int i; - for (i=0;im_disableCollisionsBetweenLinkedBodies = true; - for (i=0;im_disableCollisionsBetweenLinkedBodies = true; @@ -147,17 +141,16 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali btRigidBody& btTypedConstraint::getFixedBody() { - static btRigidBody s_fixed(0, 0,0); - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); + static btRigidBody s_fixed(0, 0, 0); + s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); return s_fixed; } - void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor) { m_halfRange = (high - low) / 2.0f; m_center = btNormalizeAngle(low + m_halfRange); - m_softness = _softness; + m_softness = _softness; m_biasFactor = _biasFactor; m_relaxationFactor = _relaxationFactor; } @@ -174,7 +167,7 @@ void btAngularLimit::test(const btScalar angle) if (deviation < -m_halfRange) { m_solveLimit = true; - m_correction = - (deviation + m_halfRange); + m_correction = -(deviation + m_halfRange); m_sign = +1.0f; } else if (deviation > m_halfRange) @@ -186,7 +179,6 @@ void btAngularLimit::test(const btScalar angle) } } - btScalar btAngularLimit::getError() const { return m_correction * m_sign; diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 8a2a2d1ae..d30f3dee5 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -16,26 +16,24 @@ subject to the following restrictions: #ifndef BT_TYPED_CONSTRAINT_H #define BT_TYPED_CONSTRAINT_H - #include "LinearMath/btScalar.h" #include "btSolverConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #ifdef BT_USE_DOUBLE_PRECISION -#define btTypedConstraintData2 btTypedConstraintDoubleData -#define btTypedConstraintDataName "btTypedConstraintDoubleData" +#define btTypedConstraintData2 btTypedConstraintDoubleData +#define btTypedConstraintDataName "btTypedConstraintDoubleData" #else -#define btTypedConstraintData2 btTypedConstraintFloatData -#define btTypedConstraintDataName "btTypedConstraintFloatData" -#endif //BT_USE_DOUBLE_PRECISION - +#define btTypedConstraintData2 btTypedConstraintFloatData +#define btTypedConstraintDataName "btTypedConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION class btSerializer; //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility enum btTypedConstraintType { - POINT2POINT_CONSTRAINT_TYPE=3, + POINT2POINT_CONSTRAINT_TYPE = 3, HINGE_CONSTRAINT_TYPE, CONETWIST_CONSTRAINT_TYPE, D6_CONSTRAINT_TYPE, @@ -48,91 +46,88 @@ enum btTypedConstraintType MAX_CONSTRAINT_TYPE }; - enum btConstraintParams { - BT_CONSTRAINT_ERP=1, + BT_CONSTRAINT_ERP = 1, BT_CONSTRAINT_STOP_ERP, BT_CONSTRAINT_CFM, BT_CONSTRAINT_STOP_CFM }; #if 1 - #define btAssertConstrParams(_par) btAssert(_par) +#define btAssertConstrParams(_par) btAssert(_par) #else - #define btAssertConstrParams(_par) +#define btAssertConstrParams(_par) #endif - -ATTRIBUTE_ALIGNED16(struct) btJointFeedback +ATTRIBUTE_ALIGNED16(struct) +btJointFeedback { BT_DECLARE_ALIGNED_ALLOCATOR(); - btVector3 m_appliedForceBodyA; - btVector3 m_appliedTorqueBodyA; - btVector3 m_appliedForceBodyB; - btVector3 m_appliedTorqueBodyB; + btVector3 m_appliedForceBodyA; + btVector3 m_appliedTorqueBodyA; + btVector3 m_appliedForceBodyB; + btVector3 m_appliedTorqueBodyB; }; - ///TypedConstraint is the baseclass for Bullet constraints and vehicles -ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject +ATTRIBUTE_ALIGNED16(class) +btTypedConstraint : public btTypedObject { - int m_userConstraintType; + int m_userConstraintType; - union - { - int m_userConstraintId; + union { + int m_userConstraintId; void* m_userConstraintPtr; }; - btScalar m_breakingImpulseThreshold; - bool m_isEnabled; - bool m_needsFeedback; - int m_overrideNumSolverIterations; + btScalar m_breakingImpulseThreshold; + bool m_isEnabled; + bool m_needsFeedback; + int m_overrideNumSolverIterations; - - btTypedConstraint& operator=(btTypedConstraint& other) + btTypedConstraint& operator=(btTypedConstraint& other) { btAssert(0); - (void) other; + (void)other; return *this; } protected: - btRigidBody& m_rbA; - btRigidBody& m_rbB; - btScalar m_appliedImpulse; - btScalar m_dbgDrawSize; - btJointFeedback* m_jointFeedback; + btRigidBody& m_rbA; + btRigidBody& m_rbB; + btScalar m_appliedImpulse; + btScalar m_dbgDrawSize; + btJointFeedback* m_jointFeedback; ///internal method used by the constraint solver, don't use them directly btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - virtual ~btTypedConstraint() {}; - btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); - btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); + virtual ~btTypedConstraint(){}; + btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA); + btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA, btRigidBody & rbB); - struct btConstraintInfo1 { - int m_numConstraintRows,nub; + struct btConstraintInfo1 + { + int m_numConstraintRows, nub; }; static btRigidBody& getFixedBody(); - struct btConstraintInfo2 { + struct btConstraintInfo2 + { // integrator parameters: frames per second (1/stepsize), default error // reduction parameter (0..1). - btScalar fps,erp; + btScalar fps, erp; // for the first and second body, pointers to two (linear and angular) // n*3 jacobian sub matrices, stored by rows. these matrices will have // been initialized to 0 on entry. if the second body is zero then the // J2xx pointers may be 0. - btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; + btScalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis; // elements to jump from one row to the next in J's int rowskip; @@ -140,19 +135,19 @@ public: // right hand sides of the equation J*v = c + cfm * lambda. cfm is the // "constraint force mixing" vector. c is set to zero on entry, cfm is // set to a constant value (typically very small or zero) value on entry. - btScalar *m_constraintError,*cfm; + btScalar *m_constraintError, *cfm; // lo and hi limits for variables (set to -/+ infinity on entry). - btScalar *m_lowerLimit,*m_upperLimit; + btScalar *m_lowerLimit, *m_upperLimit; // number of solver iterations int m_numIterations; //damping of the velocity - btScalar m_damping; + btScalar m_damping; }; - int getOverrideNumSolverIterations() const + int getOverrideNumSolverIterations() const { return m_overrideNumSolverIterations; } @@ -165,60 +160,57 @@ public: } ///internal method used by the constraint solver, don't use them directly - virtual void buildJacobian() {}; + virtual void buildJacobian(){}; ///internal method used by the constraint solver, don't use them directly - virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) + virtual void setupSolverConstraint(btConstraintArray & ca, int solverBodyA, int solverBodyB, btScalar timeStep) { - (void)ca; - (void)solverBodyA; - (void)solverBodyB; - (void)timeStep; + (void)ca; + (void)solverBodyA; + (void)solverBodyB; + (void)timeStep; } - - ///internal method used by the constraint solver, don't use them directly - virtual void getInfo1 (btConstraintInfo1* info)=0; ///internal method used by the constraint solver, don't use them directly - virtual void getInfo2 (btConstraintInfo2* info)=0; + virtual void getInfo1(btConstraintInfo1 * info) = 0; ///internal method used by the constraint solver, don't use them directly - void internalSetAppliedImpulse(btScalar appliedImpulse) + virtual void getInfo2(btConstraintInfo2 * info) = 0; + + ///internal method used by the constraint solver, don't use them directly + void internalSetAppliedImpulse(btScalar appliedImpulse) { m_appliedImpulse = appliedImpulse; } ///internal method used by the constraint solver, don't use them directly - btScalar internalGetAppliedImpulse() + btScalar internalGetAppliedImpulse() { return m_appliedImpulse; } - - btScalar getBreakingImpulseThreshold() const + btScalar getBreakingImpulseThreshold() const { - return m_breakingImpulseThreshold; + return m_breakingImpulseThreshold; } - void setBreakingImpulseThreshold(btScalar threshold) + void setBreakingImpulseThreshold(btScalar threshold) { m_breakingImpulseThreshold = threshold; } - bool isEnabled() const + bool isEnabled() const { return m_isEnabled; } - void setEnabled(bool enabled) + void setEnabled(bool enabled) { - m_isEnabled=enabled; + m_isEnabled = enabled; } - ///internal method used by the constraint solver, don't use them directly - virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar /*timeStep*/) {}; + virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/, btSolverBody& /*bodyB*/, btScalar /*timeStep*/){}; - const btRigidBody& getRigidBodyA() const { return m_rbA; @@ -228,7 +220,7 @@ public: return m_rbB; } - btRigidBody& getRigidBodyA() + btRigidBody& getRigidBodyA() { return m_rbA; } @@ -239,15 +231,15 @@ public: int getUserConstraintType() const { - return m_userConstraintType ; + return m_userConstraintType; } - void setUserConstraintType(int userConstraintType) + void setUserConstraintType(int userConstraintType) { m_userConstraintType = userConstraintType; }; - void setUserConstraintId(int uid) + void setUserConstraintId(int uid) { m_userConstraintId = uid; } @@ -257,17 +249,17 @@ public: return m_userConstraintId; } - void setUserConstraintPtr(void* ptr) + void setUserConstraintPtr(void* ptr) { m_userConstraintPtr = ptr; } - void* getUserConstraintPtr() + void* getUserConstraintPtr() { return m_userConstraintPtr; } - void setJointFeedback(btJointFeedback* jointFeedback) + void setJointFeedback(btJointFeedback * jointFeedback) { m_jointFeedback = jointFeedback; } @@ -282,37 +274,36 @@ public: return m_jointFeedback; } - int getUid() const { - return m_userConstraintId; - } + return m_userConstraintId; + } - bool needsFeedback() const + 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) + void enableFeedback(bool needsFeedback) { m_needsFeedback = needsFeedback; } - ///getAppliedImpulse is an estimated total applied impulse. + ///getAppliedImpulse is an estimated total applied impulse. ///This feedback could be used to determine breaking constraints or playing sounds. - btScalar getAppliedImpulse() const + btScalar getAppliedImpulse() const { btAssert(m_needsFeedback); return m_appliedImpulse; } - btTypedConstraintType getConstraintType () const + btTypedConstraintType getConstraintType() const { return btTypedConstraintType(m_objectType); } - + void setDbgDrawSize(btScalar dbgDrawSize) { m_dbgDrawSize = dbgDrawSize; @@ -322,35 +313,34 @@ public: return m_dbgDrawSize; } - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///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) = 0; + virtual void setParam(int num, btScalar value, int axis = -1) = 0; ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const = 0; - - virtual int calculateSerializeBufferSize() const; + virtual btScalar 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, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; -// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits +// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits // all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI]) SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians) { - if(angleLowerLimitInRadians >= angleUpperLimitInRadians) + if (angleLowerLimitInRadians >= angleUpperLimitInRadians) { return angleInRadians; } - else if(angleInRadians < angleLowerLimitInRadians) + else if (angleInRadians < angleLowerLimitInRadians) { btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians)); btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians)); return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI); } - else if(angleInRadians > angleUpperLimitInRadians) + else if (angleInRadians > angleUpperLimitInRadians) { btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians)); btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians)); @@ -362,6 +352,8 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal } } +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btTypedConstraintFloatData { @@ -385,6 +377,8 @@ struct btTypedConstraintFloatData }; + + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 #define BT_BACKWARDS_COMPATIBLE_SERIALIZATION @@ -436,18 +430,17 @@ struct btTypedConstraintDoubleData }; +// clang-format on -SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const { return sizeof(btTypedConstraintData2); } - - class btAngularLimit { private: - btScalar + btScalar m_center, m_halfRange, m_softness, @@ -462,15 +455,16 @@ private: public: /// Default constructor initializes limit as inactive, allowing free constraint movement btAngularLimit() - :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) - {} + : 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. @@ -499,13 +493,13 @@ public: return m_relaxationFactor; } - /// Returns correction value evaluated when test() was invoked + /// Returns correction value evaluated when test() was invoked inline btScalar getCorrection() const { return m_correction; } - /// Returns sign value evaluated when test() was invoked + /// Returns sign value evaluated when test() was invoked inline btScalar getSign() const { return m_sign; @@ -533,9 +527,6 @@ public: btScalar getLow() const; btScalar getHigh() const; - }; - - -#endif //BT_TYPED_CONSTRAINT_H +#endif //BT_TYPED_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp index b009f41ae..42ed1fbb8 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp @@ -13,43 +13,38 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btUniversalConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" - - #define UNIV_EPS btScalar(0.01f) - // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2) -: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), - m_anchor(anchor), - m_axis1(axis1), - m_axis2(axis2) + : btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), + m_anchor(anchor), + m_axis1(axis1), + m_axis2(axis2) { // build frame basis // 6DOF constraint uses Euler angles and to define limits // it is assumed that rotational order is : // Z - first, allowed limits are (-PI,PI); - // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number + // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number // used to prevent constraint from instability on poles; // new position of X, allowed limits are (-PI,PI); // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs // Build the frame in world coordinate system first btVector3 zAxis = m_axis1.normalize(); btVector3 yAxis = m_axis2.normalize(); - btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + 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]); + frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); frameInW.setOrigin(anchor); // now get constraint frame in local coordinate systems m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; @@ -58,30 +53,28 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, setLinearLowerLimit(btVector3(0., 0., 0.)); setLinearUpperLimit(btVector3(0., 0., 0.)); setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS)); - setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS)); + setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS)); } -void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& axis2) +void btUniversalConstraint::setAxis(const btVector3& axis1, const btVector3& axis2) { - m_axis1 = axis1; - m_axis2 = axis2; + m_axis1 = axis1; + m_axis2 = axis2; btVector3 zAxis = axis1.normalized(); btVector3 yAxis = axis2.normalized(); - btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + 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]); + frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); frameInW.setOrigin(m_anchor); // now get constraint frame in local coordinate systems m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; - calculateTransforms(); + calculateTransforms(); } - - diff --git a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h index 9e7084104..8c24d93a6 100644 --- a/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h @@ -16,35 +16,32 @@ subject to the following restrictions: #ifndef BT_UNIVERSAL_CONSTRAINT_H #define BT_UNIVERSAL_CONSTRAINT_H - - #include "LinearMath/btVector3.h" #include "btTypedConstraint.h" #include "btGeneric6DofConstraint.h" - - /// Constraint similar to ODE Universal Joint /// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1) /// and Y (axis 2) -/// Description from ODE manual : -/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular. +/// Description from ODE manual : +/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular. /// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal." -ATTRIBUTE_ALIGNED16(class) btUniversalConstraint : public btGeneric6DofConstraint +ATTRIBUTE_ALIGNED16(class) +btUniversalConstraint : public btGeneric6DofConstraint { protected: - btVector3 m_anchor; - btVector3 m_axis1; - btVector3 m_axis2; + btVector3 m_anchor; + btVector3 m_axis1; + btVector3 m_axis2; + public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - + // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 - btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2); + btUniversalConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2); // access const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } @@ -56,10 +53,7 @@ public: void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); } void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); } - void setAxis( const btVector3& axis1, const btVector3& axis2); + void setAxis(const btVector3& axis1, const btVector3& axis2); }; - - -#endif // BT_UNIVERSAL_CONSTRAINT_H - +#endif // BT_UNIVERSAL_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btActionInterface.h b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btActionInterface.h index e1fea3a49..b5cac56cd 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btActionInterface.h +++ b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btActionInterface.h @@ -26,21 +26,16 @@ class btCollisionWorld; class btActionInterface { protected: - static btRigidBody& getFixedBody(); - - -public: +public: virtual ~btActionInterface() { } - virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0; + virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTimeStep) = 0; virtual void debugDraw(btIDebugDraw* debugDrawer) = 0; - }; -#endif //_BT_ACTION_INTERFACE_H - +#endif //_BT_ACTION_INTERFACE_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index fc85b4f86..dfbbdb154 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btDiscreteDynamicsWorld.h" //collision detection @@ -38,11 +37,9 @@ subject to the following restrictions: #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" @@ -56,57 +53,52 @@ int startHit=2; int firstHit=startHit; #endif -SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) +SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { int islandId; const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); return islandId; - } - class btSortConstraintOnIslandPredicate { - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId(rhs); - lIslandId0 = btGetConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } +public: + bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetConstraintIslandId(rhs); + lIslandId0 = btGetConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } }; struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback { - btContactSolverInfo* m_solverInfo; - btConstraintSolver* m_solver; - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; + btContactSolverInfo* m_solverInfo; + btConstraintSolver* m_solver; + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; btAlignedObjectArray m_bodies; btAlignedObjectArray m_manifolds; btAlignedObjectArray m_constraints; - InplaceSolverIslandCallback( - btConstraintSolver* solver, + btConstraintSolver* solver, btStackAlloc* stackAlloc, btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_sortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) + : m_solverInfo(NULL), + m_solver(solver), + m_sortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) { - } InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) @@ -116,34 +108,34 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal return *this; } - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer) + SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer) { btAssert(solverInfo); m_solverInfo = solverInfo; m_sortedConstraints = sortedConstraints; m_numConstraints = numConstraints; m_debugDrawer = debugDrawer; - m_bodies.resize (0); - m_manifolds.resize (0); - m_constraints.resize (0); + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); } - - virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) { - if (islandId<0) + if (islandId < 0) { ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else + m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, &m_sortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); + } + else { - //also add all non-contact constraints/joints for this island + //also add all non-contact constraints/joints for this island btTypedConstraint** startConstraint = 0; int numCurConstraints = 0; int i; //find the first constraint for this island - for (i=0;im_minimumSolverBatchSize<=1) + if (m_solverInfo->m_minimumSolverBatchSize <= 1) { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else + m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, startConstraint, numCurConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); + } + else { - - for (i=0;im_solverInfo->m_minimumSolverBatchSize) + if ((m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) { processConstraints(); - } else + } + else { //printf("deferred\n"); } } } } - void processConstraints() + void processConstraints() { + btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; + btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0; + btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; - btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; - btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; - btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; - - m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher); + m_solver->solveGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); m_bodies.resize(0); m_manifolds.resize(0); m_constraints.resize(0); - } - }; - - -btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) -:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), -m_sortedConstraints (), -m_solverIslandCallback ( NULL ), -m_constraintSolver(constraintSolver), -m_gravity(0,-10,0), -m_localTime(0), -m_fixedTimeStep(0), -m_synchronizeAllMotionStates(false), -m_applySpeculativeContactRestitution(false), -m_profileTimings(0), -m_latencyMotionStateInterpolation(true) +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) + : btDynamicsWorld(dispatcher, pairCache, collisionConfiguration), + m_sortedConstraints(), + m_solverIslandCallback(NULL), + m_constraintSolver(constraintSolver), + m_gravity(0, -10, 0), + m_localTime(0), + m_fixedTimeStep(0), + m_synchronizeAllMotionStates(false), + m_applySpeculativeContactRestitution(false), + m_profileTimings(0), + m_latencyMotionStateInterpolation(true) { if (!m_constraintSolver) { - void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); + void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver), 16); m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver; m_ownsConstraintSolver = true; - } else + } + else { m_ownsConstraintSolver = false; } { - void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16); + void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager), 16); m_islandManager = new (mem) btSimulationIslandManager(); } m_ownsIslandManager = true; { - void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16); - m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, 0, dispatcher); + void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback), 16); + m_solverIslandCallback = new (mem) InplaceSolverIslandCallback(m_constraintSolver, 0, dispatcher); } } - btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() { //only delete it when we created it if (m_ownsIslandManager) { m_islandManager->~btSimulationIslandManager(); - btAlignedFree( m_islandManager); + btAlignedFree(m_islandManager); } if (m_solverIslandCallback) { @@ -253,18 +241,17 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() } if (m_ownsConstraintSolver) { - m_constraintSolver->~btConstraintSolver(); btAlignedFree(m_constraintSolver); } } -void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) +void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) { -///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows -///to switch status _after_ adding kinematic objects to the world -///fix it for Bullet 3.x release - for (int i=0;igetDebugMode(); - if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) { drawConstraints = true; } } - if(drawConstraints) + if (drawConstraints) { - for(int i = getNumConstraints()-1; i>=0 ;i--) + for (int i = getNumConstraints() - 1; i >= 0; i--) { btTypedConstraint* constraint = getConstraint(i); debugDrawConstraint(constraint); } } - - - if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals))) + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals))) { int i; if (getDebugDrawer() && getDebugDrawer()->getDebugMode()) { - for (i=0;idebugDraw(m_debugDrawer); } } } - if (getDebugDrawer()) - getDebugDrawer()->flushLines(); - + if (getDebugDrawer()) + getDebugDrawer()->flushLines(); } -void btDiscreteDynamicsWorld::clearForces() +void btDiscreteDynamicsWorld::clearForces() { ///@todo: iterate over awake simulation islands! - for ( int i=0;iisActive()) @@ -349,8 +332,7 @@ void btDiscreteDynamicsWorld::applyGravity() } } - -void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) +void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) { btAssert(body); @@ -363,32 +345,32 @@ void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) { btTransform interpolatedTransform; btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(), - (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(), - interpolatedTransform); + body->getInterpolationLinearVelocity(), body->getInterpolationAngularVelocity(), + (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime * body->getHitFraction(), + interpolatedTransform); body->getMotionState()->setWorldTransform(interpolatedTransform); } } } - -void btDiscreteDynamicsWorld::synchronizeMotionStates() +void btDiscreteDynamicsWorld::synchronizeMotionStates() { -// BT_PROFILE("synchronizeMotionStates"); + // BT_PROFILE("synchronizeMotionStates"); if (m_synchronizeAllMotionStates) { //iterate over all collision objects - for ( int i=0;iisActive()) @@ -397,12 +379,10 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() } } - -int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +int btDiscreteDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) { startProfiling(timeStep); - int numSimulationSubSteps = 0; if (maxSubSteps) @@ -412,10 +392,11 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, m_localTime += timeStep; if (m_localTime >= fixedTimeStep) { - numSimulationSubSteps = int( m_localTime / fixedTimeStep); + numSimulationSubSteps = int(m_localTime / fixedTimeStep); m_localTime -= numSimulationSubSteps * fixedTimeStep; } - } else + } + else { //variable timestep fixedTimeStep = timeStep; @@ -425,7 +406,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, { numSimulationSubSteps = 0; maxSubSteps = 0; - } else + } + else { numSimulationSubSteps = 1; maxSubSteps = 1; @@ -435,28 +417,25 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, //process some debugging flags if (getDebugDrawer()) { - btIDebugDraw* debugDrawer = getDebugDrawer (); + btIDebugDraw* debugDrawer = getDebugDrawer(); gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; } if (numSimulationSubSteps) { - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; - saveKinematicState(fixedTimeStep*clampedSimulationSteps); + saveKinematicState(fixedTimeStep * clampedSimulationSteps); applyGravity(); - - - for (int i=0;iisActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + if (body->isActive() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(gravity); } } } -btVector3 btDiscreteDynamicsWorld::getGravity () const +btVector3 btDiscreteDynamicsWorld::getGravity() const { return m_gravity; } -void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) +void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) { - btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask); + btCollisionWorld::addCollisionObject(collisionObject, collisionFilterGroup, collisionFilterMask); } -void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { btRigidBody* body = btRigidBody::upcast(collisionObject); if (body) @@ -552,16 +528,15 @@ void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collision btCollisionWorld::removeCollisionObject(collisionObject); } -void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) +void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) { m_nonStaticRigidBodies.remove(body); btCollisionWorld::removeCollisionObject(body); } - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) { - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(m_gravity); } @@ -571,22 +546,23 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) if (!body->isStaticObject()) { m_nonStaticRigidBodies.push_back(body); - } else + } + else { body->setActivationState(ISLAND_SLEEPING); } bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); - int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); - int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(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); + addCollisionObject(body, collisionFilterGroup, collisionFilterMask); } } -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) { - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(m_gravity); } @@ -597,31 +573,29 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mas { m_nonStaticRigidBodies.push_back(body); } - else + else { body->setActivationState(ISLAND_SLEEPING); } - addCollisionObject(body,group,mask); + addCollisionObject(body, group, mask); } } - -void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) +void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) { BT_PROFILE("updateActions"); - for ( int i=0;iupdateAction( this, timeStep); + m_actions[i]->updateAction(this, timeStep); } } - -void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) +void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { BT_PROFILE("updateActivationState"); - for ( int i=0;iisStaticOrKinematicObject()) { body->setActivationState(ISLAND_SLEEPING); - } else + } + else { if (body->getActivationState() == ACTIVE_TAG) - body->setActivationState( WANTS_DEACTIVATION ); + body->setActivationState(WANTS_DEACTIVATION); if (body->getActivationState() == ISLAND_SLEEPING) { - body->setAngularVelocity(btVector3(0,0,0)); - body->setLinearVelocity(btVector3(0,0,0)); + body->setAngularVelocity(btVector3(0, 0, 0)); + body->setLinearVelocity(btVector3(0, 0, 0)); } - } - } else + } + else { if (body->getActivationState() != DISABLE_DEACTIVATION) - body->setActivationState( ACTIVE_TAG ); + body->setActivationState(ACTIVE_TAG); } } } } -void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) +void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies) { m_constraints.push_back(constraint); - //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?) - btAssert(&constraint->getRigidBodyA()!=&constraint->getRigidBodyB()); - + //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?) + btAssert(&constraint->getRigidBodyA() != &constraint->getRigidBodyB()); + if (disableCollisionsBetweenLinkedBodies) { constraint->getRigidBodyA().addConstraintRef(constraint); @@ -666,105 +641,98 @@ void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool d } } -void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) +void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) { m_constraints.remove(constraint); constraint->getRigidBodyA().removeConstraintRef(constraint); constraint->getRigidBodyB().removeConstraintRef(constraint); } -void btDiscreteDynamicsWorld::addAction(btActionInterface* action) +void btDiscreteDynamicsWorld::addAction(btActionInterface* action) { m_actions.push_back(action); } -void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) +void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) { m_actions.remove(action); } - -void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) +void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) { addAction(vehicle); } -void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) +void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) { removeAction(vehicle); } -void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) +void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) { addAction(character); } -void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) +void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) { removeAction(character); } - - - -void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { BT_PROFILE("solveConstraints"); - m_sortedConstraints.resize( m_constraints.size()); + m_sortedConstraints.resize(m_constraints.size()); int i; - for (i=0;isetup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer()); + m_solverIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), getDebugDrawer()); m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback); + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverIslandCallback); m_solverIslandCallback->processConstraints(); m_constraintSolver->allSolved(solverInfo, m_debugDrawer); } - -void btDiscreteDynamicsWorld::calculateSimulationIslands() +void btDiscreteDynamicsWorld::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher()); - { - //merge islands based on speculative contact manifolds too - for (int i=0;im_predictiveManifolds.size();i++) - { - btPersistentManifold* manifold = m_predictiveManifolds[i]; + { + //merge islands based on speculative contact manifolds too + for (int i = 0; i < this->m_predictiveManifolds.size(); i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; - const btCollisionObject* colObj0 = manifold->getBody0(); - const btCollisionObject* colObj1 = manifold->getBody1(); + const btCollisionObject* colObj0 = manifold->getBody0(); + const btCollisionObject* colObj1 = manifold->getBody1(); - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); + } + } + } { int i; int numConstraints = int(m_constraints.size()); - for (i=0;i< numConstraints ; i++ ) + for (i = 0; i < numConstraints; i++) { btTypedConstraint* constraint = m_constraints[i]; if (constraint->isEnabled()) @@ -775,7 +743,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); } } } @@ -783,51 +751,44 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() //Store the island id in each body getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - - } - - - class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback { public: - btCollisionObject* m_me; btScalar m_allowedPenetration; btOverlappingPairCache* m_pairCache; btDispatcher* m_dispatcher; public: - btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : - btCollisionWorld::ClosestConvexResultCallback(fromA,toA), - m_me(me), - m_allowedPenetration(0.0f), - m_pairCache(pairCache), - m_dispatcher(dispatcher) + btClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA, toA), + m_me(me), + m_allowedPenetration(0.0f), + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace) { if (convexResult.m_hitCollisionObject == m_me) return 1.0f; //ignore result if there is no contact response - if(!convexResult.m_hitCollisionObject->hasContactResponse()) + if (!convexResult.m_hitCollisionObject->hasContactResponse()) return 1.0f; - btVector3 linVelA,linVelB; - linVelA = m_convexToWorld-m_convexFromWorld; - linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); + btVector3 linVelA, linVelB; + linVelA = m_convexToWorld - m_convexFromWorld; + linVelB = btVector3(0, 0, 0); //toB.getOrigin()-fromB.getOrigin(); - btVector3 relativeVelocity = (linVelA-linVelB); + btVector3 relativeVelocity = (linVelA - linVelB); //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration) + if (convexResult.m_hitNormalLocal.dot(relativeVelocity) >= -m_allowedPenetration) return 1.f; - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); + return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace); } virtual bool needsCollision(btBroadphaseProxy* proxy0) const @@ -840,10 +801,13 @@ public: if (!ClosestConvexResultCallback::needsCollision(proxy0)) return false; - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + 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)) + if (m_dispatcher->needsResponse(m_me, otherObj)) { #if 0 ///don't do CCD when there are already contact points (touching contact/penetration) @@ -869,28 +833,24 @@ public: return false; } - - }; ///internal debugging variable. this value shouldn't be too high -int gNumClampedCcdMotions=0; +int gNumClampedCcdMotions = 0; - -void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep) +void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep) { btTransform predictedTrans; - for ( int i=0;isetHitFraction(1.f); if (body->isActive() && (!body->isStaticOrKinematicObject())) { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2(); if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { @@ -902,60 +862,55 @@ void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bo class StaticOnlyCallback : public btClosestNotMeConvexResultCallback { public: - - StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : - btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) + StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher) { } - virtual bool needsCollision(btBroadphaseProxy* proxy0) const + virtual bool needsCollision(btBroadphaseProxy* proxy0) const { - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; if (!otherObj->isStaticOrKinematicObject()) return false; return btClosestNotMeConvexResultCallback::needsCollision(proxy0); } }; - StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #else - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #endif //btConvexShape* convexShape = static_cast(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast(body->getCollisionShape()); - sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; + btSphereShape tmpSphere(body->getCcdSweptSphereRadius()); //btConvexShape* convexShape = static_cast(body->getCollisionShape()); + sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; btTransform modifiedPredictedTrans = predictedTrans; modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); + convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { - - btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction; + btVector3 distVec = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()) * sweepResults.m_closestHitFraction; btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld); - - btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject); - btMutexLock( &m_predictiveManifoldsMutex ); + btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject); + btMutexLock(&m_predictiveManifoldsMutex); m_predictiveManifolds.push_back(manifold); - btMutexUnlock( &m_predictiveManifoldsMutex ); + btMutexUnlock(&m_predictiveManifoldsMutex); - btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec; - btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB; + btVector3 worldPointB = body->getWorldTransform().getOrigin() + distVec; + btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse() * worldPointB; - btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance); + btManifoldPoint newPoint(btVector3(0, 0, 0), localPointB, sweepResults.m_hitNormalWorld, distance); bool isPredictive = true; 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; - } } } @@ -965,42 +920,39 @@ void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bo void btDiscreteDynamicsWorld::releasePredictiveContacts() { - BT_PROFILE( "release predictive contact manifolds" ); + 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(); + 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 ); - } + releasePredictiveContacts(); + if (m_nonStaticRigidBodies.size() > 0) + { + createPredictiveContactsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep); + } } -void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ) +void btDiscreteDynamicsWorld::integrateTransformsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep) { btTransform predictedTrans; - for (int i=0;isetHitFraction(1.f); if (body->isActive() && (!body->isStaticOrKinematicObject())) { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - - + btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2(); if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { @@ -1012,43 +964,40 @@ void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, class StaticOnlyCallback : public btClosestNotMeConvexResultCallback { public: - - StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : - btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) + StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher) { } - virtual bool needsCollision(btBroadphaseProxy* proxy0) const + virtual bool needsCollision(btBroadphaseProxy* proxy0) const { - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; if (!otherObj->isStaticOrKinematicObject()) return false; return btClosestNotMeConvexResultCallback::needsCollision(proxy0); } }; - StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #else - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #endif //btConvexShape* convexShape = static_cast(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast(body->getCollisionShape()); - sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; + btSphereShape tmpSphere(body->getCcdSweptSphereRadius()); //btConvexShape* convexShape = static_cast(body->getCollisionShape()); + sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; btTransform modifiedPredictedTrans = predictedTrans; modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); + convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { - //printf("clamped integration to hit fraction = %f\n",fraction); body->setHitFraction(sweepResults.m_closestHitFraction); - body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); + body->predictIntegratedTransform(timeStep * body->getHitFraction(), predictedTrans); body->setHitFraction(0.f); - body->proceedToTransform( predictedTrans); + body->proceedToTransform(predictedTrans); #if 0 btVector3 linVel = body->getLinearVelocity(); @@ -1075,50 +1024,45 @@ void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, //btScalar depth = 0.f; //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth); - #endif - continue; + continue; } } } - - body->proceedToTransform( predictedTrans); - + body->proceedToTransform(predictedTrans); } - } - } void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); - if (m_nonStaticRigidBodies.size() > 0) - { - integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep); - } + 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 + ///this should probably be switched on by default, but it is not well tested yet if (m_applySpeculativeContactRestitution) { BT_PROFILE("apply speculative contact restitution"); - for (int i=0;igetBody0()); btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1()); - for (int p=0;pgetNumContacts();p++) + for (int p = 0; p < manifold->getNumContacts(); 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 (combinedRestitution > 0 && pt.m_appliedImpulse != 0.f) //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f) { - btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution; + btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse * combinedRestitution; const btVector3& pos1 = pt.getPositionWorldOnA(); const btVector3& pos2 = pt.getPositionWorldOnB(); @@ -1127,23 +1071,19 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin(); if (body0) - body0->applyImpulse(imp,rel_pos0); + body0->applyImpulse(imp, rel_pos0); if (body1) - body1->applyImpulse(-imp,rel_pos1); + body1->applyImpulse(-imp, rel_pos1); } } } } } - - - - -void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { BT_PROFILE("predictUnconstraintMotion"); - for ( int i=0;iisStaticOrKinematicObject()) @@ -1152,179 +1092,171 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); + body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform()); } } } - -void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) +void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) { (void)timeStep; #ifndef BT_NO_PROFILE CProfileManager::Reset(); -#endif //BT_NO_PROFILE - +#endif //BT_NO_PROFILE } - - - - - void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) { bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; btScalar dbgDrawSize = constraint->getDbgDrawSize(); - if(dbgDrawSize <= btScalar(0.f)) + if (dbgDrawSize <= btScalar(0.f)) { return; } - switch(constraint->getConstraintType()) + switch (constraint->getConstraintType()) { case POINT2POINT_CONSTRAINT_TYPE: - { - btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; - btTransform tr; - tr.setIdentity(); - btVector3 pivot = p2pC->getPivotInA(); - pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - getDebugDrawer()->drawTransform(tr, dbgDrawSize); - // that ideally should draw the same frame - pivot = p2pC->getPivotInB(); - pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - } - break; + { + btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; + btTransform tr; + tr.setIdentity(); + btVector3 pivot = p2pC->getPivotInA(); + pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + getDebugDrawer()->drawTransform(tr, dbgDrawSize); + // that ideally should draw the same frame + pivot = p2pC->getPivotInB(); + pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + } + break; case HINGE_CONSTRAINT_TYPE: + { + btHingeConstraint* pHinge = (btHingeConstraint*)constraint; + btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + btScalar minAng = pHinge->getLowerLimit(); + btScalar maxAng = pHinge->getUpperLimit(); + if (minAng == maxAng) { - btHingeConstraint* pHinge = (btHingeConstraint*)constraint; - btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - btScalar minAng = pHinge->getLowerLimit(); - btScalar maxAng = pHinge->getUpperLimit(); - if(minAng == maxAng) - { - break; - } - bool drawSect = true; - if(!pHinge->hasLimit()) - { - minAng = btScalar(0.f); - maxAng = SIMD_2_PI; - drawSect = false; - } - if(drawLimits) - { - btVector3& center = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); - } + break; } - break; + bool drawSect = true; + if (!pHinge->hasLimit()) + { + minAng = btScalar(0.f); + maxAng = SIMD_2_PI; + drawSect = false; + } + if (drawLimits) + { + btVector3& center = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0, 0, 0), drawSect); + } + } + break; case CONETWIST_CONSTRAINT_TYPE: + { + btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; + btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) { - btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; - btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) + //const btScalar length = btScalar(5); + const btScalar length = dbgDrawSize; + static int nSegments = 8 * 4; + btScalar fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)(nSegments - 1) / btScalar(nSegments); + btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); + pPrev = tr * pPrev; + for (int i = 0; i < nSegments; i++) { - //const btScalar length = btScalar(5); - const btScalar length = dbgDrawSize; - static int nSegments = 8*4; - btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); - btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); - pPrev = tr * pPrev; - for (int i=0; iGetPointForAngle(fAngleInRadians, length); - pCur = tr * pCur; - getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); + fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)i / btScalar(nSegments); + btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length); + pCur = tr * pCur; + getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0, 0, 0)); - if (i%(nSegments/8) == 0) - getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); - - pPrev = pCur; - } - btScalar tws = pCT->getTwistSpan(); - btScalar twa = pCT->getTwistAngle(); - bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); - if(useFrameB) - { - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - } - else - { - tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - } - btVector3 pivot = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis1 = tr.getBasis().getColumn(1); - getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); + if (i % (nSegments / 8) == 0) + getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0, 0, 0)); + pPrev = pCur; } + btScalar tws = pCT->getTwistSpan(); + btScalar twa = pCT->getTwistAngle(); + bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); + if (useFrameB) + { + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + } + else + { + tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + } + btVector3 pivot = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis1 = tr.getBasis().getColumn(1); + getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, btVector3(0, 0, 0), true); } - break; + } + break; case D6_SPRING_CONSTRAINT_TYPE: case D6_CONSTRAINT_TYPE: + { + btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; + btTransform tr = p6DOF->getCalculatedTransformA(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformB(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) { - btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; - btTransform tr = p6DOF->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformA(); + const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); + btVector3 up = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; + btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; + btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; + btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0)); + axis = tr.getBasis().getColumn(1); + btScalar ay = p6DOF->getAngle(1); + btScalar az = p6DOF->getAngle(2); + btScalar cy = btCos(ay); + btScalar sy = btSin(ay); + btScalar cz = btCos(az); + btScalar sz = btSin(az); + btVector3 ref; + ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2]; + ref[1] = -sz * axis[0] + cz * axis[1]; + ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2]; tr = p6DOF->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) + btVector3 normal = -tr.getBasis().getColumn(0); + btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; + btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; + if (minFi > maxFi) { - tr = p6DOF->getCalculatedTransformA(); - const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); - btVector3 up = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; - btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; - btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; - btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; - getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); - axis = tr.getBasis().getColumn(1); - btScalar ay = p6DOF->getAngle(1); - btScalar az = p6DOF->getAngle(2); - btScalar cy = btCos(ay); - btScalar sy = btSin(ay); - btScalar cz = btCos(az); - btScalar sz = btSin(az); - btVector3 ref; - ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; - ref[1] = -sz*axis[0] + cz*axis[1]; - ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; - tr = p6DOF->getCalculatedTransformB(); - btVector3 normal = -tr.getBasis().getColumn(0); - btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; - btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; - if(minFi > maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); - } - else if(minFi < maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); - } - tr = p6DOF->getCalculatedTransformA(); - btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; - btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; - getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false); } + else if (minFi < maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true); + } + tr = p6DOF->getCalculatedTransformA(); + btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; + btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; + getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0)); } - break; + } + break; ///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage case D6_SPRING_2_CONSTRAINT_TYPE: { @@ -1342,9 +1274,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); @@ -1353,9 +1288,9 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) btScalar cz = btCos(az); btScalar sz = btSin(az); btVector3 ref; - ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; - ref[1] = -sz*axis[0] + cz*axis[1]; - ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; + ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2]; + ref[1] = -sz * axis[0] + cz * axis[1]; + ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2]; tr = p6DOF->getCalculatedTransformB(); btVector3 normal = -tr.getBasis().getColumn(0); btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; @@ -1377,42 +1312,38 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) break; } case SLIDER_CONSTRAINT_TYPE: + { + btSliderConstraint* pSlider = (btSliderConstraint*)constraint; + btTransform tr = pSlider->getCalculatedTransformA(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pSlider->getCalculatedTransformB(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) { - btSliderConstraint* pSlider = (btSliderConstraint*)constraint; - btTransform tr = pSlider->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pSlider->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB(); - btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); - btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); - getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis = tr.getBasis().getColumn(1); - btScalar a_min = pSlider->getLowerAngLimit(); - btScalar a_max = pSlider->getUpperAngLimit(); - const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true); - } + btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB(); + btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); + btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); + getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis = tr.getBasis().getColumn(1); + btScalar a_min = pSlider->getLowerAngLimit(); + btScalar a_max = pSlider->getUpperAngLimit(); + const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0, 0, 0), true); } - break; - default : + } + break; + default: break; } return; } - - - - -void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) { - btAlignedFree( m_constraintSolver); + btAlignedFree(m_constraintSolver); } m_ownsConstraintSolver = false; m_constraintSolver = solver; @@ -1424,8 +1355,7 @@ btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() return m_constraintSolver; } - -int btDiscreteDynamicsWorld::getNumConstraints() const +int btDiscreteDynamicsWorld::getNumConstraints() const { return int(m_constraints.size()); } @@ -1438,93 +1368,87 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const return m_constraints[index]; } - - -void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer) +void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer) { int i; //serialize all collision objects - for (i=0;igetInternalType() & btCollisionObject::CO_RIGID_BODY) { int len = colObj->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj); + serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, colObj); } } - for (i=0;icalculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(size,1); - const char* structType = constraint->serialize(chunk->m_oldPtr,serializer); - serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint); + btChunk* chunk = serializer->allocate(size, 1); + const char* structType = constraint->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, constraint); } } - - - -void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer) +void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer) { #ifdef BT_USE_DOUBLE_PRECISION - int len = sizeof(btDynamicsWorldDoubleData); - btChunk* chunk = serializer->allocate(len,1); - btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr; -#else//BT_USE_DOUBLE_PRECISION - int len = sizeof(btDynamicsWorldFloatData); - btChunk* chunk = serializer->allocate(len,1); - btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr; -#endif//BT_USE_DOUBLE_PRECISION + int len = sizeof(btDynamicsWorldDoubleData); + btChunk* chunk = serializer->allocate(len, 1); + btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr; +#else //BT_USE_DOUBLE_PRECISION + int len = sizeof(btDynamicsWorldFloatData); + btChunk* chunk = serializer->allocate(len, 1); + btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr; +#endif //BT_USE_DOUBLE_PRECISION - memset(worldInfo ,0x00,len); + memset(worldInfo, 0x00, len); - m_gravity.serialize(worldInfo->m_gravity); - worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau; - worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping; - worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction; - worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep; + m_gravity.serialize(worldInfo->m_gravity); + worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau; + worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping; + worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction; + worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep; - worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution; - worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction; - worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor; - worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp; + worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution; + worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction; + worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor; + worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp; - worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2; - worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm; - worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold; - worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp; + worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2; + worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm; + worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold; + worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp; - worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop; - worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor; - worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce; - worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold; + worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop; + worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor; + worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce; + worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold; - worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations; - worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode; - worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold; - worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize; + worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations; + worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode; + worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold; + worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize; - worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse; + 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)); + // 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 - const char* structType = "btDynamicsWorldFloatData"; -#endif//BT_USE_DOUBLE_PRECISION - serializer->finalizeChunk(chunk,structType,BT_DYNAMICSWORLD_CODE,worldInfo); + const char* structType = "btDynamicsWorldDoubleData"; +#else //BT_USE_DOUBLE_PRECISION + const char* structType = "btDynamicsWorldFloatData"; +#endif //BT_USE_DOUBLE_PRECISION + serializer->finalizeChunk(chunk, structType, BT_DYNAMICSWORLD_CODE, worldInfo); } -void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) +void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) { - serializer->startSerialization(); serializeDynamicsWorldInfo(serializer); @@ -1533,6 +1457,7 @@ void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) serializeRigidBodies(serializer); + serializeContactManifolds(serializer); + serializer->finishSerialization(); } - diff --git a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index b0d19f48a..7fe961921 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_DISCRETE_DYNAMICS_WORLD_H #define BT_DISCRETE_DYNAMICS_WORLD_H @@ -32,159 +31,153 @@ struct InplaceSolverIslandCallback; #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btThreads.h" - ///btDiscreteDynamicsWorld provides discrete rigid body simulation ///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController -ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld +ATTRIBUTE_ALIGNED16(class) +btDiscreteDynamicsWorld : public btDynamicsWorld { protected: - - btAlignedObjectArray m_sortedConstraints; - InplaceSolverIslandCallback* m_solverIslandCallback; + btAlignedObjectArray m_sortedConstraints; + InplaceSolverIslandCallback* m_solverIslandCallback; - btConstraintSolver* m_constraintSolver; + btConstraintSolver* m_constraintSolver; - btSimulationIslandManager* m_islandManager; + btSimulationIslandManager* m_islandManager; btAlignedObjectArray m_constraints; btAlignedObjectArray m_nonStaticRigidBodies; - btVector3 m_gravity; + btVector3 m_gravity; //for variable timesteps - btScalar m_localTime; - btScalar m_fixedTimeStep; + btScalar m_localTime; + btScalar m_fixedTimeStep; //for variable timesteps - bool m_ownsIslandManager; - bool m_ownsConstraintSolver; - bool m_synchronizeAllMotionStates; - bool m_applySpeculativeContactRestitution; + bool m_ownsIslandManager; + bool m_ownsConstraintSolver; + bool m_synchronizeAllMotionStates; + bool m_applySpeculativeContactRestitution; - btAlignedObjectArray m_actions; - - int m_profileTimings; + btAlignedObjectArray m_actions; - bool m_latencyMotionStateInterpolation; + int m_profileTimings; - btAlignedObjectArray m_predictiveManifolds; - btSpinMutex m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts + bool m_latencyMotionStateInterpolation; - 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(); + btAlignedObjectArray m_predictiveManifolds; + btSpinMutex m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts - virtual void solveConstraints(btContactSolverInfo& solverInfo); - - virtual void updateActivationState(btScalar timeStep); + virtual void predictUnconstraintMotion(btScalar timeStep); - void updateActions(btScalar timeStep); + void integrateTransformsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep); // can be called in parallel + virtual void integrateTransforms(btScalar timeStep); - void startProfiling(btScalar timeStep); + virtual void calculateSimulationIslands(); - virtual void internalSingleStepSimulation( btScalar timeStep); + virtual void solveConstraints(btContactSolverInfo & solverInfo); - void releasePredictiveContacts(); - void createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ); // can be called in parallel - virtual void createPredictiveContacts(btScalar timeStep); + virtual void updateActivationState(btScalar timeStep); - virtual void saveKinematicState(btScalar timeStep); + void updateActions(btScalar timeStep); - void serializeRigidBodies(btSerializer* serializer); + void startProfiling(btScalar timeStep); - void serializeDynamicsWorldInfo(btSerializer* serializer); + virtual void internalSingleStepSimulation(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); + + void serializeRigidBodies(btSerializer * serializer); + + void serializeDynamicsWorldInfo(btSerializer * serializer); public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those - btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + btDiscreteDynamicsWorld(btDispatcher * dispatcher, btBroadphaseInterface * pairCache, btConstraintSolver * constraintSolver, btCollisionConfiguration * collisionConfiguration); virtual ~btDiscreteDynamicsWorld(); ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); - - virtual void synchronizeMotionStates(); + virtual void synchronizeMotionStates(); ///this can be useful to synchronize a single rigid body -> graphics object - void synchronizeSingleMotionState(btRigidBody* body); + void synchronizeSingleMotionState(btRigidBody * body); - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); + virtual void addConstraint(btTypedConstraint * constraint, bool disableCollisionsBetweenLinkedBodies = false); - virtual void removeConstraint(btTypedConstraint* constraint); + virtual void removeConstraint(btTypedConstraint * constraint); - virtual void addAction(btActionInterface*); + virtual void addAction(btActionInterface*); - virtual void removeAction(btActionInterface*); - - btSimulationIslandManager* getSimulationIslandManager() + virtual void removeAction(btActionInterface*); + + btSimulationIslandManager* getSimulationIslandManager() { return m_islandManager; } - const btSimulationIslandManager* getSimulationIslandManager() const + const btSimulationIslandManager* getSimulationIslandManager() const { return m_islandManager; } - btCollisionWorld* getCollisionWorld() + btCollisionWorld* getCollisionWorld() { return this; } - virtual void setGravity(const btVector3& gravity); + virtual void setGravity(const btVector3& gravity); - virtual btVector3 getGravity () const; + virtual btVector3 getGravity() const; - virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::StaticFilter, 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); - virtual void addRigidBody(btRigidBody* body, int group, int mask); + virtual void addRigidBody(btRigidBody * body, int group, int mask); - virtual void removeRigidBody(btRigidBody* body); + virtual void removeRigidBody(btRigidBody * body); ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject - virtual void removeCollisionObject(btCollisionObject* collisionObject); + virtual void removeCollisionObject(btCollisionObject * collisionObject); + virtual void debugDrawConstraint(btTypedConstraint * constraint); - virtual void debugDrawConstraint(btTypedConstraint* constraint); + virtual void debugDrawWorld(); - virtual void debugDrawWorld(); - - virtual void setConstraintSolver(btConstraintSolver* solver); + virtual void setConstraintSolver(btConstraintSolver * solver); virtual btConstraintSolver* getConstraintSolver(); - - virtual int getNumConstraints() const; - virtual btTypedConstraint* getConstraint(int index) ; + virtual int getNumConstraints() const; + + virtual btTypedConstraint* getConstraint(int index); virtual const btTypedConstraint* getConstraint(int index) const; - - virtual btDynamicsWorldType getWorldType() const + virtual btDynamicsWorldType getWorldType() const { return BT_DISCRETE_DYNAMICS_WORLD; } - + ///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep. - virtual void clearForces(); + virtual void clearForces(); ///apply gravity, call this once per timestep - virtual void applyGravity(); + virtual void applyGravity(); - virtual void setNumTasks(int numTasks) + virtual void setNumTasks(int numTasks) { - (void) numTasks; + (void)numTasks; } ///obsolete, use updateActions instead @@ -194,15 +187,15 @@ public: } ///obsolete, use addAction instead - virtual void addVehicle(btActionInterface* vehicle); + virtual void addVehicle(btActionInterface * vehicle); ///obsolete, use removeAction instead - virtual void removeVehicle(btActionInterface* vehicle); + virtual void removeVehicle(btActionInterface * vehicle); ///obsolete, use addAction instead - virtual void addCharacter(btActionInterface* character); + virtual void addCharacter(btActionInterface * character); ///obsolete, use removeAction instead - virtual void removeCharacter(btActionInterface* character); + virtual void removeCharacter(btActionInterface * character); - void setSynchronizeAllMotionStates(bool synchronizeAll) + void setSynchronizeAllMotionStates(bool synchronizeAll) { m_synchronizeAllMotionStates = synchronizeAll; } @@ -215,18 +208,18 @@ public: { m_applySpeculativeContactRestitution = enable; } - + bool getApplySpeculativeContactRestitution() const { return m_applySpeculativeContactRestitution; } ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo) - virtual void serialize(btSerializer* serializer); + virtual void serialize(btSerializer * serializer); ///Interpolate motion state between previous and current transform, instead of current and next transform. ///This can relieve discontinuities in the rendering, due to penetrations - void setLatencyMotionStateInterpolation(bool latencyInterpolation ) + void setLatencyMotionStateInterpolation(bool latencyInterpolation) { m_latencyMotionStateInterpolation = latencyInterpolation; } @@ -236,4 +229,4 @@ public: } }; -#endif //BT_DISCRETE_DYNAMICS_WORLD_H +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp index 1d10bad92..8207b4713 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btDiscreteDynamicsWorldMt.h" //collision detection @@ -38,290 +37,227 @@ subject to the following restrictions: #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" - -struct InplaceSolverIslandCallbackMt : public btSimulationIslandManagerMt::IslandCallback -{ - btContactSolverInfo* m_solverInfo; - btConstraintSolver* m_solver; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - - InplaceSolverIslandCallbackMt( - btConstraintSolver* solver, - btStackAlloc* stackAlloc, - btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) - { - - } - - InplaceSolverIslandCallbackMt& operator=(InplaceSolverIslandCallbackMt& other) - { - btAssert(0); - (void)other; - return *this; - } - - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btIDebugDraw* debugDrawer) - { - btAssert(solverInfo); - m_solverInfo = solverInfo; - m_debugDrawer = debugDrawer; - } - - - virtual void processIsland( btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifolds, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - int islandId - ) - { - m_solver->solveGroup( bodies, - numBodies, - manifolds, - numManifolds, - constraints, - numConstraints, - *m_solverInfo, - m_debugDrawer, - m_dispatcher - ); - } - -}; - - /// /// btConstraintSolverPoolMt /// btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver() { - int i = 0; + 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; + 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 ) +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(); - } + 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 ) +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 ); + 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 ) +btConstraintSolverPoolMt::btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers) { - init( solvers, 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; - } + // 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 -) +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; + 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(); - } + 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, btCollisionConfiguration* collisionConfiguration) -: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) +btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, + btBroadphaseInterface* pairCache, + btConstraintSolverPoolMt* solverPool, + btConstraintSolver* constraintSolverMt, + btCollisionConfiguration* collisionConfiguration) + : btDiscreteDynamicsWorld(dispatcher, pairCache, solverPool, collisionConfiguration) { if (m_ownsIslandManager) { m_islandManager->~btSimulationIslandManager(); - btAlignedFree( m_islandManager); + btAlignedFree(m_islandManager); } - { - void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallbackMt),16); - m_solverIslandCallbackMt = new (mem) InplaceSolverIslandCallbackMt (m_constraintSolver, 0, dispatcher); - } { - void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16); + void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt), 16); btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt(); - im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize ); - m_islandManager = im; + im->setMinimumSolverBatchSize(m_solverInfo.m_minimumSolverBatchSize); + m_islandManager = im; } + m_constraintSolverMt = constraintSolverMt; } - btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt() { - if (m_solverIslandCallbackMt) - { - m_solverIslandCallbackMt->~InplaceSolverIslandCallbackMt(); - btAlignedFree(m_solverIslandCallbackMt); - } - if (m_ownsConstraintSolver) - { - m_constraintSolver->~btConstraintSolver(); - btAlignedFree(m_constraintSolver); - } } - void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo) { BT_PROFILE("solveConstraints"); - m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer()); m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); /// solve all the constraints for this island - btSimulationIslandManagerMt* im = static_cast(m_islandManager); - im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt ); + 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; + 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 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 ) +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 ); - } + 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 ) +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 ); - } + 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 ) +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 ); - } + 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/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h index 2f144cdda..dccf35d7a 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h +++ b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h @@ -13,7 +13,6 @@ subject to the following restrictions: 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 @@ -21,8 +20,6 @@ subject to the following restrictions: #include "btSimulationIslandManagerMt.h" #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" -struct InplaceSolverIslandCallbackMt; - /// /// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them. /// @@ -35,46 +32,43 @@ struct InplaceSolverIslandCallbackMt; class btConstraintSolverPoolMt : public btConstraintSolver { public: - // create the solvers for me - explicit btConstraintSolverPoolMt( int numSolvers ); + // create the solvers for me + explicit btConstraintSolverPoolMt(int numSolvers); - // pass in fully constructed solvers (destructor will delete them) - btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers ); + // pass in fully constructed solvers (destructor will delete them) + btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers); - virtual ~btConstraintSolverPoolMt(); + 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; + ///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; } + 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; + 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 ); + 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. @@ -85,50 +79,53 @@ private: /// - integrateTransforms /// - createPredictiveContacts /// -ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld +ATTRIBUTE_ALIGNED16(class) +btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld { protected: - InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt; + btConstraintSolver* m_constraintSolverMt; - virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE; + virtual void solveConstraints(btContactSolverInfo & solverInfo) BT_OVERRIDE; - virtual void predictUnconstraintMotion( btScalar timeStep ) BT_OVERRIDE; + virtual void predictUnconstraintMotion(btScalar timeStep) BT_OVERRIDE; - struct UpdaterCreatePredictiveContacts : public btIParallelForBody - { - btScalar timeStep; - btRigidBody** rigidBodies; - btDiscreteDynamicsWorldMt* world; + 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; + 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; + 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; + 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 - btCollisionConfiguration* collisionConfiguration - ); + btDiscreteDynamicsWorldMt(btDispatcher * dispatcher, + btBroadphaseInterface * pairCache, + btConstraintSolverPoolMt * solverPool, // 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 +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 42d8fc0de..eadd8c12e 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -24,150 +24,150 @@ class btActionInterface; class btConstraintSolver; class btDynamicsWorld; - /// Type for the callback for each tick -typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); +typedef void (*btInternalTickCallback)(btDynamicsWorld* world, btScalar timeStep); enum btDynamicsWorldType { - BT_SIMPLE_DYNAMICS_WORLD=1, - BT_DISCRETE_DYNAMICS_WORLD=2, - BT_CONTINUOUS_DYNAMICS_WORLD=3, - BT_SOFT_RIGID_DYNAMICS_WORLD=4, - BT_GPU_DYNAMICS_WORLD=5, - BT_SOFT_MULTIBODY_DYNAMICS_WORLD=6 + BT_SIMPLE_DYNAMICS_WORLD = 1, + BT_DISCRETE_DYNAMICS_WORLD = 2, + BT_CONTINUOUS_DYNAMICS_WORLD = 3, + BT_SOFT_RIGID_DYNAMICS_WORLD = 4, + 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. class btDynamicsWorld : public btCollisionWorld { - protected: - btInternalTickCallback m_internalTickCallback; - btInternalTickCallback m_internalPreTickCallback; - void* m_worldUserInfo; + btInternalTickCallback m_internalTickCallback; + btInternalTickCallback m_internalPreTickCallback; + void* m_worldUserInfo; - btContactSolverInfo m_solverInfo; + btContactSolverInfo m_solverInfo; public: - + btDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphase, btCollisionConfiguration* collisionConfiguration) + : btCollisionWorld(dispatcher, broadphase, collisionConfiguration), m_internalTickCallback(0), m_internalPreTickCallback(0), m_worldUserInfo(0) + { + } - btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) - :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0) + virtual ~btDynamicsWorld() + { + } + + ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds. + ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. + ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'. + ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant. + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)) = 0; + + virtual void debugDrawWorld() = 0; + + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies = false) + { + (void)constraint; + (void)disableCollisionsBetweenLinkedBodies; + } + + virtual void removeConstraint(btTypedConstraint* constraint) { (void)constraint; } + + virtual void addAction(btActionInterface* action) = 0; + + virtual void removeAction(btActionInterface* action) = 0; + + //once a rigidbody is added to the dynamics world, it will get this gravity assigned + //existing rigidbodies in the world get gravity assigned too, during this method + virtual void setGravity(const btVector3& gravity) = 0; + virtual btVector3 getGravity() const = 0; + + virtual void synchronizeMotionStates() = 0; + + virtual void addRigidBody(btRigidBody* body) = 0; + + virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0; + + virtual void removeRigidBody(btRigidBody* body) = 0; + + virtual void setConstraintSolver(btConstraintSolver* solver) = 0; + + virtual btConstraintSolver* getConstraintSolver() = 0; + + virtual int getNumConstraints() const { return 0; } + + virtual btTypedConstraint* getConstraint(int index) + { + (void)index; + return 0; + } + + virtual const btTypedConstraint* getConstraint(int index) const + { + (void)index; + return 0; + } + + virtual btDynamicsWorldType getWorldType() const = 0; + + virtual void clearForces() = 0; + + /// Set the callback for when an internal tick (simulation substep) happens, optional user info + void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo = 0, bool isPreTick = false) + { + if (isPreTick) { + m_internalPreTickCallback = cb; } - - virtual ~btDynamicsWorld() + else { + m_internalTickCallback = cb; } - - ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds. - ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. - ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'. - ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant. - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; - - virtual void debugDrawWorld() = 0; - - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) - { - (void)constraint; (void)disableCollisionsBetweenLinkedBodies; - } + m_worldUserInfo = worldUserInfo; + } - virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;} + void setWorldUserInfo(void* worldUserInfo) + { + m_worldUserInfo = worldUserInfo; + } - virtual void addAction(btActionInterface* action) = 0; + void* getWorldUserInfo() const + { + return m_worldUserInfo; + } - virtual void removeAction(btActionInterface* action) = 0; - - //once a rigidbody is added to the dynamics world, it will get this gravity assigned - //existing rigidbodies in the world get gravity assigned too, during this method - virtual void setGravity(const btVector3& gravity) = 0; - virtual btVector3 getGravity () const = 0; - - virtual void synchronizeMotionStates() = 0; - - virtual void addRigidBody(btRigidBody* body) = 0; - - virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0; - - virtual void removeRigidBody(btRigidBody* body) = 0; - - virtual void setConstraintSolver(btConstraintSolver* solver) = 0; - - virtual btConstraintSolver* getConstraintSolver() = 0; - - virtual int getNumConstraints() const { return 0; } - - virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } - - virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } - - virtual btDynamicsWorldType getWorldType() const=0; - - virtual void clearForces() = 0; - - /// Set the callback for when an internal tick (simulation substep) happens, optional user info - void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0,bool isPreTick=false) - { - if (isPreTick) - { - m_internalPreTickCallback = cb; - } else - { - m_internalTickCallback = cb; - } - m_worldUserInfo = worldUserInfo; - } - - void setWorldUserInfo(void* worldUserInfo) - { - m_worldUserInfo = worldUserInfo; - } - - void* getWorldUserInfo() const - { - return m_worldUserInfo; - } - - btContactSolverInfo& getSolverInfo() - { - return m_solverInfo; - } - - const btContactSolverInfo& getSolverInfo() const - { - return m_solverInfo; - } - - - ///obsolete, use addAction instead. - virtual void addVehicle(btActionInterface* vehicle) {(void)vehicle;} - ///obsolete, use removeAction instead - virtual void removeVehicle(btActionInterface* vehicle) {(void)vehicle;} - ///obsolete, use addAction instead. - virtual void addCharacter(btActionInterface* character) {(void)character;} - ///obsolete, use removeAction instead - virtual void removeCharacter(btActionInterface* character) {(void)character;} + btContactSolverInfo& getSolverInfo() + { + return m_solverInfo; + } + const btContactSolverInfo& getSolverInfo() const + { + return m_solverInfo; + } + ///obsolete, use addAction instead. + virtual void addVehicle(btActionInterface* vehicle) { (void)vehicle; } + ///obsolete, use removeAction instead + virtual void removeVehicle(btActionInterface* vehicle) { (void)vehicle; } + ///obsolete, use addAction instead. + virtual void addCharacter(btActionInterface* character) { (void)character; } + ///obsolete, use removeAction instead + virtual void removeCharacter(btActionInterface* character) { (void)character; } }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btDynamicsWorldDoubleData { - btContactSolverInfoDoubleData m_solverInfo; - btVector3DoubleData m_gravity; + btContactSolverInfoDoubleData m_solverInfo; + btVector3DoubleData m_gravity; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btDynamicsWorldFloatData { - btContactSolverInfoFloatData m_solverInfo; - btVector3FloatData m_gravity; + btContactSolverInfoFloatData m_solverInfo; + btVector3FloatData m_gravity; }; - -#endif //BT_DYNAMICS_WORLD_H - - +#endif //BT_DYNAMICS_WORLD_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp index ca0714fcf..f4bcabada 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -22,36 +22,34 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" //'temporarily' global variables -btScalar gDeactivationTime = btScalar(2.); -bool gDisableDeactivation = false; +btScalar gDeactivationTime = btScalar(2.); +bool gDisableDeactivation = false; static int uniqueId = 0; - btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) { setupRigidBody(constructionInfo); } -btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia) +btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia) { - btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); + btRigidBodyConstructionInfo cinfo(mass, motionState, collisionShape, localInertia); setupRigidBody(cinfo); } -void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) +void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) { - - m_internalType=CO_RIGID_BODY; + m_internalType = CO_RIGID_BODY; m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - m_angularFactor.setValue(1,1,1); - m_linearFactor.setValue(1,1,1); + m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + m_angularFactor.setValue(1, 1, 1); + m_linearFactor.setValue(1, 1, 1); m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); + setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; @@ -67,48 +65,44 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& if (m_optionalMotionState) { m_optionalMotionState->getWorldTransform(m_worldTransform); - } else + } + else { m_worldTransform = constructionInfo.m_startWorldTransform; } m_interpolationWorldTransform = m_worldTransform; - m_interpolationLinearVelocity.setValue(0,0,0); - m_interpolationAngularVelocity.setValue(0,0,0); - + m_interpolationLinearVelocity.setValue(0, 0, 0); + m_interpolationAngularVelocity.setValue(0, 0, 0); + //moved to btCollisionObject m_friction = constructionInfo.m_friction; m_rollingFriction = constructionInfo.m_rollingFriction; - m_spinningFriction = constructionInfo.m_spinningFriction; - + m_spinningFriction = constructionInfo.m_spinningFriction; + m_restitution = constructionInfo.m_restitution; - setCollisionShape( constructionInfo.m_collisionShape ); + setCollisionShape(constructionInfo.m_collisionShape); m_debugBodyId = uniqueId++; - + setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); updateInertiaTensor(); m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY; - m_deltaLinearVelocity.setZero(); m_deltaAngularVelocity.setZero(); - m_invMass = m_inverseMass*m_linearFactor; + m_invMass = m_inverseMass * m_linearFactor; m_pushVelocity.setZero(); m_turnVelocity.setZero(); - - - } - -void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) +void btRigidBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform) { - btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); + btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); } -void btRigidBody::saveKinematicState(btScalar timeStep) +void btRigidBody::saveKinematicState(btScalar timeStep) { //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities if (timeStep != btScalar(0.)) @@ -116,25 +110,22 @@ void btRigidBody::saveKinematicState(btScalar timeStep) //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform if (getMotionState()) getMotionState()->getWorldTransform(m_worldTransform); - btVector3 linVel,angVel; - - btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity); + btVector3 linVel, angVel; + + btTransformUtil::calculateVelocity(m_interpolationWorldTransform, m_worldTransform, timeStep, m_linearVelocity, m_angularVelocity); m_interpolationLinearVelocity = m_linearVelocity; m_interpolationAngularVelocity = m_angularVelocity; m_interpolationWorldTransform = m_worldTransform; //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ()); } } - -void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const + +void btRigidBody::getAabb(btVector3& aabbMin, btVector3& aabbMax) const { - getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax); + getCollisionShape()->getAabb(m_worldTransform, aabbMin, aabbMax); } - - - -void btRigidBody::setGravity(const btVector3& acceleration) +void btRigidBody::setGravity(const btVector3& acceleration) { if (m_inverseMass != btScalar(0.0)) { @@ -143,22 +134,14 @@ void btRigidBody::setGravity(const btVector3& acceleration) m_gravity_acceleration = acceleration; } - - - - - void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) { m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } - - - ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping -void btRigidBody::applyDamping(btScalar timeStep) +void btRigidBody::applyDamping(btScalar timeStep) { //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway @@ -168,8 +151,8 @@ void btRigidBody::applyDamping(btScalar timeStep) m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); #else - m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep); - m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep); + m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep); + m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep); #endif if (m_additionalDamping) @@ -182,7 +165,6 @@ void btRigidBody::applyDamping(btScalar timeStep) m_angularVelocity *= m_additionalDampingFactor; m_linearVelocity *= m_additionalDampingFactor; } - btScalar speed = m_linearVelocity.length(); if (speed < m_linearDamping) @@ -191,10 +173,11 @@ void btRigidBody::applyDamping(btScalar timeStep) if (speed > dampVel) { btVector3 dir = m_linearVelocity.normalized(); - m_linearVelocity -= dir * dampVel; - } else + m_linearVelocity -= dir * dampVel; + } + else { - m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } } @@ -205,30 +188,28 @@ void btRigidBody::applyDamping(btScalar timeStep) if (angSpeed > angDampVel) { btVector3 dir = m_angularVelocity.normalized(); - m_angularVelocity -= dir * angDampVel; - } else + m_angularVelocity -= dir * angDampVel; + } + else { - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } } } } - void btRigidBody::applyGravity() { if (isStaticOrKinematicObject()) return; - - applyCentralForce(m_gravity); + applyCentralForce(m_gravity); } void btRigidBody::proceedToTransform(const btTransform& newTrans) { - setCenterOfMassTransform( newTrans ); + setCenterOfMassTransform(newTrans); } - void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { @@ -236,7 +217,8 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; m_inverseMass = btScalar(0.); - } else + } + else { m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); m_inverseMass = btScalar(1.0) / mass; @@ -244,50 +226,45 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) //Fg = m * a m_gravity = mass * m_gravity_acceleration; - - m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0), - inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0), - inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0)); - m_invMass = m_linearFactor*m_inverseMass; + m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), + inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), + inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); + + m_invMass = m_linearFactor * m_inverseMass; } - -void btRigidBody::updateInertiaTensor() +void btRigidBody::updateInertiaTensor() { m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); } - - btVector3 btRigidBody::getLocalInertia() const { - btVector3 inertiaLocal; const btVector3 inertia = m_invInertiaLocal; inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), - inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), - inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); + inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), + inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); return inertiaLocal; } inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt, - const btMatrix3x3 &I) + const btMatrix3x3& I) { - const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0); + const btVector3 w2 = I * w1 + w1.cross(I * w1) * dt - (T * dt + I * w0); return w2; } inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt, - const btMatrix3x3 &I) + const btMatrix3x3& I) { - btMatrix3x3 w1x, Iw1x; - const btVector3 Iwi = (I*w1); + const btVector3 Iwi = (I * w1); w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]); Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]); - const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt; + const btMatrix3x3 dfw1 = I + (w1x * I - Iw1x) * dt; return dfw1; } @@ -295,58 +272,55 @@ btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForc { btVector3 inertiaLocal = getLocalInertia(); btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); - btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); + btVector3 tmp = inertiaTensorWorld * getAngularVelocity(); btVector3 gf = getAngularVelocity().cross(tmp); btScalar l2 = gf.length2(); - if (l2>maxGyroscopicForce*maxGyroscopicForce) + if (l2 > maxGyroscopicForce * maxGyroscopicForce) { - gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce; + gf *= btScalar(1.) / btSqrt(l2) * maxGyroscopicForce; } return gf; } - btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const -{ +{ btVector3 idl = getLocalInertia(); btVector3 omega1 = getAngularVelocity(); btQuaternion q = getWorldTransform().getRotation(); - + // Convert to body coordinates btVector3 omegab = quatRotate(q.inverse(), omega1); btMatrix3x3 Ib; - Ib.setValue(idl.x(),0,0, - 0,idl.y(),0, - 0,0,idl.z()); - - btVector3 ibo = Ib*omegab; + Ib.setValue(idl.x(), 0, 0, + 0, idl.y(), 0, + 0, 0, idl.z()); + + btVector3 ibo = Ib * omegab; // Residual vector btVector3 f = step * omegab.cross(ibo); - + btMatrix3x3 skew0; omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]); - btVector3 om = Ib*omegab; + btVector3 om = Ib * omegab; btMatrix3x3 skew1; - om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]); - + om.getSkewSymmetricMatrix(&skew1[0], &skew1[1], &skew1[2]); + // Jacobian - btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step; - -// btMatrix3x3 Jinv = J.inverse(); -// btVector3 omega_div = Jinv*f; + btMatrix3x3 J = Ib + (skew0 * Ib - skew1) * step; + + // btMatrix3x3 Jinv = J.inverse(); + // btVector3 omega_div = Jinv*f; btVector3 omega_div = J.solve33(f); - + // Single Newton-Raphson update - omegab = omegab - omega_div;//Solve33(J, f); + omegab = omegab - omega_div; //Solve33(J, f); // Back to world coordinates - btVector3 omega2 = quatRotate(q,omegab); - btVector3 gf = omega2-omega1; + btVector3 omega2 = quatRotate(q, omegab); + btVector3 gf = omega2 - omega1; return gf; } - - btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const { // use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior. @@ -361,7 +335,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con m_worldTransform.getBasis().transpose(); // use newtons method to find implicit solution for new angular velocity (w') - // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 + // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 // df/dw' = I + 1xIw'*step + w'xI*step btVector3 w1 = w0; @@ -383,8 +357,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con return gf; } - -void btRigidBody::integrateVelocities(btScalar step) +void btRigidBody::integrateVelocities(btScalar step) { if (isStaticOrKinematicObject()) return; @@ -393,30 +366,28 @@ void btRigidBody::integrateVelocities(btScalar step) m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; #define MAX_ANGVEL SIMD_HALF_PI - /// clamp angular velocity. collision calculations will fail on higher angular velocities + /// clamp angular velocity. collision calculations will fail on higher angular velocities btScalar angvel = m_angularVelocity.length(); - if (angvel*step > MAX_ANGVEL) + if (angvel * step > MAX_ANGVEL) { - m_angularVelocity *= (MAX_ANGVEL/step) /angvel; + m_angularVelocity *= (MAX_ANGVEL / step) / angvel; } - } btQuaternion btRigidBody::getOrientation() const { - btQuaternion orn; - m_worldTransform.getBasis().getRotation(orn); - return orn; + btQuaternion orn; + m_worldTransform.getBasis().getRotation(orn); + return orn; } - - + void btRigidBody::setCenterOfMassTransform(const btTransform& xform) { - if (isKinematicObject()) { m_interpolationWorldTransform = m_worldTransform; - } else + } + else { m_interpolationWorldTransform = xform; } @@ -426,10 +397,6 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform) updateInertiaTensor(); } - - - - void btRigidBody::addConstraintRef(btTypedConstraint* c) { ///disable collision with the 'other' body @@ -450,39 +417,39 @@ void btRigidBody::addConstraintRef(btTypedConstraint* c) { colObjB->setIgnoreCollisionCheck(colObjA, true); } - } + } } void btRigidBody::removeConstraintRef(btTypedConstraint* c) { int index = m_constraintRefs.findLinearSearch(c); //don't remove constraints that are not referenced - if(index < m_constraintRefs.size()) - { - m_constraintRefs.remove(c); - btCollisionObject* colObjA = &c->getRigidBodyA(); - btCollisionObject* colObjB = &c->getRigidBodyB(); - if (colObjA == this) - { - colObjA->setIgnoreCollisionCheck(colObjB, false); - } - else - { - colObjB->setIgnoreCollisionCheck(colObjA, false); - } - } + if (index < m_constraintRefs.size()) + { + m_constraintRefs.remove(c); + btCollisionObject* colObjA = &c->getRigidBodyA(); + btCollisionObject* colObjB = &c->getRigidBodyB(); + if (colObjA == this) + { + colObjA->setIgnoreCollisionCheck(colObjB, false); + } + else + { + colObjB->setIgnoreCollisionCheck(colObjA, false); + } + } } -int btRigidBody::calculateSerializeBufferSize() const +int btRigidBody::calculateSerializeBufferSize() const { int sz = sizeof(btRigidBodyData); return sz; } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const { - btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer; + btRigidBodyData* rbd = (btRigidBodyData*)dataBuffer; btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer); @@ -504,7 +471,7 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr; rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr; rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor; - rbd->m_linearSleepingThreshold=m_linearSleepingThreshold; + rbd->m_linearSleepingThreshold = m_linearSleepingThreshold; rbd->m_angularSleepingThreshold = m_angularSleepingThreshold; // Fill padding with zeros to appease msan. @@ -515,13 +482,9 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali return btRigidBodyDataName; } - - void btRigidBody::serializeSingleObject(class btSerializer* serializer) const { - btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1); + btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(), 1); const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this); + serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, (void*)this); } - - diff --git a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.h b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.h index 372245031..05f270a4b 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.h @@ -25,209 +25,195 @@ class btCollisionShape; class btMotionState; class btTypedConstraint; - extern btScalar gDeactivationTime; extern bool gDisableDeactivation; #ifdef BT_USE_DOUBLE_PRECISION -#define btRigidBodyData btRigidBodyDoubleData -#define btRigidBodyDataName "btRigidBodyDoubleData" +#define btRigidBodyData btRigidBodyDoubleData +#define btRigidBodyDataName "btRigidBodyDoubleData" #else -#define btRigidBodyData btRigidBodyFloatData -#define btRigidBodyDataName "btRigidBodyFloatData" -#endif //BT_USE_DOUBLE_PRECISION +#define btRigidBodyData btRigidBodyFloatData +#define btRigidBodyDataName "btRigidBodyFloatData" +#endif //BT_USE_DOUBLE_PRECISION - -enum btRigidBodyFlags +enum btRigidBodyFlags { BT_DISABLE_WORLD_GRAVITY = 1, ///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards. ///and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY ///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2, - BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD=4, - BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY=8, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD = 4, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY = 8, BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY, }; - ///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. ///It is recommended for performance and memory use to share btCollisionShape objects whenever possible. -///There are 3 types of rigid bodies: +///There are 3 types of rigid bodies: ///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics. ///- B) Fixed objects with zero mass. They are not moving (basically collision objects) -///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform. +///- C) Kinematic objects, which are objects without mass, but the user can move them. There is one-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform. ///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time. ///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects) -class btRigidBody : public btCollisionObject +class btRigidBody : public btCollisionObject { + btMatrix3x3 m_invInertiaTensorWorld; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btScalar m_inverseMass; + btVector3 m_linearFactor; - btMatrix3x3 m_invInertiaTensorWorld; - btVector3 m_linearVelocity; - btVector3 m_angularVelocity; - btScalar m_inverseMass; - btVector3 m_linearFactor; + btVector3 m_gravity; + btVector3 m_gravity_acceleration; + btVector3 m_invInertiaLocal; + btVector3 m_totalForce; + btVector3 m_totalTorque; - btVector3 m_gravity; - btVector3 m_gravity_acceleration; - btVector3 m_invInertiaLocal; - btVector3 m_totalForce; - btVector3 m_totalTorque; - - btScalar m_linearDamping; - btScalar m_angularDamping; + btScalar m_linearDamping; + btScalar m_angularDamping; - bool m_additionalDamping; - btScalar m_additionalDampingFactor; - btScalar m_additionalLinearDampingThresholdSqr; - btScalar m_additionalAngularDampingThresholdSqr; - btScalar m_additionalAngularDampingFactor; + bool m_additionalDamping; + btScalar m_additionalDampingFactor; + btScalar m_additionalLinearDampingThresholdSqr; + btScalar m_additionalAngularDampingThresholdSqr; + btScalar m_additionalAngularDampingFactor; - - btScalar m_linearSleepingThreshold; - btScalar m_angularSleepingThreshold; + btScalar m_linearSleepingThreshold; + btScalar m_angularSleepingThreshold; //m_optionalMotionState allows to automatic synchronize the world transform for active objects - btMotionState* m_optionalMotionState; + btMotionState* m_optionalMotionState; //keep track of typed constraints referencing this rigid body, to disable collision between linked bodies btAlignedObjectArray m_constraintRefs; - int m_rigidbodyFlags; - - int m_debugBodyId; - + int m_rigidbodyFlags; + + int m_debugBodyId; protected: - - ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity); - btVector3 m_deltaAngularVelocity; - btVector3 m_angularFactor; - btVector3 m_invMass; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; - + ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity); + btVector3 m_deltaAngularVelocity; + btVector3 m_angularFactor; + btVector3 m_invMass; + btVector3 m_pushVelocity; + btVector3 m_turnVelocity; public: - - ///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument) - ///You can use the motion state to synchronize the world transform between physics and graphics objects. + ///You can use the motion state to synchronize the world transform between physics and graphics objects. ///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state, ///m_startWorldTransform is only used when you don't provide a motion state. - struct btRigidBodyConstructionInfo + struct btRigidBodyConstructionInfo { - btScalar m_mass; + btScalar m_mass; ///When a motionState is provided, the rigid body will initialize its world transform from the motion state ///In this case, m_startWorldTransform is ignored. - btMotionState* m_motionState; - btTransform m_startWorldTransform; + btMotionState* m_motionState; + btTransform m_startWorldTransform; - btCollisionShape* m_collisionShape; - btVector3 m_localInertia; - btScalar m_linearDamping; - btScalar m_angularDamping; + btCollisionShape* m_collisionShape; + btVector3 m_localInertia; + btScalar m_linearDamping; + btScalar m_angularDamping; ///best simulation results when friction is non-zero - btScalar m_friction; + btScalar m_friction; ///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; + btScalar m_rollingFriction; + btScalar m_spinningFriction; //torsional friction around contact normal - btScalar m_linearSleepingThreshold; - btScalar m_angularSleepingThreshold; + ///best simulation results using zero restitution. + btScalar m_restitution; + + btScalar m_linearSleepingThreshold; + btScalar m_angularSleepingThreshold; //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete - bool m_additionalDamping; - btScalar m_additionalDampingFactor; - btScalar m_additionalLinearDampingThresholdSqr; - btScalar m_additionalAngularDampingThresholdSqr; - btScalar m_additionalAngularDampingFactor; + bool m_additionalDamping; + btScalar m_additionalDampingFactor; + btScalar m_additionalLinearDampingThresholdSqr; + btScalar m_additionalAngularDampingThresholdSqr; + btScalar m_additionalAngularDampingFactor; - btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)): - m_mass(mass), - m_motionState(motionState), - m_collisionShape(collisionShape), - m_localInertia(localInertia), - m_linearDamping(btScalar(0.)), - 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)), - m_additionalDamping(false), - m_additionalDampingFactor(btScalar(0.005)), - m_additionalLinearDampingThresholdSqr(btScalar(0.01)), - m_additionalAngularDampingThresholdSqr(btScalar(0.01)), - m_additionalAngularDampingFactor(btScalar(0.01)) + btRigidBodyConstructionInfo(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0)) : m_mass(mass), + m_motionState(motionState), + m_collisionShape(collisionShape), + m_localInertia(localInertia), + m_linearDamping(btScalar(0.)), + 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)), + m_additionalDamping(false), + m_additionalDampingFactor(btScalar(0.005)), + m_additionalLinearDampingThresholdSqr(btScalar(0.01)), + m_additionalAngularDampingThresholdSqr(btScalar(0.01)), + m_additionalAngularDampingFactor(btScalar(0.01)) { m_startWorldTransform.setIdentity(); } }; ///btRigidBody constructor using construction info - btRigidBody( const btRigidBodyConstructionInfo& constructionInfo); + btRigidBody(const btRigidBodyConstructionInfo& constructionInfo); - ///btRigidBody constructor for backwards compatibility. + ///btRigidBody constructor for backwards compatibility. ///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo) - btRigidBody( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)); - + btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0)); virtual ~btRigidBody() - { - //No constraints should point to this rigidbody - //Remove constraints from the dynamics world before you delete the related rigidbodies. - btAssert(m_constraintRefs.size()==0); - } + { + //No constraints should point to this rigidbody + //Remove constraints from the dynamics world before you delete the related rigidbodies. + btAssert(m_constraintRefs.size() == 0); + } protected: - ///setupRigidBody is only used internally by the constructor - void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); + void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); public: + void proceedToTransform(const btTransform& newTrans); - void proceedToTransform(const btTransform& newTrans); - ///to keep collision detection and dynamics separate we don't store a rigidbody pointer ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast - static const btRigidBody* upcast(const btCollisionObject* colObj) + static const btRigidBody* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY) + if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY) return (const btRigidBody*)colObj; return 0; } - static btRigidBody* upcast(btCollisionObject* colObj) + static btRigidBody* upcast(btCollisionObject* colObj) { - if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY) + if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY) return (btRigidBody*)colObj; return 0; } - - /// continuous collision detection needs prediction - void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ; - - void saveKinematicState(btScalar step); - - void applyGravity(); - - void setGravity(const btVector3& acceleration); - const btVector3& getGravity() const + /// continuous collision detection needs prediction + void predictIntegratedTransform(btScalar step, btTransform& predictedTransform); + + void saveKinematicState(btScalar step); + + void applyGravity(); + + void setGravity(const btVector3& acceleration); + + const btVector3& getGravity() const { return m_gravity_acceleration; } - void setDamping(btScalar lin_damping, btScalar ang_damping); + void setDamping(btScalar lin_damping, btScalar ang_damping); btScalar getLinearDamping() const { @@ -249,18 +235,20 @@ public: return m_angularSleepingThreshold; } - void applyDamping(btScalar timeStep); + void applyDamping(btScalar timeStep); - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const + { return m_collisionShape; } - SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { - return m_collisionShape; + SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() + { + return m_collisionShape; } - - void setMassProps(btScalar mass, const btVector3& inertia); - + + void setMassProps(btScalar mass, const btVector3& inertia); + const btVector3& getLinearFactor() const { return m_linearFactor; @@ -268,20 +256,21 @@ public: void setLinearFactor(const btVector3& linearFactor) { m_linearFactor = linearFactor; - m_invMass = m_linearFactor*m_inverseMass; + m_invMass = m_linearFactor * m_inverseMass; } - btScalar getInvMass() const { return m_inverseMass; } - const btMatrix3x3& getInvInertiaTensorWorld() const { - return m_invInertiaTensorWorld; - } - - void integrateVelocities(btScalar step); - - void setCenterOfMassTransform(const btTransform& xform); - - void applyCentralForce(const btVector3& force) + btScalar getInvMass() const { return m_inverseMass; } + const btMatrix3x3& getInvInertiaTensorWorld() const { - m_totalForce += force*m_linearFactor; + return m_invInertiaTensorWorld; + } + + void integrateVelocities(btScalar step); + + void setCenterOfMassTransform(const btTransform& xform); + + void applyCentralForce(const btVector3& force) + { + m_totalForce += force * m_linearFactor; } const btVector3& getTotalForce() const @@ -293,90 +282,93 @@ public: { return m_totalTorque; }; - + const btVector3& getInvInertiaDiagLocal() const { return m_invInertiaLocal; }; - void setInvInertiaDiagLocal(const btVector3& diagInvInertia) + void setInvInertiaDiagLocal(const btVector3& diagInvInertia) { m_invInertiaLocal = diagInvInertia; } - void setSleepingThresholds(btScalar linear,btScalar angular) + void setSleepingThresholds(btScalar linear, btScalar angular) { m_linearSleepingThreshold = linear; m_angularSleepingThreshold = angular; } - void applyTorque(const btVector3& torque) + void applyTorque(const btVector3& torque) { - m_totalTorque += torque*m_angularFactor; + m_totalTorque += torque * m_angularFactor; } - - void applyForce(const btVector3& force, const btVector3& rel_pos) + + void applyForce(const btVector3& force, const btVector3& rel_pos) { applyCentralForce(force); - applyTorque(rel_pos.cross(force*m_linearFactor)); + applyTorque(rel_pos.cross(force * m_linearFactor)); } - + void applyCentralImpulse(const btVector3& impulse) { - m_linearVelocity += impulse *m_linearFactor * m_inverseMass; + m_linearVelocity += impulse * m_linearFactor * m_inverseMass; } - - void applyTorqueImpulse(const btVector3& torque) + + void applyTorqueImpulse(const btVector3& torque) { - m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; } - - void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) + + void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) { if (m_inverseMass != btScalar(0.)) { applyCentralImpulse(impulse); if (m_angularFactor) { - applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor)); + applyTorqueImpulse(rel_pos.cross(impulse * m_linearFactor)); } } } - void clearForces() + void clearForces() { m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); } - - void updateInertiaTensor(); - - const btVector3& getCenterOfMassPosition() const { - return m_worldTransform.getOrigin(); + + void updateInertiaTensor(); + + const btVector3& getCenterOfMassPosition() const + { + return m_worldTransform.getOrigin(); } btQuaternion getOrientation() const; - - const btTransform& getCenterOfMassTransform() const { - return m_worldTransform; + + const btTransform& getCenterOfMassTransform() const + { + return m_worldTransform; } - const btVector3& getLinearVelocity() const { - return m_linearVelocity; + const btVector3& getLinearVelocity() const + { + return m_linearVelocity; } - const btVector3& getAngularVelocity() const { - return m_angularVelocity; + const btVector3& getAngularVelocity() const + { + return m_angularVelocity; } - inline void setLinearVelocity(const btVector3& lin_vel) - { + { m_updateRevision++; - m_linearVelocity = lin_vel; + m_linearVelocity = lin_vel; } - inline void setAngularVelocity(const btVector3& ang_vel) - { + inline void setAngularVelocity(const btVector3& ang_vel) + { m_updateRevision++; - m_angularVelocity = ang_vel; + m_angularVelocity = ang_vel; } btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const @@ -388,18 +380,13 @@ public: // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; } - void translate(const btVector3& v) + void translate(const btVector3& v) { - m_worldTransform.getOrigin() += v; + m_worldTransform.getOrigin() += v; } - - void getAabb(btVector3& aabbMin,btVector3& aabbMax) const; + void getAabb(btVector3& aabbMin, btVector3& aabbMax) const; - - - - SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const { btVector3 r0 = pos - getCenterOfMassPosition(); @@ -409,7 +396,6 @@ public: btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0); return m_inverseMass + normal.dot(vec); - } SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const @@ -418,26 +404,25 @@ public: return axis.dot(vec); } - SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep) + SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep) { - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) return; - if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) && - (getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold)) + if ((getLinearVelocity().length2() < m_linearSleepingThreshold * m_linearSleepingThreshold) && + (getAngularVelocity().length2() < m_angularSleepingThreshold * m_angularSleepingThreshold)) { m_deactivationTime += timeStep; - } else + } + else { - m_deactivationTime=btScalar(0.); + m_deactivationTime = btScalar(0.); setActivationState(0); } - } - SIMD_FORCE_INLINE bool wantsSleeping() + SIMD_FORCE_INLINE bool wantsSleeping() { - if (getActivationState() == DISABLE_DEACTIVATION) return false; @@ -445,41 +430,39 @@ public: if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) return false; - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) return true; - if (m_deactivationTime> gDeactivationTime) + if (m_deactivationTime > gDeactivationTime) { return true; } return false; } - - - const btBroadphaseProxy* getBroadphaseProxy() const + const btBroadphaseProxy* getBroadphaseProxy() const { return m_broadphaseHandle; } - btBroadphaseProxy* getBroadphaseProxy() + btBroadphaseProxy* getBroadphaseProxy() { return m_broadphaseHandle; } - void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy) + void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy) { m_broadphaseHandle = broadphaseProxy; } //btMotionState allows to automatic synchronize the world transform for active objects - btMotionState* getMotionState() + btMotionState* getMotionState() { return m_optionalMotionState; } - const btMotionState* getMotionState() const + const btMotionState* getMotionState() const { return m_optionalMotionState; } - void setMotionState(btMotionState* motionState) + void setMotionState(btMotionState* motionState) { m_optionalMotionState = motionState; if (m_optionalMotionState) @@ -487,27 +470,27 @@ public: } //for experimental overriding of friction/contact solver func - int m_contactSolverType; - int m_frictionSolverType; + int m_contactSolverType; + int m_frictionSolverType; - void setAngularFactor(const btVector3& angFac) + void setAngularFactor(const btVector3& angFac) { m_updateRevision++; m_angularFactor = angFac; } - void setAngularFactor(btScalar angFac) + void setAngularFactor(btScalar angFac) { m_updateRevision++; - m_angularFactor.setValue(angFac,angFac,angFac); + m_angularFactor.setValue(angFac, angFac, angFac); } - const btVector3& getAngularFactor() const + const btVector3& getAngularFactor() const { return m_angularFactor; } //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? - bool isInWorld() const + bool isInWorld() const { return (getBroadphaseProxy() != 0); } @@ -525,7 +508,7 @@ public: return m_constraintRefs.size(); } - void setFlags(int flags) + void setFlags(int flags) { m_rigidbodyFlags = flags; } @@ -535,12 +518,9 @@ public: return m_rigidbodyFlags; } - - - ///perform implicit force computation in world space btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const; - + ///perform implicit force computation in body space (inertial frame) btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const; @@ -550,70 +530,66 @@ public: /////////////////////////////////////////////// - virtual int calculateSerializeBufferSize() const; + 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; + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; virtual void serializeSingleObject(class btSerializer* serializer) const; - }; //@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btRigidBodyFloatData +struct btRigidBodyFloatData { - btCollisionObjectFloatData m_collisionObjectData; - btMatrix3x3FloatData m_invInertiaTensorWorld; - btVector3FloatData m_linearVelocity; - btVector3FloatData m_angularVelocity; - btVector3FloatData m_angularFactor; - btVector3FloatData m_linearFactor; - btVector3FloatData m_gravity; - btVector3FloatData m_gravity_acceleration; - btVector3FloatData m_invInertiaLocal; - btVector3FloatData m_totalForce; - btVector3FloatData 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; + btCollisionObjectFloatData m_collisionObjectData; + btMatrix3x3FloatData m_invInertiaTensorWorld; + btVector3FloatData m_linearVelocity; + btVector3FloatData m_angularVelocity; + btVector3FloatData m_angularFactor; + btVector3FloatData m_linearFactor; + btVector3FloatData m_gravity; + btVector3FloatData m_gravity_acceleration; + btVector3FloatData m_invInertiaLocal; + btVector3FloatData m_totalForce; + btVector3FloatData 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; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btRigidBodyDoubleData +struct btRigidBodyDoubleData { - btCollisionObjectDoubleData m_collisionObjectData; - btMatrix3x3DoubleData m_invInertiaTensorWorld; - btVector3DoubleData m_linearVelocity; - btVector3DoubleData m_angularVelocity; - btVector3DoubleData m_angularFactor; - btVector3DoubleData m_linearFactor; - btVector3DoubleData m_gravity; - btVector3DoubleData m_gravity_acceleration; - btVector3DoubleData m_invInertiaLocal; - btVector3DoubleData m_totalForce; - btVector3DoubleData 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]; + btCollisionObjectDoubleData m_collisionObjectData; + btMatrix3x3DoubleData m_invInertiaTensorWorld; + btVector3DoubleData m_linearVelocity; + btVector3DoubleData m_angularVelocity; + btVector3DoubleData m_angularFactor; + btVector3DoubleData m_linearFactor; + btVector3DoubleData m_gravity; + btVector3DoubleData m_gravity_acceleration; + btVector3DoubleData m_invInertiaLocal; + btVector3DoubleData m_totalForce; + btVector3DoubleData 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]; }; - - -#endif //BT_RIGIDBODY_H - +#endif //BT_RIGIDBODY_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 6f63b87c8..8103390fb 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -21,47 +21,40 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - /* Make sure this dummy function never changes so that it can be used by probes that are checking whether the library is actually installed. */ -extern "C" +extern "C" { - void btBulletDynamicsProbe (); - void btBulletDynamicsProbe () {} + void btBulletDynamicsProbe(); + void btBulletDynamicsProbe() {} } - - - -btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) -:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), -m_constraintSolver(constraintSolver), -m_ownsConstraintSolver(false), -m_gravity(0,0,-10) +btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) + : btDynamicsWorld(dispatcher, pairCache, collisionConfiguration), + m_constraintSolver(constraintSolver), + m_ownsConstraintSolver(false), + m_gravity(0, 0, -10) { - } - btSimpleDynamicsWorld::~btSimpleDynamicsWorld() { if (m_ownsConstraintSolver) - btAlignedFree( m_constraintSolver); + btAlignedFree(m_constraintSolver); } -int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +int btSimpleDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) { (void)fixedTimeStep; (void)maxSubSteps; - ///apply gravity, predict motion predictUnconstraintMotion(timeStep); - btDispatcherInfo& dispatchInfo = getDispatchInfo(); + btDispatcherInfo& dispatchInfo = getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; dispatchInfo.m_debugDraw = getDebugDrawer(); @@ -74,17 +67,17 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b if (numManifolds) { btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer(); - + btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; - m_constraintSolver->prepareSolve(0,numManifolds); - m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_dispatcher1); - m_constraintSolver->allSolved(infoGlobal,m_debugDrawer); + m_constraintSolver->prepareSolve(0, numManifolds); + m_constraintSolver->solveGroup(&getCollisionObjectArray()[0], getNumCollisionObjects(), manifoldPtr, numManifolds, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1); + m_constraintSolver->allSolved(infoGlobal, m_debugDrawer); } ///integrate transforms integrateTransforms(timeStep); - + updateAabbs(); synchronizeMotionStates(); @@ -92,29 +85,27 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b clearForces(); return 1; - } -void btSimpleDynamicsWorld::clearForces() +void btSimpleDynamicsWorld::clearForces() { ///@todo: iterate over awake simulation islands! - for ( int i=0;iclearForces(); } } -} +} - -void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) +void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for ( int i=0;isetGravity(m_gravity); @@ -155,37 +145,32 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) } } -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) +void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) { body->setGravity(m_gravity); if (body->getCollisionShape()) { - addCollisionObject(body,group,mask); + addCollisionObject(body, group, mask); } } - -void btSimpleDynamicsWorld::debugDrawWorld() +void btSimpleDynamicsWorld::debugDrawWorld() { - -} - -void btSimpleDynamicsWorld::addAction(btActionInterface* action) -{ - } -void btSimpleDynamicsWorld::removeAction(btActionInterface* action) +void btSimpleDynamicsWorld::addAction(btActionInterface* action) { - } +void btSimpleDynamicsWorld::removeAction(btActionInterface* action) +{ +} -void btSimpleDynamicsWorld::updateAabbs() +void btSimpleDynamicsWorld::updateAabbs() { btTransform predictedTrans; - for ( int i=0;iisActive() && (!body->isStaticObject())) { - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btVector3 minAabb, maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb); btBroadphaseInterface* bp = getBroadphase(); - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); + bp->setAabb(body->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1); } } } } -void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) +void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) { btTransform predictedTrans; - for ( int i=0;iisActive() && (!body->isStaticObject())) { body->predictIntegratedTransform(timeStep, predictedTrans); - body->proceedToTransform( predictedTrans); + body->proceedToTransform(predictedTrans); } } } } - - -void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - for ( int i=0;iisActive()) { body->applyGravity(); - body->integrateVelocities( timeStep); + body->integrateVelocities(timeStep); body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); + body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform()); } } } } } - -void btSimpleDynamicsWorld::synchronizeMotionStates() +void btSimpleDynamicsWorld::synchronizeMotionStates() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i #include "LinearMath/btQuickprof.h" - -SIMD_FORCE_INLINE int calcBatchCost( int bodies, int manifolds, int constraints ) +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; + // 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 ) +SIMD_FORCE_INLINE int calcBatchCost(const btSimulationIslandManagerMt::Island* island) { - return calcBatchCost( island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size() ); + 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; + 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 ); + 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) +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(); + int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag(); return islandId; } - -SIMD_FORCE_INLINE int btGetConstraintIslandId( const btTypedConstraint* lhs ) +SIMD_FORCE_INLINE int btGetConstraintIslandId1(const btTypedConstraint* lhs) { - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); - return islandId; + 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; - } + 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(); - } + 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 ) +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 ] ); - } + // 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 ) +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; + 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() ); - } + // 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 ); - } + 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 ) +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; + 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 ) +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; + 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; + // 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 ) +void btSimulationIslandManagerMt::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld) { + BT_PROFILE("buildIslands"); - BT_PROFILE("islandUnionFindAndQuickSort"); - 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 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"); + // printf("error in island management\n"); } 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; } } } @@ -328,43 +312,43 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl if (allSleeping) { int idx; - for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - colObj0->setActivationState( ISLAND_SLEEPING ); + colObj0->setActivationState(ISLAND_SLEEPING); } } - } else + } + else { - int idx; - for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - if ( colObj0->getActivationState() == ISLAND_SLEEPING) + if (colObj0->getActivationState() == ISLAND_SLEEPING) { - colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setActivationState(WANTS_DEACTIVATION); colObj0->setDeactivationTime(0.f); } } @@ -373,306 +357,338 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl } } - -void btSimulationIslandManagerMt::addBodiesToIslands( btCollisionWorld* collisionWorld ) +void btSimulationIslandManagerMt::addBodiesToIslands(btCollisionWorld* collisionWorld) { - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - int endIslandIndex = 1; - int startIslandIndex; - int numElem = getUnionFind().getNumElements(); + 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; + // 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 ); - } - } - } + // 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 ) +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 ); + // 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() ); + 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 ); - } - } - } - } + ///@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 ) +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 ); - } - } - } + // walk constraints + for (int i = 0; i < constraints.size(); i++) + { + // scatter constraints into various islands + btTypedConstraint* constraint = constraints[i]; + if (constraint->isEnabled()) + { + int islandId = btGetConstraintIslandId1(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() ); + // 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++; - } + // 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::serialIslandDispatch( btAlignedObjectArray* islandsPtr, IslandCallback* callback ) +void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams) { - BT_PROFILE( "serialIslandDispatch" ); - // serial dispatch - btAlignedObjectArray& islands = *islandsPtr; - for ( int i = 0; i < islands.size(); ++i ) - { - Island* island = islands[ i ]; - btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL; - btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL; - callback->processIsland( &island->bodyArray[ 0 ], - island->bodyArray.size(), - manifolds, - island->manifoldArray.size(), - constraintsPtr, - island->constraintArray.size(), - island->id - ); - } + 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* islandsPtr; - btSimulationIslandManagerMt::IslandCallback* callback; + btAlignedObjectArray& m_islandsPtr; + const btSimulationIslandManagerMt::SolverParams& m_solverParams; - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - for ( int i = iBegin; i < iEnd; ++i ) - { - btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ i ]; - btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL; - btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL; - callback->processIsland( &island->bodyArray[ 0 ], - island->bodyArray.size(), - manifolds, - island->manifoldArray.size(), - constraintsPtr, - island->constraintArray.size(), - island->id - ); - } - } + 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, IslandCallback* callback ) +void btSimulationIslandManagerMt::parallelIslandDispatch(btAlignedObjectArray* islandsPtr, const SolverParams& solverParams) { - BT_PROFILE( "parallelIslandDispatch" ); - int grainSize = 1; // iterations per task - UpdateIslandDispatcher dispatcher; - dispatcher.islandsPtr = islandsPtr; - dispatcher.callback = callback; - btParallelFor( 0, islandsPtr->size(), grainSize, dispatcher ); + 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, - IslandCallback* callback - ) +void btSimulationIslandManagerMt::buildAndProcessIslands(btDispatcher* dispatcher, + btCollisionWorld* collisionWorld, + btAlignedObjectArray& constraints, + const SolverParams& solverParams) { + BT_PROFILE("buildAndProcessIslands"); btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - buildIslands(dispatcher,collisionWorld); + buildIslands(dispatcher, collisionWorld); - BT_PROFILE("processIslands"); - - if(!getSplitIslands()) + if (!getSplitIslands()) { - btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer(); - int maxNumManifolds = dispatcher->getNumManifolds(); + btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer(); + int maxNumManifolds = dispatcher->getNumManifolds(); - for ( int i = 0; i < maxNumManifolds; i++ ) - { - btPersistentManifold* manifold = manifolds[ i ]; + 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() ); + 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; - callback->processIsland(&collisionObjects[0], - collisionObjects.size(), - manifolds, - maxNumManifolds, - constraintsPtr, - constraints.size(), - -1 - ); + ///@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(); + initIslandPools(); - //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - addBodiesToIslands( collisionWorld ); - addManifoldsToIslands( dispatcher ); - addConstraintsToIslands( constraints ); + //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. + // 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, callback ); + // 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/Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h index 9a781aaef..ab73a899f 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h +++ b/Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h @@ -19,7 +19,9 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" class btTypedConstraint; - +class btConstraintSolver; +struct btContactSolverInfo; +class btIDebugDraw; /// /// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager @@ -33,78 +35,78 @@ class btTypedConstraint; 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; + 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 IslandCallback - { - virtual ~IslandCallback() {}; + 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); - virtual void processIsland( btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifolds, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - int islandId - ) = 0; - }; - typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray* islands, IslandCallback* callback ); - static void serialIslandDispatch( btAlignedObjectArray* islandsPtr, IslandCallback* callback ); - static void parallelIslandDispatch( btAlignedObjectArray* islandsPtr, IslandCallback* callback ); 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; + 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(); - 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, IslandCallback* callback ); + virtual void buildAndProcessIslands(btDispatcher* dispatcher, + btCollisionWorld* collisionWorld, + btAlignedObjectArray& constraints, + const SolverParams& solverParams); - virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); + 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; - } + 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 - +#endif //BT_SIMULATION_ISLAND_MANAGER_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp index 8c7e499d8..3e210d752 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -21,7 +21,6 @@ */ - #include "btMultiBody.h" #include "btMultiBodyLink.h" #include "btMultiBodyLinkCollider.h" @@ -29,28 +28,29 @@ #include "LinearMath/btTransformUtil.h" #include "LinearMath/btSerializer.h" //#include "Bullet3Common/b3Logging.h" -// #define INCLUDE_GYRO_TERM +// #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 +{ +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 btMultiBody::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; } -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; - } +namespace +{ + #if 0 void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix, @@ -83,59 +83,62 @@ namespace { bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom); } #endif - -} +} // namespace // // 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_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) + 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_canWakeup(true), + 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_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); + m_baseForce.setValue(0, 0, 0); + m_baseTorque.setValue(0, 0, 0); + + clearConstraintForces(); + clearForcesAndTorques(); } btMultiBody::~btMultiBody() @@ -143,131 +146,125 @@ 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*/) + 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_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_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].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) + 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_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_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_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].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) + 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_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_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].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) + 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_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; @@ -278,278 +275,298 @@ void btMultiBody::setupSpherical(int i, 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_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].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; // - m_links[i].updateCacheMultiDof(); + 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) + 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_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; + m_links[i].m_eVector = parentComToThisComOffset; // btVector3 vecNonParallelToRotAxis(1, 0, 0); - if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) + 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 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]); + 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].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].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // m_links[i].updateCacheMultiDof(); // updateLinksDofOffsets(); + + m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized()); + m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized()); } 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) - + 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) + m_matrixBuf.resize(m_links.size() + 1); + for (int i = 0; i < m_vectorBuf.size(); i++) + { + m_vectorBuf[i].setValue(0, 0, 0); + } updateLinksDofOffsets(); } - -int btMultiBody::getParent(int i) const + +int btMultiBody::getParent(int link_num) const { - return m_links[i].m_parent; + return m_links[link_num].m_parent; } btScalar btMultiBody::getLinkMass(int i) const { - return m_links[i].m_mass; + return m_links[i].m_mass; } -const btVector3 & btMultiBody::getLinkInertia(int i) const +const btVector3 &btMultiBody::getLinkInertia(int i) const { - return m_links[i].m_inertiaLocal; + return m_links[i].m_inertiaLocal; } btScalar btMultiBody::getJointPos(int i) const { - return m_links[i].m_jointPos[0]; + return m_links[i].m_jointPos[0]; } btScalar btMultiBody::getJointVel(int i) const { - return m_realBuf[6 + m_links[i].m_dofOffset]; + return m_realBuf[6 + m_links[i].m_dofOffset]; } -btScalar * btMultiBody::getJointPosMultiDof(int i) +btScalar *btMultiBody::getJointPosMultiDof(int i) { return &m_links[i].m_jointPos[0]; } -btScalar * btMultiBody::getJointVelMultiDof(int i) +btScalar *btMultiBody::getJointVelMultiDof(int i) { return &m_realBuf[6 + m_links[i].m_dofOffset]; } -const btScalar * btMultiBody::getJointPosMultiDof(int i) const +const btScalar *btMultiBody::getJointPosMultiDof(int i) const { return &m_links[i].m_jointPos[0]; } -const btScalar * btMultiBody::getJointVelMultiDof(int i) const +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(); + m_links[i].m_jointPos[0] = q; + m_links[i].updateCacheMultiDof(); } -void btMultiBody::setJointPosMultiDof(int i, btScalar *q) + +void btMultiBody::setJointPosMultiDof(int i, const double *q) { - for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos) - m_links[i].m_jointPos[pos] = q[pos]; + for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos) + m_links[i].m_jointPos[pos] = (btScalar)q[pos]; - m_links[i].updateCacheMultiDof(); + m_links[i].updateCacheMultiDof(); } +void btMultiBody::setJointPosMultiDof(int i, const float *q) +{ + for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos) + m_links[i].m_jointPos[pos] = (btScalar)q[pos]; + + m_links[i].updateCacheMultiDof(); +} + + + void btMultiBody::setJointVel(int i, btScalar qdot) { - m_realBuf[6 + m_links[i].m_dofOffset] = qdot; + m_realBuf[6 + m_links[i].m_dofOffset] = qdot; } -void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot) +void btMultiBody::setJointVelMultiDof(int i, const double *qdot) { - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof]; + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof]; } -const btVector3 & btMultiBody::getRVector(int i) const +void btMultiBody::setJointVelMultiDof(int i, const float* qdot) { - return m_links[i].m_cachedRVector; + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof]; } -const btQuaternion & btMultiBody::getParentToLocalRot(int i) const +const btVector3 &btMultiBody::getRVector(int i) const { - return m_links[i].m_cachedRotParentToThis; + 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 { - btAssert(i>=-1); - btAssert(i=m_links.size())) + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) { - return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + 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); - } + 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(); + // 'result' is now in the base frame. transform it to world frame + result = quatRotate(getWorldToBaseRot().inverse(), result); + result += getBasePos(); - return result; + return result; } btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const { - btAssert(i>=-1); - btAssert(i=m_links.size())) + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) { - return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + 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); - } + 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())) + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) { - return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + 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 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())) + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) { - return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + 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)); - } + 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; + 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) + // Calculates the velocities of each link (and the base) in its local frame + const btQuaternion& base_rot = getWorldToBaseRot(); + omega[0] = quatRotate(base_rot, getBaseOmega()); + vel[0] = quatRotate(base_rot, getBaseVel()); + + for (int i = 0; i < num_links; ++i) { - const int parent = m_links[i].m_parent; + const btMultibodyLink& link = getLink(i); + const int parent = link.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]); + // transform parent vel into this frame, store in omega[i+1], vel[i+1] + spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.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) + // now add qidot * shat_i + const btScalar* jointVel = getJointVelMultiDof(i); + for (int dof = 0; dof < link.m_dofCount; ++dof) { - 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: - { - } + omega[i + 1] += jointVel[dof] * link.getAxisTop(dof); + vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof); } } } @@ -557,41 +574,48 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const 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]); + // 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]); - } + // 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]); - return 0.5f * result; + 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]); + // 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])); + 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]))); - } + 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; + return result; } void btMultiBody::clearConstraintForces() @@ -599,57 +623,55 @@ 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); - } + 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); + 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); + 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) + 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; + m_links[i].m_appliedForce += f; } void btMultiBody::addLinkTorque(int i, const btVector3 &t) { - m_links[i].m_appliedTorque += t; + m_links[i].m_appliedTorque += t; } void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f) { - m_links[i].m_appliedConstraintForce += f; + m_links[i].m_appliedConstraintForce += f; } void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t) { - m_links[i].m_appliedConstraintTorque += t; + m_links[i].m_appliedConstraintTorque += t; } - - void btMultiBody::addJointTorque(int i, btScalar Q) { - m_links[i].m_jointTorque[0] += Q; + m_links[i].m_jointTorque[0] += Q; } void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q) @@ -659,70 +681,72 @@ void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q) void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q) { - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + 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 +const btVector3 &btMultiBody::getLinkForce(int i) const { - return m_links[i].m_appliedForce; + return m_links[i].m_appliedForce; } -const btVector3 & btMultiBody::getLinkTorque(int i) const +const btVector3 &btMultiBody::getLinkTorque(int i) const { - return m_links[i].m_appliedTorque; + return m_links[i].m_appliedTorque; } btScalar btMultiBody::getJointTorque(int i) const { - return m_links[i].m_jointTorque[0]; + return m_links[i].m_jointTorque[0]; } -btScalar * btMultiBody::getJointTorqueMultiDof(int i) +btScalar *btMultiBody::getJointTorqueMultiDof(int i) { - return &m_links[i].m_jointTorque[0]; + 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? +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()); + 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; + 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) + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m, + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame) { - // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) - // and the base linear & angular accelerations. + // 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). + // 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. - // 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) @@ -731,96 +755,96 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar int num_links = getNumLinks(); - const btScalar DAMPING_K1_LINEAR = m_linearDamping; + 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; + const btScalar DAMPING_K2_ANGULAR = m_angularDamping; - btVector3 base_vel = getBaseVel(); - btVector3 base_omega = getBaseOmega(); + const btVector3 base_vel = getBaseVel(); + const btVector3 base_omega = getBaseOmega(); - // Temporary matrices/vectors -- use scratch space from caller - // so that we don't have to keep reallocating every frame + // 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); + 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) + 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; + // 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; + // 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]; + // 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]; + // 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; + // 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]; + // 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 + //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; + // 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. + // Start of the algorithm proper. - rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? + // 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 + if (m_fixedBase) { - 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)); + zeroAccSpatFrc[0].setZero(); + } + else + { + const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce; + const 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.; + const 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())); + 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 @@ -828,67 +852,66 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar 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]) - ); + 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]; + 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]; + 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]); + 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) + // vhat_i += qidot * shat_i + if (!m_useGlobalVelocities) { spatJointVel.setZero(); - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + 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; + 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_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]); + // we can now calculate chat_i + spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]); - // calculate zhat_i^A + // 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 )); - + //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 { @@ -906,27 +929,26 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // //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 + 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]) - ); + 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())); + 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(); @@ -937,63 +959,65 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar //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("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) + } + + // '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; + 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) + 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]; + 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) - ; + 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) + 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) + 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]; + const 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) + 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]; + 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(); + const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); + const btMatrix3x3 invD3x3(D3x3.inverse()); //unroll the loop? - for(int row = 0; row < 3; ++row) + for (int row = 0; row < 3; ++row) { - for(int col = 0; col < 3; ++col) - { + for (int col = 0; col < 3; ++col) + { invDi[row * 3 + col] = invD3x3[row][col]; } } @@ -1002,86 +1026,82 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar } default: { - } } //determine h*D^{-1} - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + 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]; - // + for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + // spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof]; } } - dyadTemp = spatInertia[i+1]; + 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]; + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const 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) + 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) + 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]; - } + 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] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i]; + + 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]; + spatForceVecTemps[0] += hDof * invD_times_Y[dof]; } - + fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); - - zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; - } + zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1]; + } - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (m_fixedBase) { - spatAcc[0].setZero(); - } - else + spatAcc[0].setZero(); + } + else { - if (num_links > 0) + 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(); + 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) + } + + // 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 @@ -1089,79 +1109,79 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // 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; + 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) + 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); + 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); } - btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + 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]; + 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]; + 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; + 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) + if (jointFeedbackInJointFrame) { //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); + angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); } - - if (gJointFeedbackInWorldSpace) + if (jointFeedbackInWorldSpace) { 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; - + 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 = angularBotVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec; - } - } + 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(); + // transform base accelerations back to the world frame. + const 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())); + const 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]; @@ -1184,26 +1204,25 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar //printf("]\n"); ///////////////// - // Final step: add the accelerations (times dt) to the velocities. + // Final step: add the accelerations (times dt) to the velocities. if (!isConstraintPass) { - if(dt > 0.) - applyDeltaVeeMultiDof(output, dt); - + if (dt > 0.) + applyDeltaVeeMultiDof(output, dt); } ///// //btScalar angularThres = 1; - //btScalar maxAngVel = 0.; + //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.) @@ -1220,60 +1239,77 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar ///// ///////////////////// - if(m_useGlobalVelocities) + if (m_useGlobalVelocities) { - for (int i = 0; i < num_links; ++i) + 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]); + + 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 + // vhat_i += qidot * shat_i spatJointVel.setZero(); - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + 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; + spatVel[i + 1] += spatJointVel; - - fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); + 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 +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) + 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 + + 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++) + for (int i = 0; i < 6; i++) { result[i] = 0.f; } @@ -1281,82 +1317,95 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo } /// 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 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; + 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]; + 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) + 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 + 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)); + 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 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; + 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); + 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 @@ -1375,155 +1424,152 @@ void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, in } void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, - btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v) const + 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 + // 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); + 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]; + 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; + // 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]; + // 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; + // 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; + // 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; + 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. - + // 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) + // -- set to force/torque on the base, zero otherwise + if (m_fixedBase) + { + zeroAccSpatFrc[0].setZero(); + } + else { - 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) + 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(); - } + zeroAccSpatFrc[i + 1].setZero(); + } // 'Downward' loop. - // (part of TreeForwardDynamics in Mirtich.) - for (int i = num_links - 1; i >= 0; --i) + // (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; + 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) + 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]) - ; + 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) + 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) + 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]; - } + 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) + // 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]; + spatForceVecTemps[0] += hDof * invD_times_Y[dof]; } - fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); - - zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; - } - + + zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1]; + } + // ptr to the joint accel part of the output - btScalar * joint_accel = output + 6; + btScalar *joint_accel = output + 6; + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) - - if (m_fixedBase) + if (m_fixedBase) { - spatAcc[0].setZero(); - } - else + 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) + // 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; + 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) + 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); + // + 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]); + 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]; - } + 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(); + // 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(); + 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]; @@ -1536,19 +1582,16 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar ///////////////// } - - - 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; + // 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) - // + 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]; @@ -1558,92 +1601,98 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd struct { //"exponential map" based on btTransformUtil::integrateTransform(..) - void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) + 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 - + //!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 + if (!baseBody) + angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok else angvel = omega; - - btScalar fAngle = angvel.length(); + + btScalar fAngle = angvel.length(); //limit the angular motion if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) { - fAngle = btScalar(0.5)*SIMD_HALF_PI / dt; + fAngle = btScalar(0.5) * SIMD_HALF_PI / dt; } - if ( fAngle < btScalar(0.001) ) + 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 ); + 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 ); + 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(); - + + 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]); + 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) + if (pq) pq += 7; - if(pqd) + if (pqd) pqd += 6; // Finally we can update m_jointPos for each of the m_links - for (int i = 0; i < num_links; ++i) + for (int i = 0; i < num_links; ++i) { - btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); - switch(m_links[i].m_jointType) + switch (m_links[i].m_jointType) { case btMultibodyLink::ePrismatic: case btMultibodyLink::eRevolute: { - btScalar jointVel = pJointVel[0]; + 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]); + 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(); + pJointPos[0] = jointOri.x(); + pJointPos[1] = jointOri.y(); + pJointPos[2] = jointOri.z(); + pJointPos[3] = jointOri.w(); break; } case btMultibodyLink::ePlanar: @@ -1660,122 +1709,135 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd default: { } - } m_links[i].updateCacheMultiDof(pq); - if(pq) + if (pq) pq += m_links[i].m_posVarCount; - if(pqd) + 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 + const btVector3 &contact_point, + const btVector3 &normal_ang, + const btVector3 &normal_lin, + btScalar *jac, + btAlignedObjectArray &scratch_r1, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) const { - // temporary space + // 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); + 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()); + 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; + //scratch_r.resize(m_dofCount); + //btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0; - btMatrix3x3 * rot_from_world = &scratch_m[0]; + scratch_r1.resize(m_dofCount+num_links); + btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0; + btScalar* links = num_links? &scratch_r1[m_dofCount] : 0; + int numLinksChildToRoot=0; + int l = link; + while (l != -1) + { + links[numLinksChildToRoot++]=l; + l = m_links[l].m_parent; + } + + 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 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); + 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]; + // 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_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) + // Set remaining jac values to zero for now. + for (int i = 6; i < 6 + m_dofCount; ++i) { - jac[i] = 0; - } + jac[i] = 0; + } - // Qdot coefficients, if necessary. - if (num_links > 0 && link > -1) { + // Qdot coefficients, if necessary. + if (num_links > 0 && link > -1) + { + // TODO: (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...) - // 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 a = 0; a < numLinksChildToRoot; a++) + { + int i = links[numLinksChildToRoot-1-a]; + // 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]; - // 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; + 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) + 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)); + 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)); + 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)); + 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)); + 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; } @@ -1783,258 +1845,265 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link, { } } - - } + } - // Now copy through to output. + // Now copy through to output. //printf("jac[%d] = ", link); - while (link != -1) + while (link != -1) { - for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + 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"); - } -} + link = m_links[link].m_parent; + } + //printf("]\n"); + } +} void btMultiBody::wakeUp() { - m_awake = true; + m_sleepTimer = 0; + m_awake = true; } void btMultiBody::goToSleep() { - m_awake = false; + m_awake = false; } void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) { extern bool gDisableDeactivation; - if (!m_canSleep || 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; + + + // 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) + 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(); - } + if (motion < SLEEP_EPSILON) + { + m_sleepTimer += timestep; + if (m_sleepTimer > SLEEP_TIMEOUT) + { + goToSleep(); + } + } + else + { + m_sleepTimer = 0; + if (m_canWakeup) + { + if (!m_awake) + wakeUp(); + } + } } - -void btMultiBody::forwardKinematics(btAlignedObjectArray& world_to_local,btAlignedObjectArray& local_origin) +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]; + 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[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); + 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.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) +void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray &world_to_local, btAlignedObjectArray &local_origin) { - world_to_local.resize(getNumLinks()+1); - local_origin.resize(getNumLinks()+1); - + 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()}; + 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])); - + 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; - + + 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()}; + 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])); - + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); } } } -int btMultiBody::calculateSerializeBufferSize() const +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 +///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); + 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) { - char* name = (char*) serializer->findNameForPointer(m_baseName); - mbd->m_baseName = (char*)serializer->getUniquePointer(name); - if (mbd->m_baseName) - { - serializer->serializeName(name); - } + serializer->serializeName(name); } - mbd->m_numLinks = this->getNumLinks(); - if (mbd->m_numLinks) + } + 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; i < numElem; i++, memPtr++) { - 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; dof < getLink(i).m_dofCount; dof++) { + getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]); + getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]); - memPtr->m_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; - 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_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); - + 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]; } - 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. + { + 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)); + memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); #endif - return btMultiBodyDataName; + return btMultiBodyDataName; } diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.h index 655165ac1..c0b0d003b 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.h @@ -21,7 +21,6 @@ */ - #ifndef BT_MULTIBODY_H #define BT_MULTIBODY_H @@ -31,116 +30,111 @@ #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" +#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 +#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 +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); + // + // 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 setupFixed(int i, //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); + 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 setupRevolute(int i, // 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 setupSpherical(int i, // 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 + 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) + btMultibodyLink &getLink(int index) { return m_links[index]; } - - void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base + void setBaseCollider(btMultiBodyLinkCollider * collider) //collider can be NULL to disable collision for the base { m_baseCollider = collider; } - const btMultiBodyLinkCollider* getBaseCollider() const + const btMultiBodyLinkCollider *getBaseCollider() const { return m_baseCollider; } - btMultiBodyLinkCollider* getBaseCollider() + btMultiBodyLinkCollider *getBaseCollider() { return m_baseCollider; } - btMultiBodyLinkCollider* getLinkCollider(int index) + const btMultiBodyLinkCollider *getLinkCollider(int index) const { if (index >= 0 && index < getNumLinks()) { @@ -149,61 +143,68 @@ public: 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; + btMultiBodyLinkCollider *getLinkCollider(int index) + { + if (index >= 0 && index < getNumLinks()) + { + return getLink(index).m_collider; + } + return 0; } - void setBaseWorldTransform(const btTransform& tr) + // + // 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 @@ -214,190 +215,186 @@ public: return tr; } - void setBaseVel(const btVector3 &vel) - { - - m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; + 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 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]; + 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) + // - // - // 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 getJointPos(int i) const; - btScalar getJointVel(int i) const; + btScalar *getJointVelMultiDof(int i); + btScalar *getJointPosMultiDof(int i); - btScalar * getJointVelMultiDof(int i); - btScalar * getJointPosMultiDof(int i); + const btScalar *getJointVelMultiDof(int i) const; + const btScalar *getJointPosMultiDof(int i) const; - 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, const double *q); + void setJointVelMultiDof(int i, const double *qdot); + void setJointPosMultiDof(int i, const float *q); + void setJointVelMultiDof(int i, const float *qdot); - 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]; + // + // 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() + /* 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) - // + // + // 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. + 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 &local_pos) const; + btVector3 localDirToWorld(int i, const btVector3 &local_dir) const; + btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const; + btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const; - // - // 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 &local_frame) 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. + // - // - // calculate kinetic energy and angular momentum - // useful for debugging. - // + btScalar getKineticEnergy() const; + btVector3 getAngularMomentum() const; - btScalar getKineticEnergy() const; - btVector3 getAngularMomentum() const; - + // + // set external forces and torques. Note all external forces/torques are given in the WORLD frame. + // - // - // set external forces and torques. Note all external forces/torques are given in the WORLD frame. - // - - void clearForcesAndTorques(); - void clearConstraintForces(); + void clearForcesAndTorques(); + void clearConstraintForces(); void clearVelocities(); - void addBaseForce(const btVector3 &f) - { - m_baseForce += f; + 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 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 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 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); + 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. + // - // - // 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). + // - // 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 - ); + btAlignedObjectArray & scratch_r, + btAlignedObjectArray & scratch_v, + btAlignedObjectArray & scratch_m, + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame + ); -///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); - } + ///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, false, false); + //} - // 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. + // 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) + 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; - } + { + m_deltaV[dof] += delta_vee[dof] * multiplier; + } } void processDeltaVeeMultiDof2() { - applyDeltaVeeMultiDof(&m_deltaV[0],1); + 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) + void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) { //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) // printf("%.4f ", delta_vee[dof]*multiplier); @@ -418,65 +415,78 @@ void addJointTorque(int i, btScalar Q); for (int dof = 0; dof < 6 + getNumDofs(); ++dof) { m_realBuf[dof] += delta_vee[dof] * multiplier; - btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity); + btClamp(m_realBuf[dof], -m_maxCoordinateVelocity, m_maxCoordinateVelocity); } - } + } - - - // timestep the positions (given current velocities). + // timestep the positions (given current velocities). void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); + // + // contacts + // - // - // 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. - // 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); } + 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; + 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) + // + // sleeping + // + void setCanSleep(bool canSleep) { - m_canSleep = canSleep; + if (m_canWakeup) + { + m_canSleep = canSleep; + } } - bool getCanSleep()const + bool getCanSleep() const { return m_canSleep; } - bool isAwake() const { return m_awake; } - void wakeUp(); - void goToSleep(); - void checkMotionAndSleepIfRequired(btScalar timestep); - + bool getCanWakeup() const + { + return m_canWakeup; + } + + void setCanWakeup(bool canWakeup) + { + m_canWakeup = canWakeup; + } + bool isAwake() const { return m_awake; } + void wakeUp(); + void goToSleep(); + void checkMotionAndSleepIfRequired(btScalar timestep); + bool hasFixedBase() const { - return m_fixedBase; + return m_fixedBase; + } + + void setFixedBase(bool fixedBase) + { + m_fixedBase = fixedBase; } int getCompanionId() const @@ -489,16 +499,16 @@ void addJointTorque(int i, btScalar Q); 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 + 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; + return m_linearDamping; } - void setLinearDamping( btScalar damp) + void setLinearDamping(btScalar damp) { m_linearDamping = damp; } @@ -506,11 +516,11 @@ void addJointTorque(int i, btScalar Q); { return m_angularDamping; } - void setAngularDamping( btScalar damp) + void setAngularDamping(btScalar damp) { m_angularDamping = damp; } - + bool getUseGyroTerm() const { return m_useGyroTerm; @@ -519,24 +529,24 @@ void addJointTorque(int i, btScalar Q); { m_useGyroTerm = useGyro; } - btScalar getMaxCoordinateVelocity() const + btScalar getMaxCoordinateVelocity() const { - return m_maxCoordinateVelocity ; + return m_maxCoordinateVelocity; } - void setMaxCoordinateVelocity(btScalar maxVel) + void setMaxCoordinateVelocity(btScalar maxVel) { m_maxCoordinateVelocity = maxVel; } - btScalar getMaxAppliedImpulse() const + btScalar getMaxAppliedImpulse() const { return m_maxAppliedImpulse; } - void setMaxAppliedImpulse(btScalar maxImp) + void setMaxAppliedImpulse(btScalar maxImp) { m_maxAppliedImpulse = maxImp; } - void setHasSelfCollision(bool hasSelfCollision) + void setHasSelfCollision(bool hasSelfCollision) { m_hasSelfCollision = hasSelfCollision; } @@ -545,7 +555,6 @@ void addJointTorque(int i, btScalar Q); return m_hasSelfCollision; } - void finalizeMultiDof(); void useRK4Integration(bool use) { m_useRK4 = use; } @@ -561,126 +570,132 @@ void addJointTorque(int i, btScalar Q); { __posUpdated = updated; } - + //internalNeedsJointFeedback is for internal use only bool internalNeedsJointFeedback() const { return m_internalNeedsJointFeedback; } - void forwardKinematics(btAlignedObjectArray& scratch_q,btAlignedObjectArray& scratch_m); + void forwardKinematics(btAlignedObjectArray& world_to_local, btAlignedObjectArray & local_origin); - void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; + void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const; - void updateCollisionObjectWorldTransforms(btAlignedObjectArray& scratch_q,btAlignedObjectArray& scratch_m); - - virtual int calculateSerializeBufferSize() const; + void updateCollisionObjectWorldTransforms(btAlignedObjectArray & world_to_local, btAlignedObjectArray & local_origin); + + 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; + virtual const char *serialize(void *dataBuffer, class btSerializer *serializer) const; - const char* getBaseName() const + const char *getBaseName() const { return m_baseName; } ///memory of setBaseName needs to be manager by user - void setBaseName(const char* name) + void setBaseName(const char *name) { m_baseName = name; } ///users can point to their objects, userPointer is not used by Bullet - void* getUserPointer() const + void *getUserPointer() const { return m_userObjectPointer; } - int getUserIndex() const + int getUserIndex() const { return m_userIndex; } - int getUserIndex2() const + int getUserIndex2() const { return m_userIndex2; } ///users can point to their objects, userPointer is not used by Bullet - void setUserPointer(void* userPointer) + void setUserPointer(void *userPointer) { m_userObjectPointer = userPointer; } ///users can point to their objects, userPointer is not used by Bullet - void setUserIndex(int index) + void setUserIndex(int index) { m_userIndex = index; } - void setUserIndex2(int index) + void setUserIndex2(int index) { m_userIndex2 = index; } + static 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 + + + private: - btMultiBody(const btMultiBody &); // not implemented - void operator=(const btMultiBody &); // not implemented + 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 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) + 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; + 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; - - + 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! - 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 - 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) - 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_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. + btVector3 m_baseConstraintForce; // external force applied to base. World frame. + btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. - - // - // 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; + 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; @@ -688,127 +703,141 @@ private: btMatrix3x3 m_cachedInertiaLowerRight; bool m_cachedInertiaValid; - bool m_fixedBase; + bool m_fixedBase; - // Sleep parameters. - bool m_awake; - bool m_canSleep; - btScalar m_sleepTimer; + // Sleep parameters. + bool m_awake; + bool m_canSleep; + bool m_canWakeup; + btScalar m_sleepTimer; - void* m_userObjectPointer; + 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; + 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_parentComToThisComOffset; - btVector3DoubleData m_thisPivotToThisComOffset; - btVector3DoubleData m_jointAxisTop[6]; - btVector3DoubleData m_jointAxisBottom[6]; + 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) - double m_linkMass; - int m_parentIndex; - int m_jointType; + btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal) + btVector3DoubleData m_absFrameTotVelocityTop; + btVector3DoubleData m_absFrameTotVelocityBottom; + btVector3DoubleData m_absFrameLocVelocityTop; + btVector3DoubleData m_absFrameLocVelocityBottom; - int m_dofCount; - int m_posVarCount; - double m_jointPos[7]; - double m_jointVel[6]; - double m_jointTorque[6]; + double m_linkMass; + int m_parentIndex; + int m_jointType; - 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; + 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_parentComToThisComOffset; - btVector3FloatData m_thisPivotToThisComOffset; - btVector3FloatData m_jointAxisTop[6]; - btVector3FloatData m_jointAxisBottom[6]; - btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal) - int m_dofCount; - float m_linkMass; - int m_parentIndex; - int m_jointType; - + 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; - - 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; + 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 +struct btMultiBodyDoubleData { - btTransformDoubleData m_baseWorldTransform; - btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal) - double m_baseMass; + 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; - char *m_paddingPtr; - 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 +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; + 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/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index d52852dd8..e17ab94d9 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -1,32 +1,29 @@ #include "btMultiBodyConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) +#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) +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) + if (m_bodyA) { m_jacSizeA = (6 + m_bodyA->getNumDofs()); } - if(m_bodyB) + if (m_bodyB) { m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); } @@ -38,7 +35,7 @@ void btMultiBodyConstraint::allocateJacobiansMultiDof() { updateJacobianSizes(); - m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); + m_posOffset = ((1 + m_jacSizeBoth) * m_numRows); m_data.resize((2 + m_jacSizeBoth) * m_numRows); } @@ -46,296 +43,307 @@ btMultiBodyConstraint::~btMultiBodyConstraint() { } -void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +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; + 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) +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 = rb0->getInvMass() + constraintNormalAng.dot(vec); - } - 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 = rb1->getInvMass() + constraintNormalAng.dot(vec); - } - 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->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) - /* + 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; i < ndofA; i++) + data.m_jacobians[solverConstraint.m_jacAindex + i] = jacOrgA[i]; + } + else + { + btScalar* jac1 = &data.m_jacobians[solverConstraint.m_jacAindex]; + //multiBodyA->fillContactJacobianMultiDof(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; i < ndofB; i++) + data.m_jacobians[solverConstraint.m_jacBindex + i] = jacOrgB[i]; + } + else + { + //multiBodyB->fillContactJacobianMultiDof(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; @@ -367,38 +375,35 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } } 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; - + + 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; - + { + 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; - - } + // 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 @@ -407,11 +412,10 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } */ - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = lowerLimit; - solverConstraint.m_upperLimit = upperLimit; - } - - return rel_vel; - + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = lowerLimit; + solverConstraint.m_upperLimit = upperLimit; + } + + return rel_vel; } diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 83521b950..5c15f3e85 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -27,66 +27,62 @@ struct btSolverInfo; struct btMultiBodyJacobianData { - btAlignedObjectArray m_jacobians; - btAlignedObjectArray m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension - btAlignedObjectArray m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI - btAlignedObjectArray scratch_r; - btAlignedObjectArray scratch_v; - btAlignedObjectArray scratch_m; - btAlignedObjectArray* m_solverBodyPool; - int m_fixedBodyId; - + btAlignedObjectArray m_jacobians; + btAlignedObjectArray m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension + btAlignedObjectArray m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; + btAlignedObjectArray* m_solverBodyPool; + int m_fixedBodyId; }; - -ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraint +ATTRIBUTE_ALIGNED16(class) +btMultiBodyConstraint { protected: + btMultiBody* m_bodyA; + btMultiBody* m_bodyB; + int m_linkA; + int m_linkB; - btMultiBody* m_bodyA; - btMultiBody* m_bodyB; - int m_linkA; - int m_linkB; + int m_numRows; + int m_jacSizeA; + int m_jacSizeBoth; + int m_posOffset; - int m_numRows; - int m_jacSizeA; - int m_jacSizeBoth; - int m_posOffset; + bool m_isUnilateral; + int m_numDofsFinalized; + btScalar m_maxAppliedImpulse; - bool m_isUnilateral; - int m_numDofsFinalized; - btScalar m_maxAppliedImpulse; + // warning: the data block lay out is not consistent for all constraints + // data block laid out as follows: + // cached impulses. (one per row.) + // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) + // positions. (one per row.) + btAlignedObjectArray m_data; + void applyDeltaVee(btMultiBodyJacobianData & data, btScalar * delta_vee, btScalar impulse, int velocityIndex, int ndof); - // warning: the data block lay out is not consistent for all constraints - // data block laid out as follows: - // cached impulses. (one per row.) - // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) - // positions. (one per row.) - btAlignedObjectArray m_data; + btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint & solverConstraint, + btMultiBodyJacobianData & data, + btScalar * jacOrgA, btScalar * jacOrgB, + const btVector3& constraintNormalAng, - 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& constraintNormalAng, - - const btVector3& constraintNormalLin, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - bool angConstraint = false, - - btScalar relaxation = 1.f, - bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); + const btVector3& constraintNormalLin, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + 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); + btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral); virtual ~btMultiBodyConstraint(); void updateJacobianSizes(); @@ -94,102 +90,105 @@ public: //many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later. virtual void setFrameInB(const btMatrix3x3& frameInB) {} - virtual void setPivotInB(const btVector3& pivotInB){} + virtual void setPivotInB(const btVector3& pivotInB) {} - virtual void finalizeMultiDof()=0; + virtual void finalizeMultiDof() = 0; - virtual int getIslandIdA() const =0; - virtual int getIslandIdB() const =0; + virtual int getIslandIdA() const = 0; + virtual int getIslandIdB() const = 0; - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal)=0; + virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows, + btMultiBodyJacobianData & data, + const btContactSolverInfo& infoGlobal) = 0; - int getNumRows() const + int getNumRows() const { return m_numRows; } - btMultiBody* getMultiBodyA() + btMultiBody* getMultiBodyA() { return m_bodyA; } - btMultiBody* getMultiBodyB() + btMultiBody* getMultiBodyB() { return m_bodyB; } - void internalSetAppliedImpulse(int dof, btScalar appliedImpulse) + int getLinkA() const { - btAssert(dof>=0); + 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) + + btScalar getAppliedImpulse(int dof) { - btAssert(dof>=0); + btAssert(dof >= 0); btAssert(dof < getNumRows()); return m_data[dof]; } // current constraint position - // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral - // NOTE: ignored position for friction rows. - btScalar getPosition(int row) const + // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral + // NOTE: ignored position for friction rows. + btScalar getPosition(int row) const { return m_data[m_posOffset + row]; } - void setPosition(int row, btScalar pos) + void setPosition(int row, btScalar pos) { m_data[m_posOffset + row] = pos; } - bool isUnilateral() const { return m_isUnilateral; } // jacobian blocks. - // each of size 6 + num_links. (jacobian2 is null if no body2.) - // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. - btScalar* jacobianA(int row) + // each of size 6 + num_links. (jacobian2 is null if no body2.) + // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. + btScalar* jacobianA(int row) { return &m_data[m_numRows + row * m_jacSizeBoth]; } - const btScalar* jacobianA(int row) const + const btScalar* jacobianA(int row) const { return &m_data[m_numRows + (row * m_jacSizeBoth)]; } - btScalar* jacobianB(int row) + btScalar* jacobianB(int row) { return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; } - const btScalar* jacobianB(int row) const + const btScalar* jacobianB(int row) const { return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; } - btScalar getMaxAppliedImpulse() const + btScalar getMaxAppliedImpulse() const { return m_maxAppliedImpulse; } - void setMaxAppliedImpulse(btScalar maxImp) + void setMaxAppliedImpulse(btScalar maxImp) { m_maxAppliedImpulse = maxImp; } - virtual void debugDraw(class btIDebugDraw* drawer)=0; + 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){} - - + virtual void setRelativePositionTarget(btScalar relPosTarget) {} + virtual void setErp(btScalar erp) {} }; -#endif //BT_MULTIBODY_CONSTRAINT_H - +#endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 1e2d07409..23e163f0e 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btMultiBodyConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "btMultiBodyLinkCollider.h" @@ -24,33 +23,33 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" -btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - btScalar leastSquaredResidual = 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) + if (constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } //solve featherstone normal contact - for (int j0=0;j0setPosUpdated(false); - if(constraint.m_multiBodyB) + if (constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } - + //solve featherstone frictional contact - - for (int j1=0;j1m_multiBodyFrictionContactConstraints.size();j1++) + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) { - if (iteration < infoGlobal.m_numIterations) + for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++) { - 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)) + if (iteration < infoGlobal.m_numIterations) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; - btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual += residual*residual; + int index = j1; - if(frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->setPosUpdated(false); - if(frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->setPosUpdated(false); + btMultiBodySolverConstraint& frictionConstraint = m_multiBodySpinningFrictionContactConstraints[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 = btMax(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_multiBodyTorsionalFrictionContactConstraints.size(); j1++) + { + if (iteration < infoGlobal.m_numIterations) + { + int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + j1++; + int index2 = j1; + btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints[index2]; + //adjust friction limits here + if (totalImpulse > btScalar(0) && 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 = btMax(leastSquaredResidual, residual * residual); + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + + if (frictionConstraintB.m_multiBodyA) + frictionConstraintB.m_multiBodyA->setPosUpdated(false); + if (frictionConstraintB.m_multiBodyB) + frictionConstraintB.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 = btMax(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 + { + 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; + //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 = btMax(leastSquaredResidual, residual * residual); + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } } } } return leastSquaredResidual; } -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { m_multiBodyNonContactConstraints.resize(0); m_multiBodyNormalContactConstraints.resize(0); m_multiBodyFrictionContactConstraints.resize(0); + m_multiBodyTorsionalFrictionContactConstraints.resize(0); + m_multiBodySpinningFrictionContactConstraints.resize(0); + m_data.m_jacobians.resize(0); m_data.m_deltaVelocitiesUnitImpulse.resize(0); m_data.m_deltaVelocities.resize(0); - for (int i=0;igetNumDofs() + 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 @@ -180,42 +280,223 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt 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); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(c.m_solverBodyIdA >= 0) + 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) { - 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); + c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS } - return deltaImpulse; + else if (c.m_solverBodyIdB >= 0) + { + bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + } + btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv; + return deltaVel; } - - - -void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +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 + cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (cB.m_solverBodyIdB >= 0) + { + bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB); + } + + btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv; + return deltaVel; +} + +void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - BT_PROFILE("setupMultiBodyContactConstraint"); btVector3 rel_pos1; btVector3 rel_pos2; @@ -233,44 +514,46 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + 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; - - //cfm = 1 / ( dt * kp + kd ) - //erp = dt * kp / ( dt * kp + kd ) - - btScalar cfm; + + 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 + } + 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) || (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_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 ); + btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1); if (denom < SIMD_EPSILON) { denom = SIMD_EPSILON; } - cfm = btScalar(1) / denom; + cfm = btScalar(1) / denom; erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; } } @@ -280,218 +563,217 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyA) { - if (solverConstraint.m_linkA<0) + if (solverConstraint.m_linkA < 0) { rel_pos1 = pos1 - multiBodyA->getBasePos(); - } else + } + else { rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); } - const int ndofA = multiBodyA->getNumDofs() + 6; + const int ndofA = multiBodyA->getNumDofs() + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - if (solverConstraint.m_deltaVelAindex <0) + 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 + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA); + } + else { - btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + 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); + 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]; + btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex]; multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, 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); + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v); btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = contactNormal; - } else + } + else { btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = contactNormal; - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); } - - if (multiBodyB) { - if (solverConstraint.m_linkB<0) + if (solverConstraint.m_linkB < 0) { rel_pos2 = pos2 - multiBodyB->getBasePos(); - } else + } + else { rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); } - const int ndofB = multiBodyB->getNumDofs() + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) + 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); + 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); + 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->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &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 = rel_pos2.cross(contactNormal); + 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 = rel_pos2.cross(contactNormal); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormal; - - } else + } + else { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormal; - - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); } { - btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; btScalar* jacB = 0; btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; + btScalar* lambdaA = 0; + btScalar* lambdaB = 0; + int ndofA = 0; if (multiBodyA) { - ndofA = multiBodyA->getNumDofs() + 6; + 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; + btScalar j = jacA[i]; + btScalar l = lambdaA[i]; + denom0 += j * l; } - } else + } + else { if (rb0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); denom0 = rb0->getInvMass() + contactNormal.dot(vec); } } if (multiBodyB) { - const int ndofB = multiBodyB->getNumDofs() + 6; + 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; + btScalar j = jacB[i]; + btScalar l = lambdaB[i]; + denom1 += j * l; } - - } else + } + else { if (rb1) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = rb1->getInvMass() + contactNormal.dot(vec); } } - - - btScalar d = denom0+denom1+cfm; - if (d>SIMD_EPSILON) - { - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { + btScalar d = denom0 + denom1 + cfm; + if (d > SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation / (d); + } + else + { //disable the constraint row to handle singularity/redundant constraint - solverConstraint.m_jacDiagABInv = 0.f; - } - + solverConstraint.m_jacDiagABInv = 0.f; + } } - //compute rhs and remaining solverConstraint fields - - btScalar restitution = 0.f; - 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; + 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); + } + } - btVector3 vel1,vel2; + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + btVector3 vel1, vel2; if (multiBodyA) { - ndofA = multiBodyA->getNumDofs() + 6; + ndofA = multiBodyA->getNumDofs() + 6; btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) + for (int i = 0; i < ndofA; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } else + } + else { if (rb0) { - 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); + 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) { - ndofB = multiBodyB->getNumDofs() + 6; + ndofB = multiBodyB->getNumDofs() + 6; btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) + for (int i = 0; i < ndofB; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } else + } + else { if (rb1) { - 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); + 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); } } solverConstraint.m_friction = cp.m_combinedFriction; - if(!isFriction) + if (!isFriction) { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); if (restitution <= btScalar(0.)) { restitution = 0.f; @@ -499,10 +781,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } } - ///warm starting (or zero if disabled) //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) - if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + if (0) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; @@ -512,27 +793,30 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); - - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else + multiBodyA->applyDeltaVeeMultiDof(deltaV, impulse); + + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); + } + else { if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); } if (multiBodyB) { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else + multiBodyB->applyDeltaVeeMultiDof(deltaV, impulse); + applyDeltaVee(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); + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); } } - } else + } + else { solverConstraint.m_appliedImpulse = 0.f; } @@ -540,38 +824,37 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_appliedPushImpulse = 0.f; { - 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 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 if (isFriction) { - positionalError = -distance * erp/infoGlobal.m_timeStep; - } else + positionalError = -distance * erp / infoGlobal.m_timeStep; + } + else { - if (distance>0) + if (distance > 0) { positionalError = 0; velocityError -= distance / infoGlobal.m_timeStep; - - } else + } + else { - positionalError = -distance * erp/infoGlobal.m_timeStep; + positionalError = -distance * erp / infoGlobal.m_timeStep; } } - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; - if(!isFriction) + 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_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; - } - /*else + /*else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; @@ -583,309 +866,288 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } else { - solverConstraint.m_rhs = penetrationImpulse+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 = cfm*solverConstraint.m_jacDiagABInv; - - - + 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) + 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); + 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); + } + } + 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) - { + } + } + + 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) - { + 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)); + 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; - 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; - - - - } - + 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) +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) { BT_PROFILE("addMultiBodyFrictionConstraint"); btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; - + 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); + 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; @@ -899,92 +1161,129 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo solverConstraint.m_originalContactPoint = &cp; - setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); 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) +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"); - btMultiBodySolverConstraint& solverConstraint = 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; + 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) +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint(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"); + + btMultiBodySolverConstraint& solverConstraint = m_multiBodySpinningFrictionContactConstraints.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()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - btCollisionObject* colObj0=0,*colObj1=0; + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; + + btCollisionObject *colObj0 = 0, *colObj1 = 0; colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); - -// btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; -// btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; + int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; + // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; ///avoid collision response between two static objects -// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) + // 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++) - { + //only a single rollingFriction per manifold + int rollingFriction = 1; + for (int j = 0; j < manifold->getNumContacts(); j++) + { btManifoldPoint& cp = manifold->getContactPoint(j); if (cp.getDistance() <= manifold->getContactProcessingThreshold()) { - btScalar relaxation; int frictionIndex = m_multiBodyNormalContactConstraints.size(); btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); - // btRigidBody* rb0 = btRigidBody::upcast(colObj0); - // btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; + // btRigidBody* rb0 = btRigidBody::upcast(colObj0); + // btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_multiBodyA = mbA; @@ -998,60 +1297,56 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* solverConstraint.m_originalContactPoint = &cp; bool isFriction = false; - setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); + setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp, infoGlobal, relaxation, isFriction); -// const btVector3& pos1 = cp.getPositionWorldOnA(); -// const btVector3& pos2 = cp.getPositionWorldOnB(); + // const btVector3& pos1 = cp.getPositionWorldOnA(); + // const btVector3& pos2 = cp.getPositionWorldOnB(); /////setup the friction constraints #define ENABLE_FRICTION #ifdef ENABLE_FRICTION - solverConstraint.m_frictionIndex = frictionIndex; + solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size(); ///Bullet has several options to set the friction directions ///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 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. /// ///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, + ///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 /// - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + 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) - { + if (rollingFriction > 0) + { + if (cp.m_combinedSpinningFriction > 0) + { + addMultiBodySpinningFrictionConstraint(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); - 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)) - {/* + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); + 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(); if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) @@ -1074,84 +1369,77 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* } else */ { - - - 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); - + 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)) { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, 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; + cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; } } - - } else + } + else { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM); + 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_frictionCFM); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); //todo: solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; } - - -#endif //ENABLE_FRICTION +#endif //ENABLE_FRICTION } } } -void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { //btPersistentManifold* manifold = 0; - for (int i=0;igetBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); if (!fcA && !fcB) { //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case - convertContact(manifold,infoGlobal); - } else + convertContact(manifold, infoGlobal); + } + else { - convertMultiBodyContact(manifold,infoGlobal); + convertMultiBodyContact(manifold, infoGlobal); } } //also convert the multibody constraints, if any - - for (int i=0;icreateConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); - } + c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal); + } } - - -btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { - return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints); + return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); } #if 0 @@ -1175,56 +1463,54 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS } #endif - void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime) { -#if 1 - +#if 1 + //bod->addBaseForce(m_gravity * bod->getBaseMass()); //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); if (c.m_orgConstraint) { - c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse); + c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex, c.m_appliedImpulse); } - if (c.m_multiBodyA) { - c.m_multiBodyA->setCompanionId(-1); - btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime); - btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime); - if (c.m_linkA<0) + btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime); + btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime); + if (c.m_linkA < 0) { c.m_multiBodyA->addBaseConstraintForce(force); c.m_multiBodyA->addBaseConstraintTorque(torque); - } else + } + else { - c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force); - //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque); + c.m_multiBodyA->addLinkConstraintForce(c.m_linkA, force); + //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); + c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA, torque); } } - + if (c.m_multiBodyB) { { c.m_multiBodyB->setCompanionId(-1); - btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime); - btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime); - if (c.m_linkB<0) + btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime); + btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime); + if (c.m_linkB < 0) { c.m_multiBodyB->addBaseConstraintForce(force); c.m_multiBodyB->addBaseConstraintTorque(torque); - } else + } + else { { - c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force); + c.m_multiBodyB->addLinkConstraintForce(c.m_linkB, force); //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque); + c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB, torque); } - } } } @@ -1234,81 +1520,64 @@ 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 - - - } -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish"); int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); - - //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) + //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) //or as applied force, so we can measure the joint reaction forces easier - for (int i=0;im_appliedImpulse = solverConstraint.m_appliedImpulse; pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse; - + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse; + pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse; + } else + { + pt->m_appliedImpulseLateral2 = 0; } - //do a callback here? } + + //do a callback here? } #if 0 //multibody joint feedback @@ -1407,23 +1676,22 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO } } -#endif +#endif #endif - return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal); + return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); } - -void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { + //printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints); + //printf("solveMultiBodyGroup start\n"); m_tmpMultiBodyConstraints = multiBodyConstraints; m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + + btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); m_tmpMultiBodyConstraints = 0; m_tmpNumMultiBodyConstraints = 0; - - } diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 489347d87..abf571883 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -25,76 +25,76 @@ class btMultiBody; #include "btMultiBodyConstraint.h" - - -ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver +ATTRIBUTE_ALIGNED16(class) +btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver { - protected: + btMultiBodyConstraintArray m_multiBodyNonContactConstraints; - btMultiBodyConstraintArray m_multiBodyNonContactConstraints; + btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; + btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints; - btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + btMultiBodyJacobianData m_data; - btMultiBodyJacobianData m_data; - //temp storage for multi body constraints for a specific island/group called by 'solveGroup' - btMultiBodyConstraint** m_tmpMultiBodyConstraints; - int m_tmpNumMultiBodyConstraints; + btMultiBodyConstraint** m_tmpMultiBodyConstraints; + int m_tmpNumMultiBodyConstraints; btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); - - 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); + //solve 2 friction directions and clamp against the implicit friction cone + btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB); - 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 convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); - void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, - btScalar* jacA,btScalar* jacB, - btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - 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); - void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - 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); + 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 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); -// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + btMultiBodySolverConstraint& addMultiBodySpinningFrictionConstraint(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, + btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal); + + void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint, + const btVector3& contactNormal, + 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); + // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof); + void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime); - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof); - void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); - - virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); + virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); + + virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); }; - - - - -#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H - +#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 9eacc2264..1131e5378 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -23,45 +23,43 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btSerializer.h" - -void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask) +void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask) { m_multiBodies.push_back(body); - } -void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) +void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) { m_multiBodies.remove(body); } -void btMultiBodyDynamicsWorld::calculateSimulationIslands() +void btMultiBodyDynamicsWorld::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher()); + + { + //merge islands based on speculative contact manifolds too + for (int i = 0; i < this->m_predictiveManifolds.size(); i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; + + const btCollisionObject* colObj0 = manifold->getBody0(); + const btCollisionObject* colObj1 = manifold->getBody1(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); + } + } + } - { - //merge islands based on speculative contact manifolds too - for (int i=0;im_predictiveManifolds.size();i++) - { - btPersistentManifold* manifold = m_predictiveManifolds[i]; - - const btCollisionObject* colObj0 = manifold->getBody0(); - const btCollisionObject* colObj1 = manifold->getBody1(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } - { int i; int numConstraints = int(m_constraints.size()); - for (i=0;i< numConstraints ; i++ ) + for (i = 0; i < numConstraints; i++) { btTypedConstraint* constraint = m_constraints[i]; if (constraint->isEnabled()) @@ -72,23 +70,23 @@ void btMultiBodyDynamicsWorld::calculateSimulationIslands() if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); } } } } //merge islands linked by Featherstone link colliders - for (int i=0;igetBaseCollider(); - for (int b=0;bgetNumLinks();b++) + for (int b = 0; b < body->getNumLinks(); b++) { btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; - + if (((cur) && (!(cur)->isStaticOrKinematicObject())) && ((prev) && (!(prev)->isStaticOrKinematicObject()))) { @@ -98,36 +96,31 @@ void btMultiBodyDynamicsWorld::calculateSimulationIslands() } if (cur && !cur->isStaticOrKinematicObject()) prev = cur; - } } } //merge islands linked by multibody constraints { - for (int i=0;im_multiBodyConstraints.size();i++) + for (int i = 0; i < this->m_multiBodyConstraints.size(); i++) { btMultiBodyConstraint* c = m_multiBodyConstraints[i]; int tagA = c->getIslandIdA(); int tagB = c->getIslandIdB(); - if (tagA>=0 && tagB>=0) + if (tagA >= 0 && tagB >= 0) getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); } } //Store the island id in each body getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - } - -void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) +void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) { BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); - - - for ( int i=0;igetBaseCollider(); if (col && col->getActivationState() == ACTIVE_TAG) { - col->setActivationState( WANTS_DEACTIVATION); + col->setActivationState(WANTS_DEACTIVATION); col->setDeactivationTime(0.f); } - for (int b=0;bgetNumLinks();b++) + for (int b = 0; b < body->getNumLinks(); b++) { btMultiBodyLinkCollider* col = body->getLink(b).m_collider; if (col && col->getActivationState() == ACTIVE_TAG) { - col->setActivationState( WANTS_DEACTIVATION); + col->setActivationState(WANTS_DEACTIVATION); col->setDeactivationTime(0.f); } } - } else + } + else { btMultiBodyLinkCollider* col = body->getBaseCollider(); if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState( ACTIVE_TAG ); + col->setActivationState(ACTIVE_TAG); - for (int b=0;bgetNumLinks();b++) + for (int b = 0; b < body->getNumLinks(); b++) { btMultiBodyLinkCollider* col = body->getLink(b).m_collider; if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState( ACTIVE_TAG ); + col->setActivationState(ACTIVE_TAG); } } - } } btDiscreteDynamicsWorld::updateActivationState(timeStep); } - -SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) +SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) { int islandId; - + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); return islandId; - } - class btSortConstraintOnIslandPredicate2 { - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId2(rhs); - lIslandId0 = btGetConstraintIslandId2(lhs); - return lIslandId0 < rIslandId0; - } +public: + bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetConstraintIslandId2(rhs); + lIslandId0 = btGetConstraintIslandId2(lhs); + return lIslandId0 < rIslandId0; + } }; - - -SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) +SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) { int islandId; - + int islandTagA = lhs->getIslandIdA(); int islandTagB = lhs->getIslandIdB(); - islandId= islandTagA>=0?islandTagA:islandTagB; + islandId = islandTagA >= 0 ? islandTagA : islandTagB; return islandId; - } - class btSortMultiBodyConstraintOnIslandPredicate { - public: - - bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); - lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } +public: + bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); + lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } }; + struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback { - btContactSolverInfo* m_solverInfo; - btMultiBodyConstraintSolver* m_solver; - btMultiBodyConstraint** m_multiBodySortedConstraints; - int m_numMultiBodyConstraints; - - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - + btContactSolverInfo* m_solverInfo; + btMultiBodyConstraintSolver* m_solver; + btMultiBodyConstraint** m_multiBodySortedConstraints; + int m_numMultiBodyConstraints; + + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + btAlignedObjectArray m_bodies; btAlignedObjectArray m_manifolds; btAlignedObjectArray m_constraints; btAlignedObjectArray m_multiBodyConstraints; + btAlignedObjectArray m_islandAnalyticsData; - MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_multiBodySortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) + MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver, + btDispatcher* dispatcher) + : m_solverInfo(NULL), + m_solver(solver), + m_multiBodySortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) { - } - MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) + MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other) { btAssert(0); (void)other; return *this; } - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) + SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) { + m_islandAnalyticsData.clear(); btAssert(solverInfo); m_solverInfo = solverInfo; @@ -271,22 +257,32 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: m_numConstraints = numConstraints; m_debugDrawer = debugDrawer; - m_bodies.resize (0); - m_manifolds.resize (0); - m_constraints.resize (0); + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); m_multiBodyConstraints.resize(0); } - - virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) { - if (islandId<0) + m_solver = solver; + } + + virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) + { + if (islandId < 0) { ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else + m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); + if (m_solverInfo->m_reportSolverAnalytics&1) + { + m_solver->m_analyticsData.m_islandId = islandId; + m_islandAnalyticsData.push_back(m_solver->m_analyticsData); + } + } + else { - //also add all non-contact constraints/joints for this island + //also add all non-contact constraints/joints for this island btTypedConstraint** startConstraint = 0; btMultiBodyConstraint** startMultiBodyConstraint = 0; @@ -294,10 +290,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: int numCurMultiBodyConstraints = 0; int i; - + //find the first constraint for this island - for (i=0;isolveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); //} else { - - for (i=0;im_solverInfo->m_minimumSolverBatchSize) + + if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) { - processConstraints(); - } else + processConstraints(islandId); + } + else { //printf("deferred\n"); } } } } - void processConstraints() + void processConstraints(int islandId=-1) { - - btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; - btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; - btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; - btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; + btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; + btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0; + btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; + btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); - - m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); + + m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); + if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) + { + m_solver->m_analyticsData.m_islandId = islandId; + m_islandAnalyticsData.push_back(m_solver->m_analyticsData); + } m_bodies.resize(0); m_manifolds.resize(0); m_constraints.resize(0); m_multiBodyConstraints.resize(0); } - }; - - -btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) - :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_multiBodyConstraintSolver(constraintSolver) +void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray& islandAnalyticsData) const { - //split impulse is not yet supported for Featherstone hierarchies -// getSolverInfo().m_splitImpulse = false; - getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; - m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); + islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData; } -btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () +btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) + : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), + m_multiBodyConstraintSolver(constraintSolver) +{ + //split impulse is not yet supported for Featherstone hierarchies + // getSolverInfo().m_splitImpulse = false; + getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS; + m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); +} + +btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld() { delete m_solverMultiBodyIslandCallback; } -void btMultiBodyDynamicsWorld::forwardKinematics() +void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) { + m_multiBodyConstraintSolver = solver; + m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver); + btDiscreteDynamicsWorld::setConstraintSolver(solver); +} - for (int b=0;bgetSolverType() == BT_MULTIBODY_SOLVER) + { + m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver; + } + btDiscreteDynamicsWorld::setConstraintSolver(solver); +} + +void btMultiBodyDynamicsWorld::forwardKinematics() +{ + for (int b = 0; b < m_multiBodies.size(); b++) { btMultiBody* bod = m_multiBodies[b]; - bod->forwardKinematics(m_scratch_world_to_local,m_scratch_local_origin); + bod->forwardKinematics(m_scratch_world_to_local, m_scratch_local_origin); } } -void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { forwardKinematics(); - - BT_PROFILE("solveConstraints"); - - m_sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;isetup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - - /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY { BT_PROFILE("btMultiBody addForce"); - for (int i=0;im_multiBodies.size();i++) + for (int i = 0; i < this->m_multiBodies.size(); i++) { btMultiBody* bod = m_multiBodies[i]; bool isSleeping = false; - + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) { isSleeping = true; - } - for (int b=0;bgetNumLinks();b++) + } + for (int b = 0; b < bod->getNumLinks(); b++) { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) isSleeping = true; - } + } if (!isSleeping) { //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - 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); + 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()); - for (int j = 0; j < bod->getNumLinks(); ++j) + for (int j = 0; j < bod->getNumLinks(); ++j) { bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); } - }//if (!isSleeping) + } //if (!isSleeping) } } -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY { BT_PROFILE("btMultiBody stepVelocities"); - for (int i=0;im_multiBodies.size();i++) + for (int i = 0; i < this->m_multiBodies.size(); i++) { btMultiBody* bod = m_multiBodies[i]; bool isSleeping = false; - + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) { isSleeping = true; - } - for (int b=0;bgetNumLinks();b++) + } + for (int b = 0; b < bod->getNumLinks(); b++) { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) isSleeping = true; - } + } if (!isSleeping) { //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - 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); + 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; - + bool isConstraintPass = false; { - if(!bod->isUsingRK4Integration()) + if (!bod->isUsingRK4Integration()) { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } else - { + { // int numDofs = bod->getNumDofs() + 6; int numPosVars = bod->getNumPosVars() + 7; - btAlignedObjectArray scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs); + btAlignedObjectArray scratch_r2; + scratch_r2.resize(2 * numPosVars + 8 * numDofs); //convenience - btScalar *pMem = &scratch_r2[0]; - btScalar *scratch_q0 = pMem; pMem += numPosVars; - btScalar *scratch_qx = pMem; pMem += numPosVars; - btScalar *scratch_qd0 = pMem; pMem += numDofs; - btScalar *scratch_qd1 = pMem; pMem += numDofs; - btScalar *scratch_qd2 = pMem; pMem += numDofs; - btScalar *scratch_qd3 = pMem; pMem += numDofs; - btScalar *scratch_qdd0 = pMem; pMem += numDofs; - btScalar *scratch_qdd1 = pMem; pMem += numDofs; - btScalar *scratch_qdd2 = pMem; pMem += numDofs; - btScalar *scratch_qdd3 = pMem; pMem += numDofs; - btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]); + btScalar* pMem = &scratch_r2[0]; + btScalar* scratch_q0 = pMem; + pMem += numPosVars; + btScalar* scratch_qx = pMem; + pMem += numPosVars; + btScalar* scratch_qd0 = pMem; + pMem += numDofs; + btScalar* scratch_qd1 = pMem; + pMem += numDofs; + btScalar* scratch_qd2 = pMem; + pMem += numDofs; + btScalar* scratch_qd3 = pMem; + pMem += numDofs; + btScalar* scratch_qdd0 = pMem; + pMem += numDofs; + btScalar* scratch_qdd1 = pMem; + pMem += numDofs; + btScalar* scratch_qdd2 = pMem; + pMem += numDofs; + btScalar* scratch_qdd3 = pMem; + pMem += numDofs; + btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]); - ///// + ///// //copy q0 to scratch_q0 and qd0 to scratch_qd0 scratch_q0[0] = bod->getWorldToBaseRot().x(); scratch_q0[1] = bod->getWorldToBaseRot().y(); @@ -535,83 +560,88 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) scratch_q0[5] = bod->getBasePos().y(); scratch_q0[6] = bod->getBasePos().z(); // - for(int link = 0; link < bod->getNumLinks(); ++link) + for (int link = 0; link < bod->getNumLinks(); ++link) { - for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) - scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; + for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) + scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; } // - for(int dof = 0; dof < numDofs; ++dof) + for (int dof = 0; dof < numDofs; ++dof) scratch_qd0[dof] = bod->getVelocityVector()[dof]; //// struct { - btMultiBody *bod; - btScalar *scratch_qx, *scratch_q0; + btMultiBody* bod; + btScalar *scratch_qx, *scratch_q0; - void operator()() - { - for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) - scratch_qx[dof] = scratch_q0[dof]; - } + void operator()() + { + for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) + scratch_qx[dof] = scratch_q0[dof]; + } } pResetQx = {bod, scratch_qx, scratch_q0}; // struct { - void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size) - { - for(int i = 0; i < size; ++i) - pVal[i] = pCurVal[i] + dt * pDer[i]; - } + void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size) + { + for (int i = 0; i < size; ++i) + pVal[i] = pCurVal[i] + dt * pDer[i]; + } } pEulerIntegrate; // struct - { - void operator()(btMultiBody *pBody, const btScalar *pData) - { - btScalar *pVel = const_cast(pBody->getVelocityVector()); - - for(int i = 0; i < pBody->getNumDofs() + 6; ++i) - pVel[i] = pData[i]; - - } - } pCopyToVelocityVector; - // - struct { - void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size) - { - for(int i = 0; i < size; ++i) - pDst[i] = pSrc[start + i]; - } + void operator()(btMultiBody* pBody, const btScalar* pData) + { + btScalar* pVel = const_cast(pBody->getVelocityVector()); + + for (int i = 0; i < pBody->getNumDofs() + 6; ++i) + pVel[i] = pData[i]; + } + } pCopyToVelocityVector; + // + struct + { + void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size) + { + for (int i = 0; i < size; ++i) + pDst[i] = pSrc[start + i]; + } } pCopy; // btScalar h = solverInfo.m_timeStep; - #define output &m_scratch_r[bod->getNumDofs()] - //calc qdd0 from: q0 & qd0 - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); +#define output &m_scratch_r[bod->getNumDofs()] + //calc qdd0 from: q0 & qd0 + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd0, 0, numDofs); //calc q1 = q0 + h/2 * qd0 pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0); + bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0); //calc qd1 = qd0 + h/2 * qdd0 - pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); + pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); // //calc qdd1 from: q1 & qd1 pCopyToVelocityVector(bod, scratch_qd1); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd1, 0, numDofs); //calc q2 = q0 + h/2 * qd1 pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1); + bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1); //calc qd2 = qd0 + h/2 * qdd1 - pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); + pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); // //calc qdd2 from: q2 & qd2 pCopyToVelocityVector(bod, scratch_qd2); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd2, 0, numDofs); //calc q3 = q0 + h * qd2 pResetQx(); @@ -621,154 +651,162 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd3 from: q3 & qd3 pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd3, 0, numDofs); // //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) - //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) - btAlignedObjectArray delta_q; delta_q.resize(numDofs); - btAlignedObjectArray delta_qd; delta_qd.resize(numDofs); - for(int i = 0; i < numDofs; ++i) + //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) + btAlignedObjectArray delta_q; + delta_q.resize(numDofs); + btAlignedObjectArray delta_qd; + delta_qd.resize(numDofs); + for (int i = 0; i < numDofs; ++i) { - delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]); - delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]); + delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]); + delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]); //delta_q[i] = h*scratch_qd0[i]; //delta_qd[i] = h*scratch_qdd0[i]; } // pCopyToVelocityVector(bod, scratch_qd0); - bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); + bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); // - if(!doNotUpdatePos) + if (!doNotUpdatePos) { - btScalar *pRealBuf = const_cast(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + btScalar* pRealBuf = const_cast(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); - for(int i = 0; i < numDofs; ++i) + for (int i = 0; i < numDofs; ++i) pRealBuf[i] = delta_q[i]; //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->setPosUpdated(true); + bod->setPosUpdated(true); } //ugly hack which resets the cached data to t0 (needed for constraint solver) { - for(int link = 0; link < bod->getNumLinks(); ++link) + for (int link = 0; link < bod->getNumLinks(); ++link) bod->getLink(link).updateCacheMultiDof(); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } - } } - + #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY bod->clearForcesAndTorques(); -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - }//if (!isSleeping) +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + } //if (!isSleeping) } } - clearMultiBodyConstraintForces(); + /// solve all the constraints for this island + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); m_solverMultiBodyIslandCallback->processConstraints(); - + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); { - BT_PROFILE("btMultiBody stepVelocities"); - for (int i=0;im_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; + BT_PROFILE("btMultiBody stepVelocities"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; - bool isSleeping = false; + bool isSleeping = false; - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;bgetNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - 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 (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + 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, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass); - } + if (bod->internalNeedsJointFeedback()) + { + if (!bod->isUsingRK4Integration()) + { + if (bod->internalNeedsJointFeedback()) + { + bool isConstraintPass = true; + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } } } } } - for (int i=0;im_multiBodies.size();i++) + for (int i = 0; i < this->m_multiBodies.size(); i++) { btMultiBody* bod = m_multiBodies[i]; bod->processDeltaVeeMultiDof2(); } - } -void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) +void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { btDiscreteDynamicsWorld::integrateTransforms(timeStep); { BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies - - for (int b=0;bgetBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) { isSleeping = true; - } - for (int b=0;bgetNumLinks();b++) + } + for (int b = 0; b < bod->getNumLinks(); b++) { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) isSleeping = true; } - if (!isSleeping) { int nLinks = bod->getNumLinks(); ///base + num m_links - - + { - if(!bod->isPosUpdated()) + if (!bod->isPosUpdated()) bod->stepPositionsMultiDof(timeStep); else { - btScalar *pRealBuf = const_cast(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + btScalar* pRealBuf = const_cast(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); bod->stepPositionsMultiDof(1, 0, pRealBuf); bod->setPosUpdated(false); } } - - m_scratch_world_to_local.resize(nLinks+1); - m_scratch_local_origin.resize(nLinks+1); - bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local,m_scratch_local_origin); - - } else + m_scratch_world_to_local.resize(nLinks + 1); + m_scratch_local_origin.resize(nLinks + 1); + + bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + } + else { bod->clearVelocities(); } @@ -776,14 +814,12 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) } } - - -void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) +void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint) { m_multiBodyConstraints.push_back(constraint); } -void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) +void btMultiBodyDynamicsWorld::removeMultiBodyConstraint(btMultiBodyConstraint* constraint) { m_multiBodyConstraints.remove(constraint); } @@ -793,8 +829,7 @@ void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstrain constraint->debugDraw(getDebugDrawer()); } - -void btMultiBodyDynamicsWorld::debugDrawWorld() +void btMultiBodyDynamicsWorld::debugDrawWorld() { BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld"); @@ -804,7 +839,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() if (getDebugDrawer()) { int mode = getDebugDrawer()->getDebugMode(); - if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) { drawConstraints = true; } @@ -812,157 +847,148 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() if (drawConstraints) { BT_PROFILE("btMultiBody debugDrawWorld"); - - for (int c=0;cforwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1); - - getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1); + bod->forwardKinematics(m_scratch_world_to_local1, m_scratch_local_origin1); - - for (int m = 0; mgetNumLinks(); m++) + if (mode & btIDebugDraw::DBG_DrawFrames) + { + getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1); + } + + for (int m = 0; m < bod->getNumLinks(); m++) { - const btTransform& tr = bod->getLink(m).m_cachedWorldTransform; + 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) * 0.1; - getDebugDrawer()->drawTransform(tr, 0.1); + btVector4 color(0, 0, 0, 1); //1,1,1); + btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + getDebugDrawer()->drawLine(from, to, color); + } + if (bod->getLink(m).m_jointType == btMultibodyLink::eFixed) + { + btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 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); - - btVector4 color(0,0,0,1);//1,1,1); - btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - getDebugDrawer()->drawLine(from,to,color); + btVector4 color(0, 0, 0, 1); //1,1,1); + btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + getDebugDrawer()->drawLine(from, to, color); } - if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed) + if (bod->getLink(m).m_jointType == btMultibodyLink::ePrismatic) { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); - - btVector4 color(0,0,0,1);//1,1,1); - btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - getDebugDrawer()->drawLine(from,to,color); + 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); + btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + getDebugDrawer()->drawLine(from, to, color); } - if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic) - { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); - - btVector4 color(0,0,0,1);//1,1,1); - btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - getDebugDrawer()->drawLine(from,to,color); - } - } } } } - - } - - void btMultiBodyDynamicsWorld::applyGravity() { - btDiscreteDynamicsWorld::applyGravity(); + btDiscreteDynamicsWorld::applyGravity(); #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - BT_PROFILE("btMultiBody addGravity"); - for (int i=0;im_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; + BT_PROFILE("btMultiBody addGravity"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; - bool isSleeping = false; + bool isSleeping = false; - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;bgetNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } - if (!isSleeping) - { - bod->addBaseForce(m_gravity * bod->getBaseMass()); + if (!isSleeping) + { + bod->addBaseForce(m_gravity * bod->getBaseMass()); - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); - } - }//if (!isSleeping) - } -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + } //if (!isSleeping) + } +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY } void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces() -{ - for (int i=0;im_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; - bod->clearConstraintForces(); - } +{ + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->clearConstraintForces(); + } } void btMultiBodyDynamicsWorld::clearMultiBodyForces() { - { - // BT_PROFILE("clearMultiBodyForces"); - for (int i=0;im_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; + { + // BT_PROFILE("clearMultiBodyForces"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; - bool isSleeping = false; + bool isSleeping = false; - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;bgetNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } - if (!isSleeping) - { - btMultiBody* bod = m_multiBodies[i]; - bod->clearForcesAndTorques(); - } + if (!isSleeping) + { + btMultiBody* bod = m_multiBodies[i]; + bod->clearForcesAndTorques(); + } } } - } void btMultiBodyDynamicsWorld::clearForces() { - btDiscreteDynamicsWorld::clearForces(); + btDiscreteDynamicsWorld::clearForces(); #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY clearMultiBodyForces(); #endif } - - - -void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer) +void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer) { - serializer->startSerialization(); - serializeDynamicsWorldInfo( serializer); + serializeDynamicsWorldInfo(serializer); serializeMultiBodies(serializer); @@ -970,22 +996,36 @@ void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer) serializeCollisionObjects(serializer); + serializeContactManifolds(serializer); + serializer->finishSerialization(); } -void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) +void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) { int i; //serialize all collision objects - for (i=0;icalculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = mb->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); + serializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb); } } -} \ No newline at end of file + //serialize all multibody links (collision objects) + for (i = 0; i < m_collisionObjects.size(); i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + if (colObj->getInternalType() == 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); + } + } +} diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index c0c132bbb..e36c2f7aa 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -33,8 +33,8 @@ protected: btAlignedObjectArray m_multiBodies; btAlignedObjectArray m_multiBodyConstraints; btAlignedObjectArray m_sortedMultiBodyConstraints; - btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; - MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; + btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; + MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; //cached data to avoid memory allocations btAlignedObjectArray m_scratch_world_to_local; @@ -45,70 +45,71 @@ protected: btAlignedObjectArray m_scratch_v; btAlignedObjectArray m_scratch_m; - - virtual void calculateSimulationIslands(); - virtual void updateActivationState(btScalar timeStep); - virtual void solveConstraints(btContactSolverInfo& solverInfo); - - virtual void serializeMultiBodies(btSerializer* serializer); + virtual void calculateSimulationIslands(); + virtual void updateActivationState(btScalar timeStep); + virtual void solveConstraints(btContactSolverInfo& solverInfo); + + virtual void serializeMultiBodies(btSerializer* serializer); public: + btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration); - btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + virtual ~btMultiBodyDynamicsWorld(); - virtual ~btMultiBodyDynamicsWorld (); + virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter); - virtual void addMultiBody(btMultiBody* body, int group= btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter); + virtual void removeMultiBody(btMultiBody* body); - virtual void removeMultiBody(btMultiBody* body); - - virtual int getNumMultibodies() const + virtual int getNumMultibodies() const { return m_multiBodies.size(); } - btMultiBody* getMultiBody(int mbIndex) + btMultiBody* getMultiBody(int mbIndex) { return m_multiBodies[mbIndex]; } - const btMultiBody* getMultiBody(int mbIndex) const + const btMultiBody* getMultiBody(int mbIndex) const { return m_multiBodies[mbIndex]; } - virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); + virtual void addMultiBodyConstraint(btMultiBodyConstraint* constraint); - virtual int getNumMultiBodyConstraints() const + virtual int getNumMultiBodyConstraints() const { - return m_multiBodyConstraints.size(); + return m_multiBodyConstraints.size(); } - virtual btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) + virtual btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex) { - return m_multiBodyConstraints[constraintIndex]; + return m_multiBodyConstraints[constraintIndex]; } - virtual const btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) const + virtual const btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex) const { - return m_multiBodyConstraints[constraintIndex]; + return m_multiBodyConstraints[constraintIndex]; } - virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); + virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint); - virtual void integrateTransforms(btScalar timeStep); + virtual void integrateTransforms(btScalar timeStep); - virtual void debugDrawWorld(); + virtual void debugDrawWorld(); - virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); - - void forwardKinematics(); + virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); + + void forwardKinematics(); virtual void clearForces(); virtual void clearMultiBodyConstraintForces(); virtual void clearMultiBodyForces(); virtual void applyGravity(); - - virtual void serialize(btSerializer* serializer); + + virtual void serialize(btSerializer* serializer); + virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver); + virtual void setConstraintSolver(btConstraintSolver* solver); + virtual void getAnalyticsData(btAlignedObjectArray& m_islandAnalyticsData) const; }; -#endif //BT_MULTIBODY_DYNAMICS_WORLD_H +#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp index 6ca5b8b0d..5ef9444c2 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp @@ -24,27 +24,27 @@ subject to the following restrictions: #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) + : 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 + 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) + : 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 + m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses } void btMultiBodyFixedConstraint::finalizeMultiDof() @@ -57,7 +57,6 @@ btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint() { } - int btMultiBodyFixedConstraint::getIslandIdA() const { if (m_rigidBodyA) @@ -65,13 +64,16 @@ int btMultiBodyFixedConstraint::getIslandIdA() const if (m_bodyA) { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) + if (m_linkA < 0) { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); } } return -1; @@ -83,98 +85,100 @@ int btMultiBodyFixedConstraint::getIslandIdB() const return m_rigidBodyB->getIslandTag(); if (m_bodyB) { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) + if (m_linkB < 0) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->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 - ); - } + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = i; + constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal1.setValue(0, 0, 0); + constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal2.setValue(0, 0, 0); + constraintRow.m_angularComponentA.setValue(0, 0, 0); + constraintRow.m_angularComponentB.setValue(0, 0, 0); + + constraintRow.m_solverBodyIdA = data.m_fixedBodyId; + constraintRow.m_solverBodyIdB = data.m_fixedBodyId; + + // Convert local points back to world + btVector3 pivotAworld = m_pivotInA; + btMatrix3x3 frameAworld = m_frameInA; + if (m_rigidBodyA) + { + constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); + 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); + } } } diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h index 036025136..adb1cb47d 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h @@ -23,16 +23,14 @@ subject to the following restrictions: class btMultiBodyFixedConstraint : public btMultiBodyConstraint { protected: - - btRigidBody* m_rigidBodyA; - btRigidBody* m_rigidBodyB; - btVector3 m_pivotInA; - btVector3 m_pivotInB; - btMatrix3x3 m_frameInA; - btMatrix3x3 m_frameInB; + 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); @@ -44,18 +42,18 @@ public: virtual int getIslandIdB() const; virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); - const btVector3& getPivotInA() const - { - return m_pivotInA; - } - - void setPivotInA(const btVector3& pivotInA) - { - m_pivotInA = pivotInA; - } + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } const btVector3& getPivotInB() const { @@ -66,29 +64,28 @@ public: { 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 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 +#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index 5fdb7007d..bf6b811d2 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -21,20 +21,18 @@ subject to the following restrictions: #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) + : 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; } @@ -42,19 +40,20 @@ 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_linkA < 0) { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); } } return -1; @@ -64,41 +63,40 @@ int btMultiBodyGearConstraint::getIslandIdB() const { if (m_bodyB) { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) + if (m_linkB < 0) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } } return -1; } - void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) + 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. - + // 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(); + finalizeMultiDof(); } //don't crash if (m_numDofsFinalized != m_jacSizeBoth) return; - - if (m_maxAppliedImpulse==0.f) + 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; @@ -111,63 +109,66 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& 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]; + int dof = 0; + btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; btScalar auxVel = 0; - - if (m_gearAuxLink>=0) + + if (m_gearAuxLink >= 0) { auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; } currentVelocity += auxVel; - if (m_erp!=0) + if (m_erp != 0) { btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; - btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof]; - btScalar diff = currentPositionB+currentPositionA; + 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); + 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); + + 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)); + 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; - + 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; + 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(); + constraintRow.m_relpos2CrossNormal.setZero(); break; } default: @@ -175,10 +176,6 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& btAssert(0); } }; - } - } - } - diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h index 0115de624..31888fbc6 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -23,20 +23,18 @@ subject to the following restrictions: 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; - 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); @@ -48,18 +46,18 @@ public: virtual int getIslandIdB() const; virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); - const btVector3& getPivotInA() const - { - return m_pivotInA; - } - - void setPivotInA(const btVector3& pivotInA) - { - m_pivotInA = pivotInA; - } + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } const btVector3& getPivotInB() const { @@ -70,32 +68,32 @@ public: { 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 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; @@ -114,4 +112,4 @@ public: } }; -#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H +#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h index 5c2fa8ed5..d943019e7 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h @@ -12,8 +12,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_MULTIBODY_JOINT_FEEDBACK_H #define BT_MULTIBODY_JOINT_FEEDBACK_H @@ -21,7 +19,7 @@ subject to the following restrictions: struct btMultiBodyJointFeedback { - btSpatialForceVector m_reactionForces; + btSpatialForceVector m_reactionForces; }; -#endif //BT_MULTIBODY_JOINT_FEEDBACK_H +#endif //BT_MULTIBODY_JOINT_FEEDBACK_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 6d173b66a..8791ad286 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -20,21 +20,18 @@ subject to the following restrictions: #include "btMultiBodyLinkCollider.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" - - btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) //:btMultiBodyConstraint(body,0,link,-1,2,true), - :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true), - m_lowerBound(lower), - m_upperBound(upper) + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true), + m_lowerBound(lower), + m_upperBound(upper) { - } void btMultiBodyJointLimitConstraint::finalizeMultiDof() { // the data.m_jacobians never change, so may as well - // initialize them here + // initialize them here allocateJacobiansMultiDof(); @@ -55,15 +52,18 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() int btMultiBodyJointLimitConstraint::getIslandIdA() const { - if(m_bodyA) + if (m_bodyA) { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) + if (m_linkA < 0) { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); } } return -1; @@ -71,88 +71,86 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const int btMultiBodyJointLimitConstraint::getIslandIdB() const { - if(m_bodyB) + if (m_bodyB) { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) + if (m_linkB < 0) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } } return -1; } - void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) + 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. + // 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(); + finalizeMultiDof(); } + // row 0: the lower bound + setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent - // row 0: the lower bound - setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent + // row 1: the upper bound + setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); - // row 1: the upper bound - setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); - - for (int row=0;row0) + if (penetration > 0) { continue; } - btScalar direction = row? -1 : 1; + btScalar direction = row ? -1 : 1; btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = row; - + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + constraintRow.m_multiBodyA = m_bodyA; constraintRow.m_multiBodyB = m_bodyB; - const btScalar posError = 0; //why assume it's zero? + 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,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 - btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); + 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 = direction*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; - + btVector3 revoluteAxisInWorld = direction * 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 = direction* 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; + btVector3 prismaticAxisInWorld = direction * 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: @@ -160,36 +158,35 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint btAssert(0); } }; - } { - btScalar positionalError = 0.f; - btScalar velocityError = - rel_vel;// * damping; + btScalar velocityError = -rel_vel; // * damping; btScalar erp = infoGlobal.m_erp2; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { erp = infoGlobal.m_erp; } - if (penetration>0) + if (penetration > 0) { positionalError = 0; velocityError = -penetration / infoGlobal.m_timeStep; - } else + } + else { - positionalError = -penetration * erp/infoGlobal.m_timeStep; + positionalError = -penetration * erp / infoGlobal.m_timeStep; } - btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs - constraintRow.m_rhs = penetrationImpulse+velocityImpulse; + constraintRow.m_rhs = penetrationImpulse + velocityImpulse; constraintRow.m_rhsPenetration = 0.f; - - } else + } + else { //split position and velocity into rhs and m_rhsPenetration constraintRow.m_rhs = velocityImpulse; @@ -197,9 +194,4 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint } } } - } - - - - diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index 55b8d122b..6716ba490 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -22,11 +22,10 @@ struct btSolverInfo; class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint { protected: + btScalar m_lowerBound; + btScalar m_upperBound; - btScalar m_lowerBound; - btScalar m_upperBound; public: - btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); virtual ~btMultiBodyJointLimitConstraint(); @@ -36,15 +35,13 @@ public: virtual int getIslandIdB() const; virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); virtual void debugDraw(class btIDebugDraw* drawer) { //todo(erwincoumans) } - }; -#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H - +#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index e0921178e..5c816c498 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -20,22 +20,18 @@ subject to the following restrictions: #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) + : 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 - - + // initialize them here } void btMultiBodyJointMotor::finalizeMultiDof() @@ -55,18 +51,17 @@ void btMultiBodyJointMotor::finalizeMultiDof() 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) + : 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() { @@ -74,102 +69,108 @@ btMultiBodyJointMotor::~btMultiBodyJointMotor() int btMultiBodyJointMotor::getIslandIdA() const { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) + if (this->m_linkA < 0) { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + { + return m_bodyA->getLink(m_linkA).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++) + if (m_linkB < 0) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + { + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } + } return -1; } - void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) + 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. - + // 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(); + finalizeMultiDof(); } //don't crash if (m_numDofsFinalized != m_jacSizeBoth) return; - if (m_maxAppliedImpulse==0.f) + 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) + int dof = 0; + btScalar currentPosition = m_bodyA->getJointPosMultiDof(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; + rhs = m_rhsClamp; } - if (rhs<-m_rhsClamp) + if (rhs < -m_rhsClamp) { - 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); + + 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)); + 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; - + 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; + 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: @@ -177,10 +178,6 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con btAssert(0); } }; - } - } - } - diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index 4063bed79..1aca36352 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -24,41 +24,38 @@ 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 - + 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 void finalizeMultiDof(); virtual int getIslandIdA() const; virtual int getIslandIdB() const; virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); - virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f) - { - m_desiredVelocity = velTarget; - m_kd = kd; - } + 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 setPositionTarget(btScalar posTarget, btScalar kp = 1.f) - { - m_desiredPosition = posTarget; - m_kp = kp; - } - virtual void setErp(btScalar erp) { m_erp = erp; @@ -77,5 +74,4 @@ public: } }; -#endif //BT_MULTIBODY_JOINT_MOTOR_H - +#endif //BT_MULTIBODY_JOINT_MOTOR_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 01828e584..92d41dfac 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -20,7 +20,7 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" -enum btMultiBodyLinkFlags +enum btMultiBodyLinkFlags { BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1, BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2, @@ -36,7 +36,6 @@ enum btMultiBodyLinkFlags //namespace { - #include "LinearMath/btSpatialAlgebra.h" //} @@ -45,27 +44,26 @@ enum btMultiBodyLinkFlags // Link struct // -struct btMultibodyLink +struct btMultibodyLink { - BT_DECLARE_ALIGNED_ALLOCATOR(); - btScalar m_mass; // mass of link - btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal) + 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. + 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. + 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. + 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; + + btVector3 m_eVector; btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; @@ -79,13 +77,11 @@ struct btMultibodyLink 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 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) @@ -93,141 +89,141 @@ struct btMultibodyLink // 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 + // 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) + void setAxisBottom(int dof, const btVector3 &axis) { - m_axes[dof].m_topVec.setValue(x, y, z); + m_axes[dof].m_bottomVec = axis; } - void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) - { - m_axes[dof].m_bottomVec.setValue(x, y, z); + void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) + { + m_axes[dof].m_topVec.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; } + 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. + 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_appliedForce; // In WORLD frame + btVector3 m_appliedTorque; // In WORLD frame -btVector3 m_appliedConstraintForce; // In WORLD frame - btVector3 m_appliedConstraintTorque; // 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 + + //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; + + class btMultiBodyLinkCollider *m_collider; int m_flags; - - - int m_dofCount, m_posVarCount; //redundant but handy - + + 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 + 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. - 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_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_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_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 + // routine to update m_cachedRotParentToThis and m_cachedRVector void updateCacheMultiDof(btScalar *pq = 0) { btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); - switch(m_jointType) + switch (m_jointType) { case eRevolute: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + 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); + 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); + 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); + 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); + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); break; } @@ -240,5 +236,4 @@ btVector3 m_appliedConstraintForce; // In WORLD frame } }; - -#endif //BT_MULTIBODY_LINK_H +#endif //BT_MULTIBODY_LINK_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index 671e15d31..bc909990c 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -19,21 +19,32 @@ 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 { -//protected: + //protected: public: - btMultiBody* m_multiBody; int m_link; - - btMultiBodyLinkCollider (btMultiBody* multiBody,int link) - :m_multiBody(multiBody), - m_link(link) + virtual ~btMultiBodyLinkCollider() { - m_checkCollideWith = true; + + } + btMultiBodyLinkCollider(btMultiBody* multiBody, int link) + : m_multiBody(multiBody), + m_link(link) + { + m_checkCollideWith = true; //we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands //this means that some constraints might point to bodies that are not in the islands, causing crashes //if (link>=0 || (multiBody && !multiBody->hasFixedBase())) @@ -49,18 +60,18 @@ public: } static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj) { - if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK) return (btMultiBodyLinkCollider*)colObj; return 0; } static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK) return (btMultiBodyLinkCollider*)colObj; return 0; } - virtual bool checkCollideWithOverride(const btCollisionObject* co) const + virtual bool checkCollideWithOverride(const btCollisionObject* co) const { const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); if (!other) @@ -71,47 +82,46 @@ public: return false; //check if 'link' has collision disabled - if (m_link>=0) + if (m_link >= 0) { const btMultibodyLink& link = m_multiBody->getLink(this->m_link); - if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) + if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) { int parent_of_this = m_link; while (1) { - if (parent_of_this==-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) + if (parent_of_this == other->m_link) { return false; } } } - else if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) + else if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) { - if ( link.m_parent == other->m_link) + if (link.m_parent == other->m_link) return false; } - } - if (other->m_link>=0) + if (other->m_link >= 0) { const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); - if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) + if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) { int parent_of_other = other->m_link; while (1) { - if (parent_of_other==-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) + if (parent_of_other == this->m_link) return false; } } - else if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) + else if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) { if (otherLink.m_parent == this->m_link) return false; @@ -119,7 +129,50 @@ public: } 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; }; -#endif //BT_FEATHERSTONE_LINK_COLLIDER_H +// clang-format off +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]; +}; + +// clang-format on + +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/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp new file mode 100644 index 000000000..f2186a93e --- /dev/null +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp @@ -0,0 +1,966 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 Google 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. +*/ + +#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h" + +#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + +static bool interleaveContactAndFriction1 = false; + +struct btJointNode1 +{ + int jointIndex; // pointer to enclosing dxJoint object + int otherBodyIndex; // *other* body this joint is connected to + int nextJointNodeIndex; //-1 for null + int constraintRowIndex; +}; + +// Helper function to compute a delta velocity in the constraint space. +static btScalar computeDeltaVelocityInConstraintSpace( + const btVector3& angularDeltaVelocity, + const btVector3& contactNormal, + btScalar invMass, + const btVector3& angularJacobian, + const btVector3& linearJacobian) +{ + return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass; +} + +// Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are +// identical. +static btScalar computeDeltaVelocityInConstraintSpace( + const btVector3& angularDeltaVelocity, + btScalar invMass, + const btVector3& angularJacobian) +{ + return angularDeltaVelocity.dot(angularJacobian) + invMass; +} + +// Helper function to compute a delta velocity in the constraint space. +static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size) +{ + btScalar result = 0; + for (int i = 0; i < size; ++i) + result += deltaVelocity[i] * jacobian[i]; + + return result; +} + +static btScalar computeConstraintMatrixDiagElementMultiBody( + const btAlignedObjectArray& solverBodyPool, + const btMultiBodyJacobianData& data, + const btMultiBodySolverConstraint& constraint) +{ + btScalar ret = 0; + + const btMultiBody* multiBodyA = constraint.m_multiBodyA; + const btMultiBody* multiBodyB = constraint.m_multiBodyB; + + if (multiBodyA) + { + const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex]; + const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex]; + const int ndofA = multiBodyA->getNumDofs() + 6; + ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA); + } + else + { + const int solverBodyIdA = constraint.m_solverBodyIdA; + btAssert(solverBodyIdA != -1); + const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA]; + const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0; + ret += computeDeltaVelocityInConstraintSpace( + constraint.m_relpos1CrossNormal, + invMassA, + constraint.m_angularComponentA); + } + + if (multiBodyB) + { + const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex]; + const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex]; + const int ndofB = multiBodyB->getNumDofs() + 6; + ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB); + } + else + { + const int solverBodyIdB = constraint.m_solverBodyIdB; + btAssert(solverBodyIdB != -1); + const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB]; + const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0; + ret += computeDeltaVelocityInConstraintSpace( + constraint.m_relpos2CrossNormal, + invMassB, + constraint.m_angularComponentB); + } + + return ret; +} + +static btScalar computeConstraintMatrixOffDiagElementMultiBody( + const btAlignedObjectArray& solverBodyPool, + const btMultiBodyJacobianData& data, + const btMultiBodySolverConstraint& constraint, + const btMultiBodySolverConstraint& offDiagConstraint) +{ + btScalar offDiagA = btScalar(0); + + const btMultiBody* multiBodyA = constraint.m_multiBodyA; + const btMultiBody* multiBodyB = constraint.m_multiBodyB; + const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA; + const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB; + + // Assumed at least one system is multibody + btAssert(multiBodyA || multiBodyB); + btAssert(offDiagMultiBodyA || offDiagMultiBodyB); + + if (offDiagMultiBodyA) + { + const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex]; + + if (offDiagMultiBodyA == multiBodyA) + { + const int ndofA = multiBodyA->getNumDofs() + 6; + const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex]; + offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA); + } + else if (offDiagMultiBodyA == multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex]; + offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB); + } + } + else + { + const int solverBodyIdA = constraint.m_solverBodyIdA; + const int solverBodyIdB = constraint.m_solverBodyIdB; + + const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA; + btAssert(offDiagSolverBodyIdA != -1); + + if (offDiagSolverBodyIdA == solverBodyIdA) + { + btAssert(solverBodyIdA != -1); + const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA]; + const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0; + offDiagA += computeDeltaVelocityInConstraintSpace( + offDiagConstraint.m_relpos1CrossNormal, + offDiagConstraint.m_contactNormal1, + invMassA, constraint.m_angularComponentA, + constraint.m_contactNormal1); + } + else if (offDiagSolverBodyIdA == solverBodyIdB) + { + btAssert(solverBodyIdB != -1); + const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB]; + const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0; + offDiagA += computeDeltaVelocityInConstraintSpace( + offDiagConstraint.m_relpos1CrossNormal, + offDiagConstraint.m_contactNormal1, + invMassB, + constraint.m_angularComponentB, + constraint.m_contactNormal2); + } + } + + if (offDiagMultiBodyB) + { + const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex]; + + if (offDiagMultiBodyB == multiBodyA) + { + const int ndofA = multiBodyA->getNumDofs() + 6; + const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex]; + offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA); + } + else if (offDiagMultiBodyB == multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex]; + offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB); + } + } + else + { + const int solverBodyIdA = constraint.m_solverBodyIdA; + const int solverBodyIdB = constraint.m_solverBodyIdB; + + const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB; + btAssert(offDiagSolverBodyIdB != -1); + + if (offDiagSolverBodyIdB == solverBodyIdA) + { + btAssert(solverBodyIdA != -1); + const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA]; + const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0; + offDiagA += computeDeltaVelocityInConstraintSpace( + offDiagConstraint.m_relpos2CrossNormal, + offDiagConstraint.m_contactNormal2, + invMassA, constraint.m_angularComponentA, + constraint.m_contactNormal1); + } + else if (offDiagSolverBodyIdB == solverBodyIdB) + { + btAssert(solverBodyIdB != -1); + const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB]; + const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0; + offDiagA += computeDeltaVelocityInConstraintSpace( + offDiagConstraint.m_relpos2CrossNormal, + offDiagConstraint.m_contactNormal2, + invMassB, constraint.m_angularComponentB, + constraint.m_contactNormal2); + } + } + + return offDiagA; +} + +void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) +{ + createMLCPFastRigidBody(infoGlobal); + createMLCPFastMultiBody(infoGlobal); +} + +void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal) +{ + int numContactRows = interleaveContactAndFriction1 ? 3 : 1; + + int numConstraintRows = m_allConstraintPtrArray.size(); + + if (numConstraintRows == 0) + return; + + int n = numConstraintRows; + { + BT_PROFILE("init b (rhs)"); + m_b.resize(numConstraintRows); + m_bSplit.resize(numConstraintRows); + m_b.setZero(); + m_bSplit.setZero(); + for (int i = 0; i < numConstraintRows; i++) + { + btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv; + if (!btFuzzyZero(jacDiag)) + { + btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; + btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; + m_b[i] = rhs / jacDiag; + m_bSplit[i] = rhsPenetration / jacDiag; + } + } + } + + // btScalar* w = 0; + // int nub = 0; + + m_lo.resize(numConstraintRows); + m_hi.resize(numConstraintRows); + + { + BT_PROFILE("init lo/ho"); + + for (int i = 0; i < numConstraintRows; i++) + { + if (0) //m_limitDependencies[i]>=0) + { + m_lo[i] = -BT_INFINITY; + m_hi[i] = BT_INFINITY; + } + else + { + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; + } + } + } + + // + int m = m_allConstraintPtrArray.size(); + + int numBodies = m_tmpSolverBodyPool.size(); + btAlignedObjectArray bodyJointNodeArray; + { + BT_PROFILE("bodyJointNodeArray.resize"); + bodyJointNodeArray.resize(numBodies, -1); + } + btAlignedObjectArray jointNodeArray; + { + BT_PROFILE("jointNodeArray.reserve"); + jointNodeArray.reserve(2 * m_allConstraintPtrArray.size()); + } + + btMatrixXu& J3 = m_scratchJ3; + { + BT_PROFILE("J3.resize"); + J3.resize(2 * m, 8); + } + btMatrixXu& JinvM3 = m_scratchJInvM3; + { + BT_PROFILE("JinvM3.resize/setZero"); + + JinvM3.resize(2 * m, 8); + JinvM3.setZero(); + J3.setZero(); + } + int cur = 0; + int rowOffset = 0; + btAlignedObjectArray& ofs = m_scratchOfs; + { + BT_PROFILE("ofs resize"); + ofs.resize(0); + ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); + } + { + BT_PROFILE("Compute J and JinvM"); + int c = 0; + + int numRows = 0; + + for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++) + { + ofs[c] = rowOffset; + int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows; + if (orgBodyA) + { + { + int slotA = -1; + //find free jointNode slot for sbA + slotA = jointNodeArray.size(); + jointNodeArray.expand(); //NonInitializing(); + int prevSlot = bodyJointNodeArray[sbA]; + bodyJointNodeArray[sbA] = slotA; + jointNodeArray[slotA].nextJointNodeIndex = prevSlot; + jointNodeArray[slotA].jointIndex = c; + jointNodeArray[slotA].constraintRowIndex = i; + jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1; + } + for (int row = 0; row < numRows; row++, cur++) + { + btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + + for (int r = 0; r < 3; r++) + { + J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]); + J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]); + JinvM3.setElem(cur, r, normalInvMass[r]); + JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]); + } + J3.setElem(cur, 3, 0); + JinvM3.setElem(cur, 3, 0); + J3.setElem(cur, 7, 0); + JinvM3.setElem(cur, 7, 0); + } + } + else + { + cur += numRows; + } + if (orgBodyB) + { + { + int slotB = -1; + //find free jointNode slot for sbA + slotB = jointNodeArray.size(); + jointNodeArray.expand(); //NonInitializing(); + int prevSlot = bodyJointNodeArray[sbB]; + bodyJointNodeArray[sbB] = slotB; + jointNodeArray[slotB].nextJointNodeIndex = prevSlot; + jointNodeArray[slotB].jointIndex = c; + jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; + jointNodeArray[slotB].constraintRowIndex = i; + } + + for (int row = 0; row < numRows; row++, cur++) + { + btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + + for (int r = 0; r < 3; r++) + { + J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]); + J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]); + JinvM3.setElem(cur, r, normalInvMassB[r]); + JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]); + } + J3.setElem(cur, 3, 0); + JinvM3.setElem(cur, 3, 0); + J3.setElem(cur, 7, 0); + JinvM3.setElem(cur, 7, 0); + } + } + else + { + cur += numRows; + } + rowOffset += numRows; + } + } + + //compute JinvM = J*invM. + const btScalar* JinvM = JinvM3.getBufferPointer(); + + const btScalar* Jptr = J3.getBufferPointer(); + { + BT_PROFILE("m_A.resize"); + m_A.resize(n, n); + } + + { + BT_PROFILE("m_A.setZero"); + m_A.setZero(); + } + int c = 0; + { + int numRows = 0; + BT_PROFILE("Compute A"); + for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++) + { + int row__ = ofs[c]; + int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows; + + const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; + + { + int startJointNodeA = bodyJointNodeArray[sbA]; + while (startJointNodeA >= 0) + { + int j0 = jointNodeArray[startJointNodeA].jointIndex; + int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; + if (j0 < c) + { + int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows; + size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0; + //printf("%d joint i %d and j0: %d: ",count++,i,j0); + m_A.multiplyAdd2_p8r(JinvMrow, + Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]); + } + startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; + } + } + + { + int startJointNodeB = bodyJointNodeArray[sbB]; + while (startJointNodeB >= 0) + { + int j1 = jointNodeArray[startJointNodeB].jointIndex; + int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; + + if (j1 < c) + { + int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; + size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0; + m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows, + Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]); + } + startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; + } + } + } + + { + BT_PROFILE("compute diagonal"); + // compute diagonal blocks of m_A + + int row__ = 0; + int numJointRows = m_allConstraintPtrArray.size(); + + int jj = 0; + for (; row__ < numJointRows;) + { + //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; + + const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; + const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__; + m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__); + if (orgBodyB) + { + m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__); + } + row__ += infom; + jj++; + } + } + } + + if (1) + { + // add cfm to the diagonal of m_A + for (int i = 0; i < m_A.rows(); ++i) + { + m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); + } + } + + ///fill the upper triangle of the matrix, to make it symmetric + { + BT_PROFILE("fill the upper triangle "); + m_A.copyLowerToUpperTriangle(); + } + + { + BT_PROFILE("resize/init x"); + m_x.resize(numConstraintRows); + m_xSplit.resize(numConstraintRows); + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + for (int i = 0; i < m_allConstraintPtrArray.size(); i++) + { + const btSolverConstraint& c = *m_allConstraintPtrArray[i]; + m_x[i] = c.m_appliedImpulse; + m_xSplit[i] = c.m_appliedPushImpulse; + } + } + else + { + m_x.setZero(); + m_xSplit.setZero(); + } + } +} + +void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal) +{ + const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size(); + + if (multiBodyNumConstraints == 0) + return; + + // 1. Compute b + { + BT_PROFILE("init b (rhs)"); + + m_multiBodyB.resize(multiBodyNumConstraints); + m_multiBodyB.setZero(); + + for (int i = 0; i < multiBodyNumConstraints; ++i) + { + const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; + const btScalar jacDiag = constraint.m_jacDiagABInv; + + if (!btFuzzyZero(jacDiag)) + { + // Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet. + const btScalar rhs = constraint.m_rhs; + m_multiBodyB[i] = rhs / jacDiag; + } + } + } + + // 2. Compute lo and hi + { + BT_PROFILE("init lo/ho"); + + m_multiBodyLo.resize(multiBodyNumConstraints); + m_multiBodyHi.resize(multiBodyNumConstraints); + + for (int i = 0; i < multiBodyNumConstraints; ++i) + { + const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; + m_multiBodyLo[i] = constraint.m_lowerLimit; + m_multiBodyHi[i] = constraint.m_upperLimit; + } + } + + // 3. Construct A matrix by using the impulse testing + { + BT_PROFILE("Compute A"); + + { + BT_PROFILE("m_A.resize"); + m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints); + } + + for (int i = 0; i < multiBodyNumConstraints; ++i) + { + // Compute the diagonal of A, which is A(i, i) + const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; + const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint); + m_multiBodyA.setElem(i, i, diagA); + + // Computes the off-diagonals of A: + // a. The rest of i-th row of A, from A(i, i+1) to A(i, n) + // b. The rest of i-th column of A, from A(i+1, i) to A(n, i) + for (int j = i + 1; j < multiBodyNumConstraints; ++j) + { + const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j]; + const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint); + + // Set the off-diagonal values of A. Note that A is symmetric. + m_multiBodyA.setElem(i, j, offDiagA); + m_multiBodyA.setElem(j, i, offDiagA); + } + } + } + + // Add CFM to the diagonal of m_A + for (int i = 0; i < m_multiBodyA.rows(); ++i) + { + m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); + } + + // 4. Initialize x + { + BT_PROFILE("resize/init x"); + + m_multiBodyX.resize(multiBodyNumConstraints); + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + for (int i = 0; i < multiBodyNumConstraints; ++i) + { + const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; + m_multiBodyX[i] = constraint.m_appliedImpulse; + } + } + else + { + m_multiBodyX.setZero(); + } + } +} + +bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal) +{ + bool result = true; + + if (m_A.rows() != 0) + { + // If using split impulse, we solve 2 separate (M)LCPs + if (infoGlobal.m_splitImpulse) + { + const btMatrixXu Acopy = m_A; + const btAlignedObjectArray limitDependenciesCopy = m_limitDependencies; + // TODO(JS): Do we really need these copies when solveMLCP takes them as const? + + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); + if (result) + result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations); + } + else + { + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); + } + } + + if (!result) + return false; + + if (m_multiBodyA.rows() != 0) + { + result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations); + } + + return result; +} + +btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup( + btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& infoGlobal, + btIDebugDraw* debugDrawer) +{ + // 1. Setup for rigid-bodies + btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup( + bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + + // 2. Setup for multi-bodies + // a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray + // b. Set the index array for frictional contact constraints, m_limitDependencies + { + BT_PROFILE("gather constraint data"); + + int dindex = 0; + + const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size(); + const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size(); + + m_allConstraintPtrArray.resize(0); + m_multiBodyAllConstraintPtrArray.resize(0); + + // i. Setup for rigid bodies + + m_limitDependencies.resize(numRigidBodyConstraints); + + for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i) + { + m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]); + m_limitDependencies[dindex++] = -1; + } + + int firstContactConstraintOffset = dindex; + + // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead + if (interleaveContactAndFriction1) + { + for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) + { + const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2; + + m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); + m_limitDependencies[dindex++] = -1; + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]); + int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact)); + m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; + if (numFrictionPerContact == 2) + { + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]); + m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; + } + } + } + else + { + for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) + { + m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); + m_limitDependencies[dindex++] = -1; + } + for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++) + { + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]); + m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset; + } + } + + if (!m_allConstraintPtrArray.size()) + { + m_A.resize(0, 0); + m_b.resize(0); + m_x.resize(0); + m_lo.resize(0); + m_hi.resize(0); + } + + // ii. Setup for multibodies + + dindex = 0; + + m_multiBodyLimitDependencies.resize(numMultiBodyConstraints); + + for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i) + { + m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]); + m_multiBodyLimitDependencies[dindex++] = -1; + } + + firstContactConstraintOffset = dindex; + + // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead + if (interleaveContactAndFriction1) + { + for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i) + { + const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2; + + m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]); + m_multiBodyLimitDependencies[dindex++] = -1; + + btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact]; + m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1); + + const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset; + + m_multiBodyLimitDependencies[dindex++] = findex; + + if (numtiBodyNumFrictionPerContact == 2) + { + btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1]; + m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2); + + m_multiBodyLimitDependencies[dindex++] = findex; + } + } + } + else + { + for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i) + { + m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]); + m_multiBodyLimitDependencies[dindex++] = -1; + } + for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i) + { + m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]); + m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset; + } + } + + if (!m_multiBodyAllConstraintPtrArray.size()) + { + m_multiBodyA.resize(0, 0); + m_multiBodyB.resize(0); + m_multiBodyX.resize(0); + m_multiBodyLo.resize(0); + m_multiBodyHi.resize(0); + } + } + + // Construct MLCP terms + { + BT_PROFILE("createMLCPFast"); + createMLCPFast(infoGlobal); + } + + return btScalar(0); +} + +btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +{ + bool result = true; + { + BT_PROFILE("solveMLCP"); + result = solveMLCP(infoGlobal); + } + + // Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid. + if (!result) + { + m_fallback++; + return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + } + + { + BT_PROFILE("process MLCP results"); + + for (int i = 0; i < m_allConstraintPtrArray.size(); ++i) + { + const btSolverConstraint& c = *m_allConstraintPtrArray[i]; + + const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse; + c.m_appliedImpulse = m_x[i]; + + int sbA = c.m_solverBodyIdA; + int sbB = c.m_solverBodyIdB; + + btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; + btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; + + solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + + if (infoGlobal.m_splitImpulse) + { + const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse; + solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse); + solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse); + c.m_appliedPushImpulse = m_xSplit[i]; + } + } + + for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i) + { + btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i]; + + const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse; + c.m_appliedImpulse = m_multiBodyX[i]; + + btMultiBody* multiBodyA = c.m_multiBodyA; + if (multiBodyA) + { + const int ndofA = multiBodyA->getNumDofs() + 6; + 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 + multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse); +#endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else + { + const int sbA = c.m_solverBodyIdA; + btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; + solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + } + + btMultiBody* multiBodyB = c.m_multiBodyB; + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + 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 + multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse); +#endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else + { + const int sbB = c.m_solverBodyIdB; + btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; + solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + } + } + } + + return btScalar(0); +} + +btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver) + : m_solver(solver), m_fallback(0) +{ + // Do nothing +} + +btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver() +{ + // Do nothing +} + +void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver) +{ + m_solver = solver; +} + +int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const +{ + return m_fallback; +} + +void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num) +{ + m_fallback = num; +} + +btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const +{ + return BT_MLCP_SOLVER; +} diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h new file mode 100644 index 000000000..77fdb86bb --- /dev/null +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h @@ -0,0 +1,187 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 Google 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 BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H +#define BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H + +#include "LinearMath/btMatrixX.h" +#include "LinearMath/btThreads.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" + +class btMLCPSolverInterface; +class btMultiBody; + +class btMultiBodyMLCPConstraintSolver : public btMultiBodyConstraintSolver +{ +protected: + /// \name MLCP Formulation for Rigid Bodies + /// \{ + + /// A matrix in the MLCP formulation + btMatrixXu m_A; + + /// b vector in the MLCP formulation. + btVectorXu m_b; + + /// Constraint impulse, which is an output of MLCP solving. + btVectorXu m_x; + + /// Lower bound of constraint impulse, \c m_x. + btVectorXu m_lo; + + /// Upper bound of constraint impulse, \c m_x. + btVectorXu m_hi; + + /// \} + + /// \name Cache Variables for Split Impulse for Rigid Bodies + /// When using 'split impulse' we solve two separate (M)LCPs + /// \{ + + /// Split impulse Cache vector corresponding to \c m_b. + btVectorXu m_bSplit; + + /// Split impulse cache vector corresponding to \c m_x. + btVectorXu m_xSplit; + + /// \} + + /// \name MLCP Formulation for Multibodies + /// \{ + + /// A matrix in the MLCP formulation + btMatrixXu m_multiBodyA; + + /// b vector in the MLCP formulation. + btVectorXu m_multiBodyB; + + /// Constraint impulse, which is an output of MLCP solving. + btVectorXu m_multiBodyX; + + /// Lower bound of constraint impulse, \c m_x. + btVectorXu m_multiBodyLo; + + /// Upper bound of constraint impulse, \c m_x. + btVectorXu m_multiBodyHi; + + /// \} + + /// Indices of normal contact constraint associated with frictional contact constraint for rigid bodies. + /// + /// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate + /// normal contact impulse. For example, i-th element represents the index of a normal constraint that is + /// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint. + /// Otherwise, -1. + btAlignedObjectArray m_limitDependencies; + + /// Indices of normal contact constraint associated with frictional contact constraint for multibodies. + /// + /// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate + /// normal contact impulse. For example, i-th element represents the index of a normal constraint that is + /// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint. + /// Otherwise, -1. + btAlignedObjectArray m_multiBodyLimitDependencies; + + /// Array of all the rigid body constraints + btAlignedObjectArray m_allConstraintPtrArray; + + /// Array of all the multibody constraints + btAlignedObjectArray m_multiBodyAllConstraintPtrArray; + + /// MLCP solver + btMLCPSolverInterface* m_solver; + + /// Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver fails. + int m_fallback; + + /// \name MLCP Scratch Variables + /// 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. + /// + /// \{ + + /// Cache variable for constraint Jacobian matrix. + btMatrixXu m_scratchJ3; + + /// Cache variable for constraint Jacobian times inverse mass matrix. + btMatrixXu m_scratchJInvM3; + + /// Cache variable for offsets. + btAlignedObjectArray m_scratchOfs; + + /// \} + + /// Constructs MLCP terms, which are \c m_A, \c m_b, \c m_lo, and \c m_hi. + virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); + + /// Constructs MLCP terms for constraints of two rigid bodies + void createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal); + + /// Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody + void createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal); + + /// Solves MLCP and returns the success + virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); + + // Documentation inherited + btScalar solveGroupCacheFriendlySetup( + btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& infoGlobal, + btIDebugDraw* debugDrawer) BT_OVERRIDE; + + // Documentation inherited + btScalar solveGroupCacheFriendlyIterations( + btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& infoGlobal, + btIDebugDraw* debugDrawer) ; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR() + + /// Constructor + /// + /// \param[in] solver MLCP solver. Assumed it's not null. + /// \param[in] maxLCPSize Maximum size of LCP to solve using MLCP solver. If the MLCP size exceeds this number, sequaltial impulse method will be used. + explicit btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver); + + /// Destructor + virtual ~btMultiBodyMLCPConstraintSolver(); + + /// Sets MLCP solver. Assumed it's not null. + void setMLCPSolver(btMLCPSolverInterface* solver); + + /// Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP + /// solver fails. + int getNumFallbacks() const; + + /// Sets the number of fallbacks. This function may be used to reset the number to zero. + void setNumFallbacks(int num); + + /// Returns the constraint solver type. + virtual btConstraintSolverType getSolverType() const; +}; + +#endif // BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 125d52ad0..37d3aede3 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -21,29 +21,29 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - #define BTMBP2PCONSTRAINT_DIM 3 +#define BTMBP2PCONSTRAINT_DIM 3 #else - #define BTMBP2PCONSTRAINT_DIM 6 +#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) + : 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 + 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) + : 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 + m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses } void btMultiBodyPoint2Point::finalizeMultiDof() @@ -56,7 +56,6 @@ btMultiBodyPoint2Point::~btMultiBodyPoint2Point() { } - int btMultiBodyPoint2Point::getIslandIdA() const { if (m_rigidBodyA) @@ -64,13 +63,16 @@ int btMultiBodyPoint2Point::getIslandIdA() const if (m_bodyA) { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) + if (m_linkA < 0) { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); } } return -1; @@ -82,62 +84,58 @@ int btMultiBodyPoint2Point::getIslandIdB() const return m_rigidBodyB->getIslandTag(); if (m_bodyB) { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) + if (m_linkB < 0) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } } return -1; } - - void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) { - -// int i=1; -int numDim = BTMBP2PCONSTRAINT_DIM; - for (int i=0;igetCompanionId(); - pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; - } else + pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + } + else { if (m_bodyA) pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); @@ -146,44 +144,41 @@ int numDim = BTMBP2PCONSTRAINT_DIM; if (m_rigidBodyB) { constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); - pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; - } else + 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; + 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 + 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; + 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 - ); + dummy, dummy, dummy, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse); #endif } } diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index bf39acc5b..ef03a557e 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -22,22 +22,20 @@ subject to the following restrictions: //#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST -ATTRIBUTE_ALIGNED16(class) btMultiBodyPoint2Point : public btMultiBodyConstraint +ATTRIBUTE_ALIGNED16(class) +btMultiBodyPoint2Point : public btMultiBodyConstraint { protected: - - btRigidBody* m_rigidBodyA; - btRigidBody* m_rigidBodyB; - btVector3 m_pivotInA; - btVector3 m_pivotInB; - + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; 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); + 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); virtual ~btMultiBodyPoint2Point(); @@ -46,9 +44,9 @@ public: virtual int getIslandIdA() const; virtual int getIslandIdB() const; - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); + virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows, + btMultiBodyJacobianData & data, + const btContactSolverInfo& infoGlobal); const btVector3& getPivotInB() const { @@ -60,9 +58,7 @@ public: m_pivotInB = pivotInB; } - - virtual void debugDraw(class btIDebugDraw* drawer); - + virtual void debugDraw(class btIDebugDraw * drawer); }; -#endif //BT_MULTIBODY_POINT2POINT_H +#endif //BT_MULTIBODY_POINT2POINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp index 3b64b8183..e025302ce 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -25,29 +25,29 @@ subject to the following restrictions: #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) + : 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 + 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) + : 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 + m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses } void btMultiBodySliderConstraint::finalizeMultiDof() @@ -60,7 +60,6 @@ btMultiBodySliderConstraint::~btMultiBodySliderConstraint() { } - int btMultiBodySliderConstraint::getIslandIdA() const { if (m_rigidBodyA) @@ -68,13 +67,16 @@ int btMultiBodySliderConstraint::getIslandIdA() const if (m_bodyA) { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) + if (m_linkA < 0) { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); } } return -1; @@ -86,114 +88,116 @@ int btMultiBodySliderConstraint::getIslandIdB() const return m_rigidBodyB->getIslandTag(); if (m_bodyB) { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) + if (m_linkB < 0) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->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 - ); - } + 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; i < numDim; i++) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = i; + constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal1.setValue(0, 0, 0); + constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal2.setValue(0, 0, 0); + constraintRow.m_angularComponentA.setValue(0, 0, 0); + constraintRow.m_angularComponentB.setValue(0, 0, 0); + + constraintRow.m_solverBodyIdA = data.m_fixedBodyId; + constraintRow.m_solverBodyIdB = data.m_fixedBodyId; + + if (m_rigidBodyA) + { + constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); + } + 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); + } } } diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h index 0a6cf3df1..b192b6f8f 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h @@ -23,17 +23,15 @@ subject to the following restrictions: 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; + 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); @@ -45,18 +43,18 @@ public: virtual int getIslandIdB() const; virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); - const btVector3& getPivotInA() const - { - return m_pivotInA; - } - - void setPivotInA(const btVector3& pivotInA) - { - m_pivotInA = pivotInA; - } + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } const btVector3& getPivotInB() const { @@ -67,39 +65,38 @@ public: { 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; - } + + 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 +#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index 6fa1550e9..deed3e2a1 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -25,66 +25,66 @@ class btMultiBodyConstraint; #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint +ATTRIBUTE_ALIGNED16(struct) +btMultiBodySolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1) - {} - - int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 - int m_jacAindex; - int m_deltaVelBindex; - int m_jacBindex; - - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal1; - btVector3 m_relpos2CrossNormal; - btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always - - - btVector3 m_angularComponentA; - btVector3 m_angularComponentB; - - mutable btSimdScalar m_appliedPushImpulse; - mutable btSimdScalar m_appliedImpulse; - - btScalar m_friction; - btScalar m_jacDiagABInv; - btScalar m_rhs; - btScalar m_cfm; - - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_rhsPenetration; - union + btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1), m_orgConstraint(0), m_orgDofIndex(-1) { - void* m_originalContactPoint; - btScalar m_unusedPadding4; + } + + int m_deltaVelAindex; //more generic version of m_relpos1CrossNormal/m_contactNormal1 + int m_jacAindex; + int m_deltaVelBindex; + int m_jacBindex; + + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal1; + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always + + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + mutable btSimdScalar m_appliedPushImpulse; + mutable btSimdScalar m_appliedImpulse; + + btScalar m_friction; + btScalar m_jacDiagABInv; + btScalar m_rhs; + btScalar m_cfm; + + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_rhsPenetration; + union { + void* m_originalContactPoint; + btScalar m_unusedPadding4; }; - int m_overrideNumSolverIterations; - int m_frictionIndex; + int m_overrideNumSolverIterations; + int m_frictionIndex; int m_solverBodyIdA; btMultiBody* m_multiBodyA; - int m_linkA; - + int m_linkA; + int m_solverBodyIdB; btMultiBody* m_multiBodyB; - int m_linkB; + int m_linkB; //for writing back applied impulses - btMultiBodyConstraint* m_orgConstraint; + btMultiBodyConstraint* m_orgConstraint; int m_orgDofIndex; - enum btSolverConstraintType + enum btSolverConstraintType { BT_SOLVER_CONTACT_1D = 0, BT_SOLVER_FRICTION_1D }; }; -typedef btAlignedObjectArray btMultiBodyConstraintArray; +typedef btAlignedObjectArray btMultiBodyConstraintArray; -#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H +#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp new file mode 100644 index 000000000..3e5aa30f2 --- /dev/null +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -0,0 +1,172 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 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 "btMultiBodySphericalJointMotor.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "LinearMath/btTransformUtil.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" + +btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse) + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true), + m_desiredVelocity(0, 0, 0), + m_desiredPosition(0,0,0,1), + m_kd(1.), + m_kp(0.2), + m_erp(1), + m_rhsClamp(SIMD_INFINITY) +{ + + m_maxAppliedImpulse = maxMotorImpulse; +} + + +void btMultiBodySphericalJointMotor::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; +} + + +btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor() +{ +} + +int btMultiBodySphericalJointMotor::getIslandIdA() const +{ + if (this->m_linkA < 0) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + { + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodySphericalJointMotor::getIslandIdB() const +{ + if (m_linkB < 0) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + { + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } + } + return -1; +} + +void btMultiBodySphericalJointMotor::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); + + + btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) }; + + btQuaternion desiredQuat = m_desiredPosition; + btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0], + m_bodyA->getJointPosMultiDof(m_linkA)[1], + m_bodyA->getJointPosMultiDof(m_linkA)[2], + m_bodyA->getJointPosMultiDof(m_linkA)[3]); + +btQuaternion relRot = currentQuat.inverse() * desiredQuat; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff); + + + + for (int row = 0; row < getNumRows(); row++) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + + int dof = row; + + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar desiredVelocity = this->m_desiredVelocity[row]; + + btScalar velocityError = desiredVelocity - currentVelocity; + + btMatrix3x3 frameAworld; + frameAworld.setIdentity(); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld); + btScalar posError = 0; + { + btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical); + switch (m_bodyA->getLink(m_linkA).m_jointType) + { + case btMultibodyLink::eSpherical: + { + btVector3 constraintNormalAng = frameAworld.getColumn(row % 3); + posError = m_kp*angleDiff[row % 3]; + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + btVector3(0,0,0), dummy, dummy, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse, true); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + break; + } + default: + { + btAssert(0); + } + }; + } + } +} diff --git a/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h new file mode 100644 index 000000000..621beab5a --- /dev/null +++ b/Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h @@ -0,0 +1,77 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 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_SPHERICAL_JOINT_MOTOR_H +#define BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodySphericalJointMotor : public btMultiBodyConstraint +{ +protected: + btVector3 m_desiredVelocity; + btQuaternion m_desiredPosition; + btScalar m_kd; + btScalar m_kp; + btScalar m_erp; + btScalar m_rhsClamp; //maximum error + +public: + btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse); + + virtual ~btMultiBodySphericalJointMotor(); + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f) + { + m_desiredVelocity = velTarget; + m_kd = kd; + } + + virtual void setPositionTarget(const btQuaternion& 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_SPHERICAL_JOINT_MOTOR_H diff --git a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp index 986f21487..98ecdc079 100644 --- a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp @@ -108,18 +108,16 @@ rows/columns and manipulate C. */ - #include "btDantzigLCP.h" -#include //memcpy +#include //memcpy bool s_error = false; //*************************************************************************** // code generation parameters - -#define btLCP_FAST // use fast btLCP object +#define btLCP_FAST // use fast btLCP object // option 1 : matrix row pointers (less data copying) #define BTROWPTRS @@ -133,8 +131,6 @@ bool s_error = false; #define BTNUB_OPTIMIZATIONS - - /* solve L*X=B, with B containing 1 right hand sides. * L is an n*n lower triangular matrix with ones on the diagonal. * L is stored by rows and its leading dimension is lskip. @@ -145,66 +141,69 @@ bool s_error = false; * if this is in the factorizer source file, n must be a multiple of 2. */ -static void btSolveL1_1 (const btScalar *L, btScalar *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - btScalar Z11,m11,Z21,m21,p1,q1,p2,*ex; - const btScalar *ell; - int i,j; - /* compute all 2 x 1 blocks of X */ - for (i=0; i < n; i+=2) { - /* compute all 2 x 1 block of X, from rows i..i+2-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z21=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-2; j >= 0; j -= 2) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - p2=ell[lskip1]; - m21 = p2 * q1; - Z11 += m11; - Z21 += m21; - /* compute outer product and add it to the Z matrix */ - p1=ell[1]; - q1=ex[1]; - m11 = p1 * q1; - p2=ell[1+lskip1]; - m21 = p2 * q1; - /* advance pointers */ - ell += 2; - ex += 2; - Z11 += m11; - Z21 += m21; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 2; - for (; j > 0; j--) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - p2=ell[lskip1]; - m21 = p2 * q1; - /* advance pointers */ - ell += 1; - ex += 1; - Z11 += m11; - Z21 += m21; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - p1 = ell[lskip1]; - Z21 = ex[1] - Z21 - p1*Z11; - ex[1] = Z21; - /* end of outer loop */ - } +static void btSolveL1_1(const btScalar *L, btScalar *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + btScalar Z11, m11, Z21, m21, p1, q1, p2, *ex; + const btScalar *ell; + int i, j; + /* compute all 2 x 1 blocks of X */ + for (i = 0; i < n; i += 2) + { + /* compute all 2 x 1 block of X, from rows i..i+2-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + Z21 = 0; + ell = L + i * lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 2; j >= 0; j -= 2) + { + /* compute outer product and add it to the Z matrix */ + p1 = ell[0]; + q1 = ex[0]; + m11 = p1 * q1; + p2 = ell[lskip1]; + m21 = p2 * q1; + Z11 += m11; + Z21 += m21; + /* compute outer product and add it to the Z matrix */ + p1 = ell[1]; + q1 = ex[1]; + m11 = p1 * q1; + p2 = ell[1 + lskip1]; + m21 = p2 * q1; + /* advance pointers */ + ell += 2; + ex += 2; + Z11 += m11; + Z21 += m21; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 2; + for (; j > 0; j--) + { + /* compute outer product and add it to the Z matrix */ + p1 = ell[0]; + q1 = ex[0]; + m11 = p1 * q1; + p2 = ell[lskip1]; + m21 = p2 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + Z11 += m11; + Z21 += m21; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1 * Z11; + ex[1] = Z21; + /* end of outer loop */ + } } /* solve L*X=B, with B containing 2 right hand sides. @@ -217,300 +216,308 @@ static void btSolveL1_1 (const btScalar *L, btScalar *B, int n, int lskip1) * if this is in the factorizer source file, n must be a multiple of 2. */ -static void btSolveL1_2 (const btScalar *L, btScalar *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - btScalar Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex; - const btScalar *ell; - int i,j; - /* compute all 2 x 2 blocks of X */ - for (i=0; i < n; i+=2) { - /* compute all 2 x 2 block of X, from rows i..i+2-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z12=0; - Z21=0; - Z22=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-2; j >= 0; j -= 2) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - q2=ex[lskip1]; - m12 = p1 * q2; - p2=ell[lskip1]; - m21 = p2 * q1; - m22 = p2 * q2; - Z11 += m11; - Z12 += m12; - Z21 += m21; - Z22 += m22; - /* compute outer product and add it to the Z matrix */ - p1=ell[1]; - q1=ex[1]; - m11 = p1 * q1; - q2=ex[1+lskip1]; - m12 = p1 * q2; - p2=ell[1+lskip1]; - m21 = p2 * q1; - m22 = p2 * q2; - /* advance pointers */ - ell += 2; - ex += 2; - Z11 += m11; - Z12 += m12; - Z21 += m21; - Z22 += m22; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 2; - for (; j > 0; j--) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - q2=ex[lskip1]; - m12 = p1 * q2; - p2=ell[lskip1]; - m21 = p2 * q1; - m22 = p2 * q2; - /* advance pointers */ - ell += 1; - ex += 1; - Z11 += m11; - Z12 += m12; - Z21 += m21; - Z22 += m22; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - Z12 = ex[lskip1] - Z12; - ex[lskip1] = Z12; - p1 = ell[lskip1]; - Z21 = ex[1] - Z21 - p1*Z11; - ex[1] = Z21; - Z22 = ex[1+lskip1] - Z22 - p1*Z12; - ex[1+lskip1] = Z22; - /* end of outer loop */ - } +static void btSolveL1_2(const btScalar *L, btScalar *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + btScalar Z11, m11, Z12, m12, Z21, m21, Z22, m22, p1, q1, p2, q2, *ex; + const btScalar *ell; + int i, j; + /* compute all 2 x 2 blocks of X */ + for (i = 0; i < n; i += 2) + { + /* compute all 2 x 2 block of X, from rows i..i+2-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + Z12 = 0; + Z21 = 0; + Z22 = 0; + ell = L + i * lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 2; j >= 0; j -= 2) + { + /* compute outer product and add it to the Z matrix */ + p1 = ell[0]; + q1 = ex[0]; + m11 = p1 * q1; + q2 = ex[lskip1]; + m12 = p1 * q2; + p2 = ell[lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + /* compute outer product and add it to the Z matrix */ + p1 = ell[1]; + q1 = ex[1]; + m11 = p1 * q1; + q2 = ex[1 + lskip1]; + m12 = p1 * q2; + p2 = ell[1 + lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + /* advance pointers */ + ell += 2; + ex += 2; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 2; + for (; j > 0; j--) + { + /* compute outer product and add it to the Z matrix */ + p1 = ell[0]; + q1 = ex[0]; + m11 = p1 * q1; + q2 = ex[lskip1]; + m12 = p1 * q2; + p2 = ell[lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + /* advance pointers */ + ell += 1; + ex += 1; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + Z12 = ex[lskip1] - Z12; + ex[lskip1] = Z12; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1 * Z11; + ex[1] = Z21; + Z22 = ex[1 + lskip1] - Z22 - p1 * Z12; + ex[1 + lskip1] = Z22; + /* end of outer loop */ + } } +void btFactorLDLT(btScalar *A, btScalar *d, int n, int nskip1) +{ + int i, j; + btScalar sum, *ell, *dee, dd, p1, p2, q1, q2, Z11, m11, Z21, m21, Z22, m22; + if (n < 1) return; -void btFactorLDLT (btScalar *A, btScalar *d, int n, int nskip1) -{ - int i,j; - btScalar sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22; - if (n < 1) return; - - for (i=0; i<=n-2; i += 2) { - /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */ - btSolveL1_2 (A,A+i*nskip1,i,nskip1); - /* scale the elements in a 2 x i block at A(i,0), and also */ - /* compute Z = the outer product matrix that we'll need. */ - Z11 = 0; - Z21 = 0; - Z22 = 0; - ell = A+i*nskip1; - dee = d; - for (j=i-6; j >= 0; j -= 6) { - p1 = ell[0]; - p2 = ell[nskip1]; - dd = dee[0]; - q1 = p1*dd; - q2 = p2*dd; - ell[0] = q1; - ell[nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[1]; - p2 = ell[1+nskip1]; - dd = dee[1]; - q1 = p1*dd; - q2 = p2*dd; - ell[1] = q1; - ell[1+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[2]; - p2 = ell[2+nskip1]; - dd = dee[2]; - q1 = p1*dd; - q2 = p2*dd; - ell[2] = q1; - ell[2+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[3]; - p2 = ell[3+nskip1]; - dd = dee[3]; - q1 = p1*dd; - q2 = p2*dd; - ell[3] = q1; - ell[3+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[4]; - p2 = ell[4+nskip1]; - dd = dee[4]; - q1 = p1*dd; - q2 = p2*dd; - ell[4] = q1; - ell[4+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[5]; - p2 = ell[5+nskip1]; - dd = dee[5]; - q1 = p1*dd; - q2 = p2*dd; - ell[5] = q1; - ell[5+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - ell += 6; - dee += 6; - } - /* compute left-over iterations */ - j += 6; - for (; j > 0; j--) { - p1 = ell[0]; - p2 = ell[nskip1]; - dd = dee[0]; - q1 = p1*dd; - q2 = p2*dd; - ell[0] = q1; - ell[nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - ell++; - dee++; - } - /* solve for diagonal 2 x 2 block at A(i,i) */ - Z11 = ell[0] - Z11; - Z21 = ell[nskip1] - Z21; - Z22 = ell[1+nskip1] - Z22; - dee = d + i; - /* factorize 2 x 2 block Z,dee */ - /* factorize row 1 */ - dee[0] = btRecip(Z11); - /* factorize row 2 */ - sum = 0; - q1 = Z21; - q2 = q1 * dee[0]; - Z21 = q2; - sum += q1*q2; - dee[1] = btRecip(Z22 - sum); - /* done factorizing 2 x 2 block */ - ell[nskip1] = Z21; - } - /* compute the (less than 2) rows at the bottom */ - switch (n-i) { - case 0: - break; - - case 1: - btSolveL1_1 (A,A+i*nskip1,i,nskip1); - /* scale the elements in a 1 x i block at A(i,0), and also */ - /* compute Z = the outer product matrix that we'll need. */ - Z11 = 0; - ell = A+i*nskip1; - dee = d; - for (j=i-6; j >= 0; j -= 6) { - p1 = ell[0]; - dd = dee[0]; - q1 = p1*dd; - ell[0] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[1]; - dd = dee[1]; - q1 = p1*dd; - ell[1] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[2]; - dd = dee[2]; - q1 = p1*dd; - ell[2] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[3]; - dd = dee[3]; - q1 = p1*dd; - ell[3] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[4]; - dd = dee[4]; - q1 = p1*dd; - ell[4] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[5]; - dd = dee[5]; - q1 = p1*dd; - ell[5] = q1; - m11 = p1*q1; - Z11 += m11; - ell += 6; - dee += 6; - } - /* compute left-over iterations */ - j += 6; - for (; j > 0; j--) { - p1 = ell[0]; - dd = dee[0]; - q1 = p1*dd; - ell[0] = q1; - m11 = p1*q1; - Z11 += m11; - ell++; - dee++; - } - /* solve for diagonal 1 x 1 block at A(i,i) */ - Z11 = ell[0] - Z11; - dee = d + i; - /* factorize 1 x 1 block Z,dee */ - /* factorize row 1 */ - dee[0] = btRecip(Z11); - /* done factorizing 1 x 1 block */ - break; - - //default: *((char*)0)=0; /* this should never happen! */ - } + for (i = 0; i <= n - 2; i += 2) + { + /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */ + btSolveL1_2(A, A + i * nskip1, i, nskip1); + /* scale the elements in a 2 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + Z11 = 0; + Z21 = 0; + Z22 = 0; + ell = A + i * nskip1; + dee = d; + for (j = i - 6; j >= 0; j -= 6) + { + p1 = ell[0]; + p2 = ell[nskip1]; + dd = dee[0]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[0] = q1; + ell[nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[1]; + p2 = ell[1 + nskip1]; + dd = dee[1]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[1] = q1; + ell[1 + nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[2]; + p2 = ell[2 + nskip1]; + dd = dee[2]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[2] = q1; + ell[2 + nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[3]; + p2 = ell[3 + nskip1]; + dd = dee[3]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[3] = q1; + ell[3 + nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[4]; + p2 = ell[4 + nskip1]; + dd = dee[4]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[4] = q1; + ell[4 + nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[5]; + p2 = ell[5 + nskip1]; + dd = dee[5]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[5] = q1; + ell[5 + nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + ell += 6; + dee += 6; + } + /* compute left-over iterations */ + j += 6; + for (; j > 0; j--) + { + p1 = ell[0]; + p2 = ell[nskip1]; + dd = dee[0]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[0] = q1; + ell[nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + ell++; + dee++; + } + /* solve for diagonal 2 x 2 block at A(i,i) */ + Z11 = ell[0] - Z11; + Z21 = ell[nskip1] - Z21; + Z22 = ell[1 + nskip1] - Z22; + dee = d + i; + /* factorize 2 x 2 block Z,dee */ + /* factorize row 1 */ + dee[0] = btRecip(Z11); + /* factorize row 2 */ + sum = 0; + q1 = Z21; + q2 = q1 * dee[0]; + Z21 = q2; + sum += q1 * q2; + dee[1] = btRecip(Z22 - sum); + /* done factorizing 2 x 2 block */ + ell[nskip1] = Z21; + } + /* compute the (less than 2) rows at the bottom */ + switch (n - i) + { + case 0: + break; + + case 1: + btSolveL1_1(A, A + i * nskip1, i, nskip1); + /* scale the elements in a 1 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + Z11 = 0; + ell = A + i * nskip1; + dee = d; + for (j = i - 6; j >= 0; j -= 6) + { + p1 = ell[0]; + dd = dee[0]; + q1 = p1 * dd; + ell[0] = q1; + m11 = p1 * q1; + Z11 += m11; + p1 = ell[1]; + dd = dee[1]; + q1 = p1 * dd; + ell[1] = q1; + m11 = p1 * q1; + Z11 += m11; + p1 = ell[2]; + dd = dee[2]; + q1 = p1 * dd; + ell[2] = q1; + m11 = p1 * q1; + Z11 += m11; + p1 = ell[3]; + dd = dee[3]; + q1 = p1 * dd; + ell[3] = q1; + m11 = p1 * q1; + Z11 += m11; + p1 = ell[4]; + dd = dee[4]; + q1 = p1 * dd; + ell[4] = q1; + m11 = p1 * q1; + Z11 += m11; + p1 = ell[5]; + dd = dee[5]; + q1 = p1 * dd; + ell[5] = q1; + m11 = p1 * q1; + Z11 += m11; + ell += 6; + dee += 6; + } + /* compute left-over iterations */ + j += 6; + for (; j > 0; j--) + { + p1 = ell[0]; + dd = dee[0]; + q1 = p1 * dd; + ell[0] = q1; + m11 = p1 * q1; + Z11 += m11; + ell++; + dee++; + } + /* solve for diagonal 1 x 1 block at A(i,i) */ + Z11 = ell[0] - Z11; + dee = d + i; + /* factorize 1 x 1 block Z,dee */ + /* factorize row 1 */ + dee[0] = btRecip(Z11); + /* done factorizing 1 x 1 block */ + break; + + //default: *((char*)0)=0; /* this should never happen! */ + } } /* solve L*X=B, with B containing 1 right hand sides. @@ -523,289 +530,295 @@ void btFactorLDLT (btScalar *A, btScalar *d, int n, int nskip1) * if this is in the factorizer source file, n must be a multiple of 4. */ -void btSolveL1 (const btScalar *L, btScalar *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - btScalar Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex; - const btScalar *ell; - int lskip2,lskip3,i,j; - /* compute lskip values */ - lskip2 = 2*lskip1; - lskip3 = 3*lskip1; - /* compute all 4 x 1 blocks of X */ - for (i=0; i <= n-4; i+=4) { - /* compute all 4 x 1 block of X, from rows i..i+4-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z21=0; - Z31=0; - Z41=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-12; j >= 0; j -= 12) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[lskip1]; - p3=ell[lskip2]; - p4=ell[lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[1]; - q1=ex[1]; - p2=ell[1+lskip1]; - p3=ell[1+lskip2]; - p4=ell[1+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[2]; - q1=ex[2]; - p2=ell[2+lskip1]; - p3=ell[2+lskip2]; - p4=ell[2+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[3]; - q1=ex[3]; - p2=ell[3+lskip1]; - p3=ell[3+lskip2]; - p4=ell[3+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[4]; - q1=ex[4]; - p2=ell[4+lskip1]; - p3=ell[4+lskip2]; - p4=ell[4+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[5]; - q1=ex[5]; - p2=ell[5+lskip1]; - p3=ell[5+lskip2]; - p4=ell[5+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[6]; - q1=ex[6]; - p2=ell[6+lskip1]; - p3=ell[6+lskip2]; - p4=ell[6+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[7]; - q1=ex[7]; - p2=ell[7+lskip1]; - p3=ell[7+lskip2]; - p4=ell[7+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[8]; - q1=ex[8]; - p2=ell[8+lskip1]; - p3=ell[8+lskip2]; - p4=ell[8+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[9]; - q1=ex[9]; - p2=ell[9+lskip1]; - p3=ell[9+lskip2]; - p4=ell[9+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[10]; - q1=ex[10]; - p2=ell[10+lskip1]; - p3=ell[10+lskip2]; - p4=ell[10+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[11]; - q1=ex[11]; - p2=ell[11+lskip1]; - p3=ell[11+lskip2]; - p4=ell[11+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* advance pointers */ - ell += 12; - ex += 12; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 12; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[lskip1]; - p3=ell[lskip2]; - p4=ell[lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* advance pointers */ - ell += 1; - ex += 1; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - p1 = ell[lskip1]; - Z21 = ex[1] - Z21 - p1*Z11; - ex[1] = Z21; - p1 = ell[lskip2]; - p2 = ell[1+lskip2]; - Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21; - ex[2] = Z31; - p1 = ell[lskip3]; - p2 = ell[1+lskip3]; - p3 = ell[2+lskip3]; - Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; - ex[3] = Z41; - /* end of outer loop */ - } - /* compute rows at end that are not a multiple of block size */ - for (; i < n; i++) { - /* compute all 1 x 1 block of X, from rows i..i+1-1 */ - /* set the Z matrix to 0 */ - Z11=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-12; j >= 0; j -= 12) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[1]; - q1=ex[1]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[2]; - q1=ex[2]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[3]; - q1=ex[3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[4]; - q1=ex[4]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[5]; - q1=ex[5]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[6]; - q1=ex[6]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[7]; - q1=ex[7]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[8]; - q1=ex[8]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[9]; - q1=ex[9]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[10]; - q1=ex[10]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[11]; - q1=ex[11]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* advance pointers */ - ell += 12; - ex += 12; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 12; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* advance pointers */ - ell += 1; - ex += 1; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - } +void btSolveL1(const btScalar *L, btScalar *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + btScalar Z11, Z21, Z31, Z41, p1, q1, p2, p3, p4, *ex; + const btScalar *ell; + int lskip2, lskip3, i, j; + /* compute lskip values */ + lskip2 = 2 * lskip1; + lskip3 = 3 * lskip1; + /* compute all 4 x 1 blocks of X */ + for (i = 0; i <= n - 4; i += 4) + { + /* compute all 4 x 1 block of X, from rows i..i+4-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + Z21 = 0; + Z31 = 0; + Z41 = 0; + ell = L + i * lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 12; j >= 0; j -= 12) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + p2 = ell[lskip1]; + p3 = ell[lskip2]; + p4 = ell[lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[1]; + q1 = ex[1]; + p2 = ell[1 + lskip1]; + p3 = ell[1 + lskip2]; + p4 = ell[1 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[2]; + q1 = ex[2]; + p2 = ell[2 + lskip1]; + p3 = ell[2 + lskip2]; + p4 = ell[2 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[3]; + q1 = ex[3]; + p2 = ell[3 + lskip1]; + p3 = ell[3 + lskip2]; + p4 = ell[3 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[4]; + q1 = ex[4]; + p2 = ell[4 + lskip1]; + p3 = ell[4 + lskip2]; + p4 = ell[4 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[5]; + q1 = ex[5]; + p2 = ell[5 + lskip1]; + p3 = ell[5 + lskip2]; + p4 = ell[5 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[6]; + q1 = ex[6]; + p2 = ell[6 + lskip1]; + p3 = ell[6 + lskip2]; + p4 = ell[6 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[7]; + q1 = ex[7]; + p2 = ell[7 + lskip1]; + p3 = ell[7 + lskip2]; + p4 = ell[7 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[8]; + q1 = ex[8]; + p2 = ell[8 + lskip1]; + p3 = ell[8 + lskip2]; + p4 = ell[8 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[9]; + q1 = ex[9]; + p2 = ell[9 + lskip1]; + p3 = ell[9 + lskip2]; + p4 = ell[9 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[10]; + q1 = ex[10]; + p2 = ell[10 + lskip1]; + p3 = ell[10 + lskip2]; + p4 = ell[10 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[11]; + q1 = ex[11]; + p2 = ell[11 + lskip1]; + p3 = ell[11 + lskip2]; + p4 = ell[11 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* advance pointers */ + ell += 12; + ex += 12; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 12; + for (; j > 0; j--) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + p2 = ell[lskip1]; + p3 = ell[lskip2]; + p4 = ell[lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1 * Z11; + ex[1] = Z21; + p1 = ell[lskip2]; + p2 = ell[1 + lskip2]; + Z31 = ex[2] - Z31 - p1 * Z11 - p2 * Z21; + ex[2] = Z31; + p1 = ell[lskip3]; + p2 = ell[1 + lskip3]; + p3 = ell[2 + lskip3]; + Z41 = ex[3] - Z41 - p1 * Z11 - p2 * Z21 - p3 * Z31; + ex[3] = Z41; + /* end of outer loop */ + } + /* compute rows at end that are not a multiple of block size */ + for (; i < n; i++) + { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + ell = L + i * lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 12; j >= 0; j -= 12) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[1]; + q1 = ex[1]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[2]; + q1 = ex[2]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[3]; + q1 = ex[3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[4]; + q1 = ex[4]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[5]; + q1 = ex[5]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[6]; + q1 = ex[6]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[7]; + q1 = ex[7]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[8]; + q1 = ex[8]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[9]; + q1 = ex[9]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[10]; + q1 = ex[10]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[11]; + q1 = ex[11]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* advance pointers */ + ell += 12; + ex += 12; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 12; + for (; j > 0; j--) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + } } /* solve L^T * x=b, with b containing 1 right hand side. @@ -816,214 +829,217 @@ void btSolveL1 (const btScalar *L, btScalar *B, int n, int lskip1) * this processes blocks of 4. */ -void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex; - const btScalar *ell; - int lskip2,i,j; -// int lskip3; - /* special handling for L and B because we're solving L1 *transpose* */ - L = L + (n-1)*(lskip1+1); - B = B + n-1; - lskip1 = -lskip1; - /* compute lskip values */ - lskip2 = 2*lskip1; - //lskip3 = 3*lskip1; - /* compute all 4 x 1 blocks of X */ - for (i=0; i <= n-4; i+=4) { - /* compute all 4 x 1 block of X, from rows i..i+4-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z21=0; - Z31=0; - Z41=0; - ell = L - i; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-4; j >= 0; j -= 4) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* load p and q values */ - p1=ell[0]; - q1=ex[-1]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* load p and q values */ - p1=ell[0]; - q1=ex[-2]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* load p and q values */ - p1=ell[0]; - q1=ex[-3]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - ex -= 4; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 4; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - ex -= 1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - p1 = ell[-1]; - Z21 = ex[-1] - Z21 - p1*Z11; - ex[-1] = Z21; - p1 = ell[-2]; - p2 = ell[-2+lskip1]; - Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21; - ex[-2] = Z31; - p1 = ell[-3]; - p2 = ell[-3+lskip1]; - p3 = ell[-3+lskip2]; - Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; - ex[-3] = Z41; - /* end of outer loop */ - } - /* compute rows at end that are not a multiple of block size */ - for (; i < n; i++) { - /* compute all 1 x 1 block of X, from rows i..i+1-1 */ - /* set the Z matrix to 0 */ - Z11=0; - ell = L - i; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-4; j >= 0; j -= 4) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - Z11 += m11; - /* load p and q values */ - p1=ell[0]; - q1=ex[-1]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - Z11 += m11; - /* load p and q values */ - p1=ell[0]; - q1=ex[-2]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - Z11 += m11; - /* load p and q values */ - p1=ell[0]; - q1=ex[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - ex -= 4; - Z11 += m11; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 4; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - ex -= 1; - Z11 += m11; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - } -} - - - -void btVectorScale (btScalar *a, const btScalar *d, int n) +void btSolveL1T(const btScalar *L, btScalar *B, int n, int lskip1) { - btAssert (a && d && n >= 0); - for (int i=0; i= 0; j -= 4) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + p2 = ell[-1]; + p3 = ell[-2]; + p4 = ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-1]; + p2 = ell[-1]; + p3 = ell[-2]; + p4 = ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-2]; + p2 = ell[-1]; + p3 = ell[-2]; + p4 = ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-3]; + p2 = ell[-1]; + p3 = ell[-2]; + p4 = ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + ex -= 4; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 4; + for (; j > 0; j--) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + p2 = ell[-1]; + p3 = ell[-2]; + p4 = ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + ex -= 1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[-1]; + Z21 = ex[-1] - Z21 - p1 * Z11; + ex[-1] = Z21; + p1 = ell[-2]; + p2 = ell[-2 + lskip1]; + Z31 = ex[-2] - Z31 - p1 * Z11 - p2 * Z21; + ex[-2] = Z31; + p1 = ell[-3]; + p2 = ell[-3 + lskip1]; + p3 = ell[-3 + lskip2]; + Z41 = ex[-3] - Z41 - p1 * Z11 - p2 * Z21 - p3 * Z31; + ex[-3] = Z41; + /* end of outer loop */ + } + /* compute rows at end that are not a multiple of block size */ + for (; i < n; i++) + { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + ell = L - i; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 4; j >= 0; j -= 4) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-1]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-2]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + ex -= 4; + Z11 += m11; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 4; + for (; j > 0; j--) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + ex -= 1; + Z11 += m11; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + } } -void btSolveLDLT (const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip) +void btVectorScale(btScalar *a, const btScalar *d, int n) { - btAssert (L && d && b && n > 0 && nskip >= n); - btSolveL1 (L,b,n,nskip); - btVectorScale (b,d,n); - btSolveL1T (L,b,n,nskip); + btAssert(a && d && n >= 0); + for (int i = 0; i < n; i++) + { + a[i] *= d[i]; + } } - +void btSolveLDLT(const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip) +{ + btAssert(L && d && b && n > 0 && nskip >= n); + btSolveL1(L, b, n, nskip); + btVectorScale(b, d, n); + btSolveL1T(L, b, n, nskip); +} //*************************************************************************** @@ -1033,125 +1049,130 @@ void btSolveLDLT (const btScalar *L, const btScalar *d, btScalar *b, int n, int // rows will be swapped by exchanging row pointers. otherwise the data will // be copied. -static void btSwapRowsAndCols (BTATYPE A, int n, int i1, int i2, int nskip, - int do_fast_row_swaps) +static void btSwapRowsAndCols(BTATYPE A, int n, int i1, int i2, int nskip, + int do_fast_row_swaps) { - btAssert (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && - nskip >= n && i1 < i2); + btAssert(A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && + nskip >= n && i1 < i2); -# ifdef BTROWPTRS - btScalar *A_i1 = A[i1]; - btScalar *A_i2 = A[i2]; - for (int i=i1+1; i0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2); - if (i1==i2) return; - - btSwapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps); - - tmpr = x[i1]; - x[i1] = x[i2]; - x[i2] = tmpr; - - tmpr = b[i1]; - b[i1] = b[i2]; - b[i2] = tmpr; - - tmpr = w[i1]; - w[i1] = w[i2]; - w[i2] = tmpr; - - tmpr = lo[i1]; - lo[i1] = lo[i2]; - lo[i2] = tmpr; + btScalar tmpr; + int tmpi; + bool tmpb; + btAssert(n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2); + if (i1 == i2) return; - tmpr = hi[i1]; - hi[i1] = hi[i2]; - hi[i2] = tmpr; + btSwapRowsAndCols(A, n, i1, i2, nskip, do_fast_row_swaps); - tmpi = p[i1]; - p[i1] = p[i2]; - p[i2] = tmpi; + tmpr = x[i1]; + x[i1] = x[i2]; + x[i2] = tmpr; - tmpb = state[i1]; - state[i1] = state[i2]; - state[i2] = tmpb; + tmpr = b[i1]; + b[i1] = b[i2]; + b[i2] = tmpr; - if (findex) { - tmpi = findex[i1]; - findex[i1] = findex[i2]; - findex[i2] = tmpi; - } + tmpr = w[i1]; + w[i1] = w[i2]; + w[i2] = tmpr; + + tmpr = lo[i1]; + lo[i1] = lo[i2]; + lo[i2] = tmpr; + + tmpr = hi[i1]; + hi[i1] = hi[i2]; + hi[i2] = tmpr; + + tmpi = p[i1]; + p[i1] = p[i2]; + p[i2] = tmpi; + + tmpb = state[i1]; + state[i1] = state[i2]; + state[i2] = tmpb; + + if (findex) + { + tmpi = findex[i1]; + findex[i1] = findex[i2]; + findex[i2] = tmpi; + } } - - - //*************************************************************************** // btLCP manipulator object. this represents an n*n LCP problem. // @@ -1186,79 +1207,88 @@ static void btSwapProblem (BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btS #ifdef btLCP_FAST -struct btLCP +struct btLCP { const int m_n; const int m_nskip; int m_nub; - int m_nC, m_nN; // size of each index set - BTATYPE const m_A; // A rows - btScalar *const m_x, * const m_b, *const m_w, *const m_lo,* const m_hi; // permuted LCP problem data - btScalar *const m_L, *const m_d; // L*D*L' factorization of set C + int m_nC, m_nN; // size of each index set + BTATYPE const m_A; // A rows + btScalar *const m_x, *const m_b, *const m_w, *const m_lo, *const m_hi; // permuted LCP problem data + btScalar *const m_L, *const m_d; // L*D*L' factorization of set C btScalar *const m_Dell, *const m_ell, *const m_tmp; bool *const m_state; int *const m_findex, *const m_p, *const m_C; - btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, - btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d, - btScalar *_Dell, btScalar *_ell, btScalar *_tmp, - bool *_state, int *_findex, int *p, int *c, btScalar **Arows); + btLCP(int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, + btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d, + btScalar *_Dell, btScalar *_ell, btScalar *_tmp, + bool *_state, int *_findex, int *p, int *c, btScalar **Arows); int getNub() const { return m_nub; } - void transfer_i_to_C (int i); - void transfer_i_to_N (int i) { m_nN++; } // because we can assume C and N span 1:i-1 - void transfer_i_from_N_to_C (int i); - void transfer_i_from_C_to_N (int i, btAlignedObjectArray& scratch); + void transfer_i_to_C(int i); + void transfer_i_to_N(int i) { m_nN++; } // because we can assume C and N span 1:i-1 + void transfer_i_from_N_to_C(int i); + void transfer_i_from_C_to_N(int i, btAlignedObjectArray &scratch); int numC() const { return m_nC; } int numN() const { return m_nN; } - int indexC (int i) const { return i; } - int indexN (int i) const { return i+m_nC; } - btScalar Aii (int i) const { return BTAROW(i)[i]; } - btScalar AiC_times_qC (int i, btScalar *q) const { return btLargeDot (BTAROW(i), q, m_nC); } - btScalar AiN_times_qN (int i, btScalar *q) const { return btLargeDot (BTAROW(i)+m_nC, q+m_nC, m_nN); } - void pN_equals_ANC_times_qC (btScalar *p, btScalar *q); - void pN_plusequals_ANi (btScalar *p, int i, int sign=1); - void pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q); - void pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q); - void solve1 (btScalar *a, int i, int dir=1, int only_transfer=0); + int indexC(int i) const { return i; } + int indexN(int i) const { return i + m_nC; } + btScalar Aii(int i) const { return BTAROW(i)[i]; } + btScalar AiC_times_qC(int i, btScalar *q) const { return btLargeDot(BTAROW(i), q, m_nC); } + btScalar AiN_times_qN(int i, btScalar *q) const { return btLargeDot(BTAROW(i) + m_nC, q + m_nC, m_nN); } + void pN_equals_ANC_times_qC(btScalar *p, btScalar *q); + void pN_plusequals_ANi(btScalar *p, int i, int sign = 1); + void pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q); + void pN_plusequals_s_times_qN(btScalar *p, btScalar s, btScalar *q); + void solve1(btScalar *a, int i, int dir = 1, int only_transfer = 0); void unpermute(); }; - -btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, - btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d, - btScalar *_Dell, btScalar *_ell, btScalar *_tmp, - bool *_state, int *_findex, int *p, int *c, btScalar **Arows): - m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0), -# ifdef BTROWPTRS - m_A(Arows), +btLCP::btLCP(int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, + btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d, + btScalar *_Dell, btScalar *_ell, btScalar *_tmp, + bool *_state, int *_findex, int *p, int *c, btScalar **Arows) : m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0), +#ifdef BTROWPTRS + m_A(Arows), #else - m_A(_Adata), + m_A(_Adata), #endif - m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi), - m_L(l), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp), - m_state(_state), m_findex(_findex), m_p(p), m_C(c) + m_x(_x), + m_b(_b), + m_w(_w), + m_lo(_lo), + m_hi(_hi), + m_L(l), + m_d(_d), + m_Dell(_Dell), + m_ell(_ell), + m_tmp(_tmp), + m_state(_state), + m_findex(_findex), + m_p(p), + m_C(c) { - { - btSetZero (m_x,m_n); - } + { + btSetZero(m_x, m_n); + } - { -# ifdef BTROWPTRS - // make matrix row pointers - btScalar *aptr = _Adata; - BTATYPE A = m_A; - const int n = m_n, nskip = m_nskip; - for (int k=0; k nub { const int n = m_n; @@ -1277,63 +1307,69 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc } */ - // permute the problem so that *all* the unbounded variables are at the - // start, i.e. look for unbounded variables not included in `nub'. we can - // potentially push up `nub' this way and get a bigger initial factorization. - // note that when we swap rows/cols here we must not just swap row pointers, - // as the initial factorization relies on the data being all in one chunk. - // variables that have findex >= 0 are *not* considered to be unbounded even - // if lo=-inf and hi=inf - this is because these limits may change during the - // solution process. + // permute the problem so that *all* the unbounded variables are at the + // start, i.e. look for unbounded variables not included in `nub'. we can + // potentially push up `nub' this way and get a bigger initial factorization. + // note that when we swap rows/cols here we must not just swap row pointers, + // as the initial factorization relies on the data being all in one chunk. + // variables that have findex >= 0 are *not* considered to be unbounded even + // if lo=-inf and hi=inf - this is because these limits may change during the + // solution process. - { - int *findex = m_findex; - btScalar *lo = m_lo, *hi = m_hi; - const int n = m_n; - for (int k = m_nub; k= 0) continue; - if (lo[k]==-BT_INFINITY && hi[k]==BT_INFINITY) { - btSwapProblem (m_A,m_x,m_b,m_w,lo,hi,m_p,m_state,findex,n,m_nub,k,m_nskip,0); - m_nub++; - } - } - } + { + int *findex = m_findex; + btScalar *lo = m_lo, *hi = m_hi; + const int n = m_n; + for (int k = m_nub; k < n; ++k) + { + if (findex && findex[k] >= 0) continue; + if (lo[k] == -BT_INFINITY && hi[k] == BT_INFINITY) + { + btSwapProblem(m_A, m_x, m_b, m_w, lo, hi, m_p, m_state, findex, n, m_nub, k, m_nskip, 0); + m_nub++; + } + } + } - // if there are unbounded variables at the start, factorize A up to that - // point and solve for x. this puts all indexes 0..nub-1 into C. - if (m_nub > 0) { - const int nub = m_nub; - { - btScalar *Lrow = m_L; - const int nskip = m_nskip; - for (int j=0; j 0) + { + const int nub = m_nub; + { + btScalar *Lrow = m_L; + const int nskip = m_nskip; + for (int j = 0; j < nub; Lrow += nskip, ++j) memcpy(Lrow, BTAROW(j), (j + 1) * sizeof(btScalar)); + } + btFactorLDLT(m_L, m_d, nub, m_nskip); + memcpy(m_x, m_b, nub * sizeof(btScalar)); + btSolveLDLT(m_L, m_d, m_x, nub, m_nskip); + btSetZero(m_w, nub); + { + int *C = m_C; + for (int k = 0; k < nub; ++k) C[k] = k; + } + m_nC = nub; + } - // permute the indexes > nub such that all findex variables are at the end - if (m_findex) { - const int nub = m_nub; - int *findex = m_findex; - int num_at_end = 0; - for (int k=m_n-1; k >= nub; k--) { - if (findex[k] >= 0) { - btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,findex,m_n,k,m_n-1-num_at_end,m_nskip,1); - num_at_end++; - } - } - } + // permute the indexes > nub such that all findex variables are at the end + if (m_findex) + { + const int nub = m_nub; + int *findex = m_findex; + int num_at_end = 0; + for (int k = m_n - 1; k >= nub; k--) + { + if (findex[k] >= 0) + { + btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, findex, m_n, k, m_n - 1 - num_at_end, m_nskip, 1); + num_at_end++; + } + } + } - // print info about indexes - /* + // print info about indexes + /* { const int n = m_n; const int nub = m_nub; @@ -1347,734 +1383,776 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc */ } - -void btLCP::transfer_i_to_C (int i) +void btLCP::transfer_i_to_C(int i) { - { - if (m_nC > 0) { - // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C)) - { - const int nC = m_nC; - btScalar *const Ltgt = m_L + nC*m_nskip, *ell = m_ell; - for (int j=0; j 0) + { + // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C)) + { + const int nC = m_nC; + btScalar *const Ltgt = m_L + nC * m_nskip, *ell = m_ell; + for (int j = 0; j < nC; ++j) Ltgt[j] = ell[j]; + } + const int nC = m_nC; + m_d[nC] = btRecip(BTAROW(i)[i] - btLargeDot(m_ell, m_Dell, nC)); + } + else + { + m_d[0] = btRecip(BTAROW(i)[i]); + } - btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1); - - const int nC = m_nC; - m_C[nC] = nC; - m_nC = nC + 1; // nC value is outdated after this line - } + btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, m_nC, i, m_nskip, 1); + const int nC = m_nC; + m_C[nC] = nC; + m_nC = nC + 1; // nC value is outdated after this line + } } - -void btLCP::transfer_i_from_N_to_C (int i) +void btLCP::transfer_i_from_N_to_C(int i) { - { - if (m_nC > 0) { - { - btScalar *const aptr = BTAROW(i); - btScalar *Dell = m_Dell; - const int *C = m_C; -# ifdef BTNUB_OPTIMIZATIONS - // if nub>0, initial part of aptr unpermuted - const int nub = m_nub; - int j=0; - for ( ; j 0) + { + { + btScalar *const aptr = BTAROW(i); + btScalar *Dell = m_Dell; + const int *C = m_C; +#ifdef BTNUB_OPTIMIZATIONS + // if nub>0, initial part of aptr unpermuted + const int nub = m_nub; + int j = 0; + for (; j < nub; ++j) Dell[j] = aptr[j]; + const int nC = m_nC; + for (; j < nC; ++j) Dell[j] = aptr[C[j]]; +#else + const int nC = m_nC; + for (int j = 0; j < nC; ++j) Dell[j] = aptr[C[j]]; +#endif + } + btSolveL1(m_L, m_Dell, m_nC, m_nskip); + { + const int nC = m_nC; + btScalar *const Ltgt = m_L + nC * m_nskip; + btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d; + for (int j = 0; j < nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j]; + } + const int nC = m_nC; + m_d[nC] = btRecip(BTAROW(i)[i] - btLargeDot(m_ell, m_Dell, nC)); + } + else + { + m_d[0] = btRecip(BTAROW(i)[i]); + } - btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1); + btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, m_nC, i, m_nskip, 1); - const int nC = m_nC; - m_C[nC] = nC; - m_nN--; - m_nC = nC + 1; // nC value is outdated after this line - } - - // @@@ TO DO LATER - // if we just finish here then we'll go back and re-solve for - // delta_x. but actually we can be more efficient and incrementally - // update delta_x here. but if we do this, we wont have ell and Dell - // to use in updating the factorization later. + const int nC = m_nC; + m_C[nC] = nC; + m_nN--; + m_nC = nC + 1; // nC value is outdated after this line + } + // @@@ TO DO LATER + // if we just finish here then we'll go back and re-solve for + // delta_x. but actually we can be more efficient and incrementally + // update delta_x here. but if we do this, we wont have ell and Dell + // to use in updating the factorization later. } -void btRemoveRowCol (btScalar *A, int n, int nskip, int r) +void btRemoveRowCol(btScalar *A, int n, int nskip, int r) { - btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n); - if (r >= n-1) return; - if (r > 0) { - { - const size_t move_size = (n-r-1)*sizeof(btScalar); - btScalar *Adst = A + r; - for (int i=0; i 0 && nskip >= n && r >= 0 && r < n); + if (r >= n - 1) return; + if (r > 0) + { + { + const size_t move_size = (n - r - 1) * sizeof(btScalar); + btScalar *Adst = A + r; + for (int i = 0; i < r; Adst += nskip, ++i) + { + btScalar *Asrc = Adst + 1; + memmove(Adst, Asrc, move_size); + } + } + { + const size_t cpy_size = r * sizeof(btScalar); + btScalar *Adst = A + r * nskip; + for (int i = r; i < (n - 1); ++i) + { + btScalar *Asrc = Adst + nskip; + memcpy(Adst, Asrc, cpy_size); + Adst = Asrc; + } + } + } + { + const size_t cpy_size = (n - r - 1) * sizeof(btScalar); + btScalar *Adst = A + r * (nskip + 1); + for (int i = r; i < (n - 1); ++i) + { + btScalar *Asrc = Adst + (nskip + 1); + memcpy(Adst, Asrc, cpy_size); + Adst = Asrc - 1; + } + } } - - - -void btLDLTAddTL (btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray& scratch) +void btLDLTAddTL(btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray &scratch) { - btAssert (L && d && a && n > 0 && nskip >= n); + btAssert(L && d && a && n > 0 && nskip >= n); - if (n < 2) return; - scratch.resize(2*nskip); - btScalar *W1 = &scratch[0]; - - btScalar *W2 = W1 + nskip; + if (n < 2) return; + scratch.resize(2 * nskip); + btScalar *W1 = &scratch[0]; - W1[0] = btScalar(0.0); - W2[0] = btScalar(0.0); - for (int j=1; j j) ? _BTGETA(i,j) : _BTGETA(j,i)) +#define BTGETA(i, j) ((i > j) ? _BTGETA(i, j) : _BTGETA(j, i)) inline size_t btEstimateLDLTAddTLTmpbufSize(int nskip) { - return nskip * 2 * sizeof(btScalar); + return nskip * 2 * sizeof(btScalar); } - -void btLDLTRemove (btScalar **A, const int *p, btScalar *L, btScalar *d, - int n1, int n2, int r, int nskip, btAlignedObjectArray& scratch) +void btLDLTRemove(btScalar **A, const int *p, btScalar *L, btScalar *d, + int n1, int n2, int r, int nskip, btAlignedObjectArray &scratch) { - btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 && - n1 >= n2 && nskip >= n1); - #ifdef BT_DEBUG - for (int i=0; i 0 && n2 > 0 && r >= 0 && r < n2 && + n1 >= n2 && nskip >= n1); +#ifdef BT_DEBUG + for (int i = 0; i < n2; ++i) btAssert(p[i] >= 0 && p[i] < n1); - #endif +#endif - if (r==n2-1) { - return; // deleting last row/col is easy - } - else { - size_t LDLTAddTL_size = btEstimateLDLTAddTLTmpbufSize(nskip); - btAssert(LDLTAddTL_size % sizeof(btScalar) == 0); - scratch.resize(nskip * 2+n2); - btScalar *tmp = &scratch[0]; - if (r==0) { - btScalar *a = (btScalar *)((char *)tmp + LDLTAddTL_size); - const int p_0 = p[0]; - for (int i=0; i& scratch) +void btLCP::transfer_i_from_C_to_N(int i, btAlignedObjectArray &scratch) { - { - int *C = m_C; - // remove a row/column from the factorization, and adjust the - // indexes (black magic!) - int last_idx = -1; - const int nC = m_nC; - int j = 0; - for ( ; j 0) { - const int nN = m_nN; - for (int j=0; j 0) + { + const int nN = m_nN; + for (int j = 0; j < nN; ++j) ptgt[j] += aptr[j]; + } + else + { + const int nN = m_nN; + for (int j = 0; j < nN; ++j) ptgt[j] -= aptr[j]; + } } -void btLCP::pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q) +void btLCP::pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q) { - const int nC = m_nC; - for (int i=0; i 0) { - { - btScalar *Dell = m_Dell; - int *C = m_C; - btScalar *aptr = BTAROW(i); -# ifdef BTNUB_OPTIMIZATIONS - // if nub>0, initial part of aptr[] is guaranteed unpermuted - const int nub = m_nub; - int j=0; - for ( ; j 0) + { + { + btScalar *Dell = m_Dell; + int *C = m_C; + btScalar *aptr = BTAROW(i); +#ifdef BTNUB_OPTIMIZATIONS + // if nub>0, initial part of aptr[] is guaranteed unpermuted + const int nub = m_nub; + int j = 0; + for (; j < nub; ++j) Dell[j] = aptr[j]; + const int nC = m_nC; + for (; j < nC; ++j) Dell[j] = aptr[C[j]]; +#else + const int nC = m_nC; + for (int j = 0; j < nC; ++j) Dell[j] = aptr[C[j]]; +#endif + } + btSolveL1(m_L, m_Dell, m_nC, m_nskip); + { + btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d; + const int nC = m_nC; + for (int j = 0; j < nC; ++j) ell[j] = Dell[j] * d[j]; + } - if (!only_transfer) { - btScalar *tmp = m_tmp, *ell = m_ell; - { - const int nC = m_nC; - for (int j=0; j 0) { - int *C = m_C; - btScalar *tmp = m_tmp; - const int nC = m_nC; - for (int j=0; j 0) + { + int *C = m_C; + btScalar *tmp = m_tmp; + const int nC = m_nC; + for (int j = 0; j < nC; ++j) a[C[j]] = -tmp[j]; + } + else + { + int *C = m_C; + btScalar *tmp = m_tmp; + const int nC = m_nC; + for (int j = 0; j < nC; ++j) a[C[j]] = tmp[j]; + } + } + } } - void btLCP::unpermute() { - // now we have to un-permute x and w - { - memcpy (m_tmp,m_x,m_n*sizeof(btScalar)); - btScalar *x = m_x, *tmp = m_tmp; - const int *p = m_p; - const int n = m_n; - for (int j=0; j0 && A && x && b && lo && hi && nub >= 0 && nub <= n); - btAssert(outer_w); + // printf("btSolveDantzigLCP n=%d\n",n); + btAssert(n > 0 && A && x && b && lo && hi && nub >= 0 && nub <= n); + btAssert(outer_w); #ifdef BT_DEBUG - { - // check restrictions on lo and hi - for (int k=0; k= 0); - } -# endif - - - // if all the variables are unbounded then we can just factor, solve, - // and return - if (nub >= n) - { - - - int nskip = (n); - btFactorLDLT (A, outer_w, n, nskip); - btSolveLDLT (A, outer_w, b, n, nskip); - memcpy (x, b, n*sizeof(btScalar)); - - return !s_error; - } - - const int nskip = (n); - scratchMem.L.resize(n*nskip); - - scratchMem.d.resize(n); - - btScalar *w = outer_w; - scratchMem.delta_w.resize(n); - scratchMem.delta_x.resize(n); - scratchMem.Dell.resize(n); - scratchMem.ell.resize(n); - scratchMem.Arows.resize(n); - scratchMem.p.resize(n); - scratchMem.C.resize(n); - - // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) - scratchMem.state.resize(n); - - - // create LCP object. note that tmp is set to delta_w to save space, this - // optimization relies on knowledge of how tmp is used, so be careful! - btLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,&scratchMem.L[0],&scratchMem.d[0],&scratchMem.Dell[0],&scratchMem.ell[0],&scratchMem.delta_w[0],&scratchMem.state[0],findex,&scratchMem.p[0],&scratchMem.C[0],&scratchMem.Arows[0]); - int adj_nub = lcp.getNub(); - - // loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the - // LCP conditions then i is added to the appropriate index set. otherwise - // x(i),w(i) is driven either +ve or -ve to force it to the valid region. - // as we drive x(i), x(C) is also adjusted to keep w(C) at zero. - // while driving x(i) we maintain the LCP conditions on the other variables - // 0..i-1. we do this by watching out for other x(i),w(i) values going - // outside the valid region, and then switching them between index sets - // when that happens. - - bool hit_first_friction_index = false; - for (int i=adj_nub; i= 0) { - // un-permute x into delta_w, which is not being used at the moment - for (int j=0; j= 0) { - lcp.transfer_i_to_N (i); - scratchMem.state[i] = false; - } - else if (hi[i]==0 && w[i] <= 0) { - lcp.transfer_i_to_N (i); - scratchMem.state[i] = true; - } - else if (w[i]==0) { - // this is a degenerate case. by the time we get to this test we know - // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve, - // and similarly that hi > 0. this means that the line segment - // corresponding to set C is at least finite in extent, and we are on it. - // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C() - lcp.solve1 (&scratchMem.delta_x[0],i,0,1); - - lcp.transfer_i_to_C (i); - } - else { - // we must push x(i) and w(i) - for (;;) { - int dir; - btScalar dirf; - // find direction to push on x(i) - if (w[i] <= 0) { - dir = 1; - dirf = btScalar(1.0); - } - else { - dir = -1; - dirf = btScalar(-1.0); - } - - // compute: delta_x(C) = -dir*A(C,C)\A(C,i) - lcp.solve1 (&scratchMem.delta_x[0],i,dir); - - // note that delta_x[i] = dirf, but we wont bother to set it - - // compute: delta_w = A*delta_x ... note we only care about - // delta_w(N) and delta_w(i), the rest is ignored - lcp.pN_equals_ANC_times_qC (&scratchMem.delta_w[0],&scratchMem.delta_x[0]); - lcp.pN_plusequals_ANi (&scratchMem.delta_w[0],i,dir); - scratchMem.delta_w[i] = lcp.AiC_times_qC (i,&scratchMem.delta_x[0]) + lcp.Aii(i)*dirf; - - // find largest step we can take (size=s), either to drive x(i),w(i) - // to the valid LCP region or to drive an already-valid variable - // outside the valid region. - - int cmd = 1; // index switching command - int si = 0; // si = index to switch if cmd>3 - btScalar s = -w[i]/scratchMem.delta_w[i]; - if (dir > 0) { - if (hi[i] < BT_INFINITY) { - btScalar s2 = (hi[i]-x[i])*dirf; // was (hi[i]-x[i])/dirf // step to x(i)=hi(i) - if (s2 < s) { - s = s2; - cmd = 3; - } - } - } - else { - if (lo[i] > -BT_INFINITY) { - btScalar s2 = (lo[i]-x[i])*dirf; // was (lo[i]-x[i])/dirf // step to x(i)=lo(i) - if (s2 < s) { - s = s2; - cmd = 2; - } - } - } - - { - const int numN = lcp.numN(); - for (int k=0; k < numN; ++k) { - const int indexN_k = lcp.indexN(k); - if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0) { - // don't bother checking if lo=hi=0 - if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue; - btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k]; - if (s2 < s) { - s = s2; - cmd = 4; - si = indexN_k; - } - } - } - } - - { - const int numC = lcp.numC(); - for (int k=adj_nub; k < numC; ++k) { - const int indexC_k = lcp.indexC(k); - if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY) { - btScalar s2 = (lo[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k]; - if (s2 < s) { - s = s2; - cmd = 5; - si = indexC_k; - } - } - if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY) { - btScalar s2 = (hi[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k]; - if (s2 < s) { - s = s2; - cmd = 6; - si = indexC_k; - } - } - } - } - - //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C", - // "C->NL","C->NH"}; - //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i); - - // if s <= 0 then we've got a problem. if we just keep going then - // we're going to get stuck in an infinite loop. instead, just cross - // our fingers and exit with the current solution. - if (s <= btScalar(0.0)) - { -// printf("LCP internal error, s <= 0 (s=%.4e)",(double)s); - if (i < n) { - btSetZero (x+i,n-i); - btSetZero (w+i,n-i); - } - s_error = true; - break; - } - - // apply x = x + s * delta_x - lcp.pC_plusequals_s_times_qC (x, s, &scratchMem.delta_x[0]); - x[i] += s * dirf; - - // apply w = w + s * delta_w - lcp.pN_plusequals_s_times_qN (w, s, &scratchMem.delta_w[0]); - w[i] += s * scratchMem.delta_w[i]; - -// void *tmpbuf; - // switch indexes between sets if necessary - switch (cmd) { - case 1: // done - w[i] = 0; - lcp.transfer_i_to_C (i); - break; - case 2: // done - x[i] = lo[i]; - scratchMem.state[i] = false; - lcp.transfer_i_to_N (i); - break; - case 3: // done - x[i] = hi[i]; - scratchMem.state[i] = true; - lcp.transfer_i_to_N (i); - break; - case 4: // keep going - w[si] = 0; - lcp.transfer_i_from_N_to_C (si); - break; - case 5: // keep going - x[si] = lo[si]; - scratchMem.state[si] = false; - lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch); - break; - case 6: // keep going - x[si] = hi[si]; - scratchMem.state[si] = true; - lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch); - break; - } - - if (cmd <= 3) break; - } // for (;;) - } // else - - if (s_error) { - break; - } - } // for (int i=adj_nub; i= 0); + } +#endif - lcp.unpermute(); + // if all the variables are unbounded then we can just factor, solve, + // and return + if (nub >= n) + { + int nskip = (n); + btFactorLDLT(A, outer_w, n, nskip); + btSolveLDLT(A, outer_w, b, n, nskip); + memcpy(x, b, n * sizeof(btScalar)); + return !s_error; + } - return !s_error; + const int nskip = (n); + scratchMem.L.resize(n * nskip); + + scratchMem.d.resize(n); + + btScalar *w = outer_w; + scratchMem.delta_w.resize(n); + scratchMem.delta_x.resize(n); + scratchMem.Dell.resize(n); + scratchMem.ell.resize(n); + scratchMem.Arows.resize(n); + scratchMem.p.resize(n); + scratchMem.C.resize(n); + + // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) + scratchMem.state.resize(n); + + // create LCP object. note that tmp is set to delta_w to save space, this + // optimization relies on knowledge of how tmp is used, so be careful! + btLCP lcp(n, nskip, nub, A, x, b, w, lo, hi, &scratchMem.L[0], &scratchMem.d[0], &scratchMem.Dell[0], &scratchMem.ell[0], &scratchMem.delta_w[0], &scratchMem.state[0], findex, &scratchMem.p[0], &scratchMem.C[0], &scratchMem.Arows[0]); + int adj_nub = lcp.getNub(); + + // loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the + // LCP conditions then i is added to the appropriate index set. otherwise + // x(i),w(i) is driven either +ve or -ve to force it to the valid region. + // as we drive x(i), x(C) is also adjusted to keep w(C) at zero. + // while driving x(i) we maintain the LCP conditions on the other variables + // 0..i-1. we do this by watching out for other x(i),w(i) values going + // outside the valid region, and then switching them between index sets + // when that happens. + + bool hit_first_friction_index = false; + for (int i = adj_nub; i < n; ++i) + { + s_error = false; + // the index i is the driving index and indexes i+1..n-1 are "dont care", + // i.e. when we make changes to the system those x's will be zero and we + // don't care what happens to those w's. in other words, we only consider + // an (i+1)*(i+1) sub-problem of A*x=b+w. + + // if we've hit the first friction index, we have to compute the lo and + // hi values based on the values of x already computed. we have been + // permuting the indexes, so the values stored in the findex vector are + // no longer valid. thus we have to temporarily unpermute the x vector. + // for the purposes of this computation, 0*infinity = 0 ... so if the + // contact constraint's normal force is 0, there should be no tangential + // force applied. + + if (!hit_first_friction_index && findex && findex[i] >= 0) + { + // un-permute x into delta_w, which is not being used at the moment + for (int j = 0; j < n; ++j) scratchMem.delta_w[scratchMem.p[j]] = x[j]; + + // set lo and hi values + for (int k = i; k < n; ++k) + { + btScalar wfk = scratchMem.delta_w[findex[k]]; + if (wfk == 0) + { + hi[k] = 0; + lo[k] = 0; + } + else + { + hi[k] = btFabs(hi[k] * wfk); + lo[k] = -hi[k]; + } + } + hit_first_friction_index = true; + } + + // thus far we have not even been computing the w values for indexes + // greater than i, so compute w[i] now. + w[i] = lcp.AiC_times_qC(i, x) + lcp.AiN_times_qN(i, x) - b[i]; + + // if lo=hi=0 (which can happen for tangential friction when normals are + // 0) then the index will be assigned to set N with some state. however, + // set C's line has zero size, so the index will always remain in set N. + // with the "normal" switching logic, if w changed sign then the index + // would have to switch to set C and then back to set N with an inverted + // state. this is pointless, and also computationally expensive. to + // prevent this from happening, we use the rule that indexes with lo=hi=0 + // will never be checked for set changes. this means that the state for + // these indexes may be incorrect, but that doesn't matter. + + // see if x(i),w(i) is in a valid region + if (lo[i] == 0 && w[i] >= 0) + { + lcp.transfer_i_to_N(i); + scratchMem.state[i] = false; + } + else if (hi[i] == 0 && w[i] <= 0) + { + lcp.transfer_i_to_N(i); + scratchMem.state[i] = true; + } + else if (w[i] == 0) + { + // this is a degenerate case. by the time we get to this test we know + // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve, + // and similarly that hi > 0. this means that the line segment + // corresponding to set C is at least finite in extent, and we are on it. + // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C() + lcp.solve1(&scratchMem.delta_x[0], i, 0, 1); + + lcp.transfer_i_to_C(i); + } + else + { + // we must push x(i) and w(i) + for (;;) + { + int dir; + btScalar dirf; + // find direction to push on x(i) + if (w[i] <= 0) + { + dir = 1; + dirf = btScalar(1.0); + } + else + { + dir = -1; + dirf = btScalar(-1.0); + } + + // compute: delta_x(C) = -dir*A(C,C)\A(C,i) + lcp.solve1(&scratchMem.delta_x[0], i, dir); + + // note that delta_x[i] = dirf, but we wont bother to set it + + // compute: delta_w = A*delta_x ... note we only care about + // delta_w(N) and delta_w(i), the rest is ignored + lcp.pN_equals_ANC_times_qC(&scratchMem.delta_w[0], &scratchMem.delta_x[0]); + lcp.pN_plusequals_ANi(&scratchMem.delta_w[0], i, dir); + scratchMem.delta_w[i] = lcp.AiC_times_qC(i, &scratchMem.delta_x[0]) + lcp.Aii(i) * dirf; + + // find largest step we can take (size=s), either to drive x(i),w(i) + // to the valid LCP region or to drive an already-valid variable + // outside the valid region. + + int cmd = 1; // index switching command + int si = 0; // si = index to switch if cmd>3 + btScalar s = -w[i] / scratchMem.delta_w[i]; + if (dir > 0) + { + if (hi[i] < BT_INFINITY) + { + btScalar s2 = (hi[i] - x[i]) * dirf; // was (hi[i]-x[i])/dirf // step to x(i)=hi(i) + if (s2 < s) + { + s = s2; + cmd = 3; + } + } + } + else + { + if (lo[i] > -BT_INFINITY) + { + btScalar s2 = (lo[i] - x[i]) * dirf; // was (lo[i]-x[i])/dirf // step to x(i)=lo(i) + if (s2 < s) + { + s = s2; + cmd = 2; + } + } + } + + { + const int numN = lcp.numN(); + for (int k = 0; k < numN; ++k) + { + const int indexN_k = lcp.indexN(k); + if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0) + { + // don't bother checking if lo=hi=0 + if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue; + btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k]; + if (s2 < s) + { + s = s2; + cmd = 4; + si = indexN_k; + } + } + } + } + + { + const int numC = lcp.numC(); + for (int k = adj_nub; k < numC; ++k) + { + const int indexC_k = lcp.indexC(k); + if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY) + { + btScalar s2 = (lo[indexC_k] - x[indexC_k]) / scratchMem.delta_x[indexC_k]; + if (s2 < s) + { + s = s2; + cmd = 5; + si = indexC_k; + } + } + if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY) + { + btScalar s2 = (hi[indexC_k] - x[indexC_k]) / scratchMem.delta_x[indexC_k]; + if (s2 < s) + { + s = s2; + cmd = 6; + si = indexC_k; + } + } + } + } + + //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C", + // "C->NL","C->NH"}; + //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i); + + // if s <= 0 then we've got a problem. if we just keep going then + // we're going to get stuck in an infinite loop. instead, just cross + // our fingers and exit with the current solution. + if (s <= btScalar(0.0)) + { + // printf("LCP internal error, s <= 0 (s=%.4e)",(double)s); + if (i < n) + { + btSetZero(x + i, n - i); + btSetZero(w + i, n - i); + } + s_error = true; + break; + } + + // apply x = x + s * delta_x + lcp.pC_plusequals_s_times_qC(x, s, &scratchMem.delta_x[0]); + x[i] += s * dirf; + + // apply w = w + s * delta_w + lcp.pN_plusequals_s_times_qN(w, s, &scratchMem.delta_w[0]); + w[i] += s * scratchMem.delta_w[i]; + + // void *tmpbuf; + // switch indexes between sets if necessary + switch (cmd) + { + case 1: // done + w[i] = 0; + lcp.transfer_i_to_C(i); + break; + case 2: // done + x[i] = lo[i]; + scratchMem.state[i] = false; + lcp.transfer_i_to_N(i); + break; + case 3: // done + x[i] = hi[i]; + scratchMem.state[i] = true; + lcp.transfer_i_to_N(i); + break; + case 4: // keep going + w[si] = 0; + lcp.transfer_i_from_N_to_C(si); + break; + case 5: // keep going + x[si] = lo[si]; + scratchMem.state[si] = false; + lcp.transfer_i_from_C_to_N(si, scratchMem.m_scratch); + break; + case 6: // keep going + x[si] = hi[si]; + scratchMem.state[si] = true; + lcp.transfer_i_from_C_to_N(si, scratchMem.m_scratch); + break; + } + + if (cmd <= 3) break; + } // for (;;) + } // else + + if (s_error) + { + break; + } + } // for (int i=adj_nub; i #include - #include "LinearMath/btScalar.h" #include "LinearMath/btAlignedObjectArray.h" @@ -62,16 +60,14 @@ struct btDantzigScratchMemory btAlignedObjectArray delta_x; btAlignedObjectArray Dell; btAlignedObjectArray ell; - btAlignedObjectArray Arows; + btAlignedObjectArray Arows; btAlignedObjectArray p; btAlignedObjectArray C; btAlignedObjectArray state; }; //return false if solving failed -bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w, - int nub, btScalar *lo, btScalar *hi, int *findex,btDantzigScratchMemory& scratch); +bool btSolveDantzigLCP(int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w, + int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory &scratch); - - -#endif //_BT_LCP_H_ +#endif //_BT_LCP_H_ diff --git a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h index 2a2f2d3d3..1f669751c 100644 --- a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h +++ b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h @@ -20,30 +20,28 @@ subject to the following restrictions: #include "btMLCPSolverInterface.h" #include "btDantzigLCP.h" - class btDantzigSolver : public btMLCPSolverInterface { protected: - btScalar m_acceptableUpperLimitSolution; - btAlignedObjectArray m_tempBuffer; + btAlignedObjectArray m_tempBuffer; btAlignedObjectArray m_A; btAlignedObjectArray m_b; btAlignedObjectArray m_x; btAlignedObjectArray m_lo; btAlignedObjectArray m_hi; - btAlignedObjectArray m_dependencies; + btAlignedObjectArray m_dependencies; btDantzigScratchMemory m_scratchMemory; -public: +public: btDantzigSolver() - :m_acceptableUpperLimitSolution(btScalar(1000)) + : m_acceptableUpperLimitSolution(btScalar(1000)) { } - 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) + 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) { bool result = true; int n = b.rows(); @@ -52,14 +50,12 @@ public: int nub = 0; btAlignedObjectArray ww; ww.resize(n); - const btScalar* Aptr = A.getBufferPointer(); - m_A.resize(n*n); - for (int i=0;i= 1) { - cout << "Dimension = " << dim << endl; - } -#endif //BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 1) + { + cout << "Dimension = " << dim << endl; + } +#endif //BT_DEBUG_OSTREAM btVectorXu solutionVector(2 * dim); solutionVector.setZero(); - - //, INIT, 0.); + + //, INIT, 0.); btMatrixXu ident(dim, dim); ident.setIdentity(); @@ -85,287 +81,289 @@ btScalar btEpsRoot() { #endif btMatrixXu mNeg = m_M.negative(); - - btMatrixXu A(dim, 2 * dim + 2); + + btMatrixXu A(dim, 2 * dim + 2); // - A.setSubMatrix(0, 0, dim - 1, dim - 1,ident); - A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,mNeg); + A.setSubMatrix(0, 0, dim - 1, dim - 1, ident); + A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1, mNeg); A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f); - A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,m_q); + A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1, m_q); #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; -#endif //BT_DEBUG_OSTREAM +#endif //BT_DEBUG_OSTREAM + // btVectorXu q_; + // q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1); - // btVectorXu q_; - // q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1); - - btAlignedObjectArray basis; - //At first, all w-values are in the basis - for (int i = 0; i < dim; i++) - basis.push_back(i); + btAlignedObjectArray basis; + //At first, all w-values are in the basis + for (int i = 0; i < dim; i++) + basis.push_back(i); int pivotRowIndex = -1; btScalar minValue = 1e30f; bool greaterZero = true; - for (int i=0;i= 3) + if (DEBUGLEVEL >= 3) { - // cout << "A: " << A << endl; - cout << "pivotRowIndex " << pivotRowIndex << endl; - cout << "pivotColIndex " << pivotColIndex << endl; - cout << "Basis: "; - for (int i = 0; i < basis.size(); i++) - cout << basis[i] << " "; - cout << endl; - } -#endif //BT_DEBUG_OSTREAM + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; + cout << "Basis: "; + for (int i = 0; i < basis.size(); i++) + cout << basis[i] << " "; + cout << endl; + } +#endif //BT_DEBUG_OSTREAM if (!greaterZero) { + if (maxloops == 0) + { + maxloops = 100; + // maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<= 3) { - // cout << "A: " << A << endl; - cout << "pivotRowIndex " << pivotRowIndex << endl; - cout << "pivotColIndex " << pivotColIndex << endl; - cout << "Basis: "; - for (int i = 0; i < basis.size(); i++) - cout << basis[i] << " "; - cout << endl; - } -#endif //BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 3) + { + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; + cout << "Basis: "; + for (int i = 0; i < basis.size(); i++) + cout << basis[i] << " "; + cout << endl; + } +#endif //BT_DEBUG_OSTREAM - int pivotColIndexOld = pivotColIndex; + int pivotColIndexOld = pivotColIndex; - /*find new column index */ - if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value - pivotColIndex = basis[pivotRowIndex] + dim; - else - //else do it the other way round and get in the corresponding w-value - pivotColIndex = basis[pivotRowIndex] - dim; + /*find new column index */ + if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value + pivotColIndex = basis[pivotRowIndex] + dim; + else + //else do it the other way round and get in the corresponding w-value + pivotColIndex = basis[pivotRowIndex] - dim; - /*the column becomes part of the basis*/ - basis[pivotRowIndex] = pivotColIndexOld; + /*the column becomes part of the basis*/ + basis[pivotRowIndex] = pivotColIndexOld; - pivotRowIndex = findLexicographicMinimum(A, pivotColIndex); + pivotRowIndex = findLexicographicMinimum(A, pivotColIndex); - if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary - GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); - basis[pivotRowIndex] = pivotColIndex; //update basis - break; - } - - } + if (z0Row == pivotRowIndex) + { //if z0 leaves the basis the solution is found --> one last elimination step is necessary + GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); + basis[pivotRowIndex] = pivotColIndex; //update basis + break; + } + } #ifdef BT_DEBUG_OSTREAM - if(DEBUGLEVEL >= 1) { - cout << "Number of loops: " << steps << endl; - cout << "Number of maximal loops: " << maxloops << endl; - } -#endif //BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 1) + { + cout << "Number of loops: " << steps << endl; + cout << "Number of maximal loops: " << maxloops << endl; + } +#endif //BT_DEBUG_OSTREAM - if(!validBasis(basis)) { - info = -1; + if (!validBasis(basis)) + { + info = -1; #ifdef BT_DEBUG_OSTREAM - if(DEBUGLEVEL >= 1) - cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl; -#endif //BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 1) + cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl; +#endif //BT_DEBUG_OSTREAM - return solutionVector; - } - - } + return solutionVector; + } + } #ifdef BT_DEBUG_OSTREAM - if (DEBUGLEVEL >= 2) { - // cout << "A: " << A << endl; - cout << "pivotRowIndex " << pivotRowIndex << endl; - cout << "pivotColIndex " << pivotColIndex << endl; - } -#endif //BT_DEBUG_OSTREAM - - for (int i = 0; i < basis.size(); i++) + if (DEBUGLEVEL >= 2) { - solutionVector[basis[i]] = A(i,2*dim+1);//q_[i]; + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; + } +#endif //BT_DEBUG_OSTREAM + + for (int i = 0; i < basis.size(); i++) + { + solutionVector[basis[i]] = A(i, 2 * dim + 1); //q_[i]; } - info = 0; + info = 0; - return solutionVector; - } + return solutionVector; +} - int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) { - int RowIndex = 0; - int dim = A.rows(); - btAlignedObjectArray Rows; - for (int row = 0; row < dim; row++) - { - - btVectorXu vec(dim + 1); - vec.setZero();//, INIT, 0.) - Rows.push_back(vec); - btScalar a = A(row, pivotColIndex); - if (a > 0) { - Rows[row][0] = A(row, 2 * dim + 1) / a; - Rows[row][1] = A(row, 2 * dim) / a; - for (int j = 2; j < dim + 1; j++) - Rows[row][j] = A(row, j - 1) / a; +int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex) +{ + int RowIndex = 0; + int dim = A.rows(); + btAlignedObjectArray Rows; + for (int row = 0; row < dim; row++) + { + btVectorXu vec(dim + 1); + vec.setZero(); //, INIT, 0.) + Rows.push_back(vec); + btScalar a = A(row, pivotColIndex); + if (a > 0) + { + Rows[row][0] = A(row, 2 * dim + 1) / a; + Rows[row][1] = A(row, 2 * dim) / a; + for (int j = 2; j < dim + 1; j++) + Rows[row][j] = A(row, j - 1) / a; #ifdef BT_DEBUG_OSTREAM - // if (DEBUGLEVEL) { - // cout << "Rows(" << row << ") = " << Rows[row] << endl; + // if (DEBUGLEVEL) { + // cout << "Rows(" << row << ") = " << Rows[row] << endl; // } -#endif - } - } +#endif + } + } - for (int i = 0; i < Rows.size(); i++) - { - if (Rows[i].nrm2() > 0.) { + for (int i = 0; i < Rows.size(); i++) + { + if (Rows[i].nrm2() > 0.) + { + int j = 0; + for (; j < Rows.size(); j++) + { + if (i != j) + { + if (Rows[j].nrm2() > 0.) + { + btVectorXu test(dim + 1); + for (int ii = 0; ii < dim + 1; ii++) + { + test[ii] = Rows[j][ii] - Rows[i][ii]; + } - int j = 0; - for (; j < Rows.size(); j++) - { - if(i != j) - { - if(Rows[j].nrm2() > 0.) - { - btVectorXu test(dim + 1); - for (int ii=0;ii 0) - return true; + while (i < v.size() - 1 && fabs(v[i]) < btMachEps()) + i++; + if (v[i] > 0) + return true; - return false; - } + return false; +} -void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray& basis) +void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray& basis) { - btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex); #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; #endif - for (int i = 0; i < A.rows(); i++) + for (int i = 0; i < A.rows(); i++) { - if (i != pivotRowIndex) - { - for (int j = 0; j < A.cols(); j++) + if (i != pivotRowIndex) { - if (j != pivotColumnIndex) - { - btScalar v = A(i, j); - v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a; - A.setElem(i, j, v); - } + for (int j = 0; j < A.cols(); j++) + { + if (j != pivotColumnIndex) + { + btScalar v = A(i, j); + v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a; + A.setElem(i, j, v); + } + } } - } } #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; -#endif //BT_DEBUG_OSTREAM - for (int i = 0; i < A.cols(); i++) +#endif //BT_DEBUG_OSTREAM + for (int i = 0; i < A.cols(); i++) { - A.mulElem(pivotRowIndex, i,-a); - } -#ifdef BT_DEBUG_OSTREAM - cout << A << std::endl; -#endif //#ifdef BT_DEBUG_OSTREAM - - for (int i = 0; i < A.rows(); i++) - { - if (i != pivotRowIndex) - { - A.setElem(i, pivotColumnIndex,0); - } + A.mulElem(pivotRowIndex, i, -a); } #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; -#endif //#ifdef BT_DEBUG_OSTREAM - } +#endif //#ifdef BT_DEBUG_OSTREAM - bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector) + for (int i = 0; i < A.rows(); i++) + { + if (i != pivotRowIndex) + { + A.setElem(i, pivotColumnIndex, 0); + } + } +#ifdef BT_DEBUG_OSTREAM + cout << A << std::endl; +#endif //#ifdef BT_DEBUG_OSTREAM +} + +bool btLemkeAlgorithm::greaterZero(const btVectorXu& vector) { - bool isGreater = true; - for (int i = 0; i < vector.size(); i++) { - if (vector[i] < 0) { - isGreater = false; - break; - } - } + bool isGreater = true; + for (int i = 0; i < vector.size(); i++) + { + if (vector[i] < 0) + { + isGreater = false; + break; + } + } - return isGreater; - } - - bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray& basis) - { - bool isValid = true; - for (int i = 0; i < basis.size(); i++) { - if (basis[i] >= basis.size() * 2) { //then z0 is in the base - isValid = false; - break; - } - } - - return isValid; - } + return isGreater; +} +bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray& basis) +{ + bool isValid = true; + for (int i = 0; i < basis.size(); i++) + { + if (basis[i] >= basis.size() * 2) + { //then z0 is in the base + isValid = false; + break; + } + } + return isValid; +} diff --git a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h index 7555cd9d2..3c6bf72a2 100644 --- a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h +++ b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h @@ -19,90 +19,84 @@ subject to the following restrictions: //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h //STL/std::vector replaced by btAlignedObjectArray - - #ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_ #define BT_NUMERICS_LEMKE_ALGORITHM_H_ #include "LinearMath/btMatrixX.h" - -#include //todo: replace by btAlignedObjectArray +#include //todo: replace by btAlignedObjectArray class btLemkeAlgorithm { public: - + btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int& DEBUGLEVEL_ = 0) : DEBUGLEVEL(DEBUGLEVEL_) + { + setSystem(M_, q_); + } - btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) : - DEBUGLEVEL(DEBUGLEVEL_) - { - setSystem(M_, q_); - } - - /* GETTER / SETTER */ - /** + /* GETTER / SETTER */ + /** * \brief return info of solution process */ - int getInfo() { - return info; - } + int getInfo() + { + return info; + } - /** + /** * \brief get the number of steps until the solution was found */ - int getSteps(void) { - return steps; - } + int getSteps(void) + { + return steps; + } - - - /** + /** * \brief set system with Matrix M and vector q */ - void setSystem(const btMatrixXu & M_, const btVectorXu & q_) + void setSystem(const btMatrixXu& M_, const btVectorXu& q_) { m_M = M_; m_q = q_; - } - /***************************************************/ + } + /***************************************************/ - /** + /** * \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) */ - btVectorXu solve(unsigned int maxloops = 0); + btVectorXu solve(unsigned int maxloops = 0); - virtual ~btLemkeAlgorithm() { - } + virtual ~btLemkeAlgorithm() + { + } protected: - int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex); - bool LexicographicPositive(const btVectorXu & v); - void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray& basis); - bool greaterZero(const btVectorXu & vector); - bool validBasis(const btAlignedObjectArray& basis); + int findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex); + bool LexicographicPositive(const btVectorXu& v); + void GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray& basis); + bool greaterZero(const btVectorXu& vector); + bool validBasis(const btAlignedObjectArray& basis); - btMatrixXu m_M; - btVectorXu m_q; + btMatrixXu m_M; + btVectorXu m_q; - /** + /** * \brief number of steps until the Lemke algorithm found a solution */ - unsigned int steps; + unsigned int steps; - /** + /** * \brief define level of debug output */ - int DEBUGLEVEL; + int DEBUGLEVEL; - /** + /** * \brief did the algorithm find a solution * * -1 : not successful * 0 : successful */ - int info; + int info; }; - #endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */ diff --git a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h index 98484c379..f18c4ea41 100644 --- a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h +++ b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h @@ -17,334 +17,322 @@ subject to the following restrictions: #ifndef BT_LEMKE_SOLVER_H #define BT_LEMKE_SOLVER_H - #include "btMLCPSolverInterface.h" #include "btLemkeAlgorithm.h" - - - -///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) " +///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) " ///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time. ///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team class btLemkeSolver : public btMLCPSolverInterface { protected: - public: - - btScalar m_maxValue; - int m_debugLevel; - int m_maxLoops; - bool m_useLoHighBounds; - - + btScalar m_maxValue; + int m_debugLevel; + int m_maxLoops; + bool m_useLoHighBounds; btLemkeSolver() - :m_maxValue(100000), - m_debugLevel(0), - m_maxLoops(1000), - m_useLoHighBounds(true) + : m_maxValue(100000), + m_debugLevel(0), + m_maxLoops(1000), + m_useLoHighBounds(true) { } - 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) + 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 (m_useLoHighBounds) { + BT_PROFILE("btLemkeSolver::solveMLCP"); + int n = A.rows(); + if (0 == n) + return true; - BT_PROFILE("btLemkeSolver::solveMLCP"); - int n = A.rows(); - if (0==n) - return true; - - bool fail = false; + bool fail = false; - btVectorXu solution(n); - btVectorXu q1; - q1.resize(n); - for (int row=0;rowm_maxValue) - { - if (x[i]> errorValueMax) + b1.setElem(row, 0, -b[row]); + for (int col = 0; col < n; col++) { - fail = true; - errorIndexMax = i; - errorValueMax = x[i]; - } - ////printf("x[i] = %f,",x[i]); - } - if (x[i]<-m_maxValue) - { - if (x[i] m_maxValue) + { + if (x[i] > errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i] < -m_maxValue) + { + if (x[i] < errorValueMin) + { + errorIndexMin = i; + errorValueMin = x[i]; + fail = true; + //printf("x[i] = %f,",x[i]); + } + } + } + if (fail) + { + int m_errorCountTimes = 0; + if (errorIndexMin < 0) + errorValueMin = 0.f; + if (errorIndexMax < 0) + errorValueMax = 0.f; + m_errorCountTimes++; + // printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); + for (int i = 0; i < n; i++) + { + x[i] = 0.f; + } + } + return !fail; } - return !fail; - } else - - { + else + + { int dimension = A.rows(); - if (0==dimension) - return true; - -// printf("================ solving using Lemke/Newton/Fixpoint\n"); + if (0 == dimension) + return true; - btVectorXu q; - q.resize(dimension); - for (int row=0;rowm_maxValue) + + btLemkeAlgorithm lemke(A, q, m_debugLevel); + + lemke.setSystem(A, q); + + btVectorXu solution = lemke.solve(m_maxLoops); + + //check solution + + bool fail = false; + int errorIndexMax = -1; + int errorIndexMin = -1; + float errorValueMax = -1e30; + float errorValueMin = 1e30; + + for (int i = 0; i < dimension; i++) { - if (x[i]> errorValueMax) + x[i] = solution[i + dimension]; + volatile btScalar check = x[i]; + if (x[i] != check) { - fail = true; - errorIndexMax = i; - errorValueMax = x[i]; + x.setZero(); + return false; } - ////printf("x[i] = %f,",x[i]); - } - if (x[i]<-m_maxValue) - { - if (x[i] m_maxValue) { - errorIndexMin = i; - errorValueMin = x[i]; - fail = true; - //printf("x[i] = %f,",x[i]); + if (x[i] > errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i] < -m_maxValue) + { + if (x[i] < errorValueMin) + { + errorIndexMin = i; + errorValueMin = x[i]; + fail = true; + //printf("x[i] = %f,",x[i]); + } } } - } - if (fail) - { - static int errorCountTimes = 0; - if (errorIndexMin<0) - errorValueMin = 0.f; - if (errorIndexMax<0) - errorValueMax = 0.f; - printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); - for (int i=0;i limitDependenciesCopy = m_limitDependencies; -// printf("solve first LCP\n"); - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + // printf("solve first LCP\n"); + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); if (result) - result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations ); - - } else + result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations); + } + else { - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); } return result; } struct btJointNode { - int jointIndex; // pointer to enclosing dxJoint object - int otherBodyIndex; // *other* body this joint is connected to - int nextJointNodeIndex;//-1 for null + int jointIndex; // pointer to enclosing dxJoint object + int otherBodyIndex; // *other* body this joint is connected to + int nextJointNodeIndex; //-1 for null int constraintRowIndex; }; - - void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numContactRows = interleaveContactAndFriction ? 3 : 1; @@ -163,36 +157,36 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_bSplit.resize(numConstraintRows); m_b.setZero(); m_bSplit.setZero(); - for (int i=0;im_jacDiagABInv; if (!btFuzzyZero(jacDiag)) { btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; - m_b[i]=rhs/jacDiag; - m_bSplit[i] = rhsPenetration/jacDiag; + m_b[i] = rhs / jacDiag; + m_bSplit[i] = rhsPenetration / jacDiag; } - } } -// btScalar* w = 0; -// int nub = 0; + // btScalar* w = 0; + // int nub = 0; m_lo.resize(numConstraintRows); m_hi.resize(numConstraintRows); - + { BT_PROFILE("init lo/ho"); - for (int i=0;i=0) + if (0) //m_limitDependencies[i]>=0) { m_lo[i] = -BT_INFINITY; m_hi[i] = BT_INFINITY; - } else + } + else { m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; @@ -201,48 +195,48 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } // - int m=m_allConstraintPtrArray.size(); + int m = m_allConstraintPtrArray.size(); int numBodies = m_tmpSolverBodyPool.size(); btAlignedObjectArray bodyJointNodeArray; { BT_PROFILE("bodyJointNodeArray.resize"); - bodyJointNodeArray.resize(numBodies,-1); + bodyJointNodeArray.resize(numBodies, -1); } btAlignedObjectArray jointNodeArray; { BT_PROFILE("jointNodeArray.reserve"); - jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); + jointNodeArray.reserve(2 * m_allConstraintPtrArray.size()); } - btMatrixXu& J3 = m_scratchJ3; + btMatrixXu& J3 = m_scratchJ3; { BT_PROFILE("J3.resize"); - J3.resize(2*m,8); + J3.resize(2 * m, 8); } - btMatrixXu& JinvM3 = m_scratchJInvM3; + btMatrixXu& JinvM3 = m_scratchJInvM3; { BT_PROFILE("JinvM3.resize/setZero"); - JinvM3.resize(2*m,8); + JinvM3.resize(2 * m, 8); JinvM3.setZero(); J3.setZero(); } - int cur=0; + int cur = 0; int rowOffset = 0; - btAlignedObjectArray& ofs = m_scratchOfs; + btAlignedObjectArray& ofs = m_scratchOfs; { BT_PROFILE("ofs resize"); ofs.resize(0); ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); - } + } { BT_PROFILE("Compute J and JinvM"); - int c=0; + int c = 0; int numRows = 0; - for (int i=0;im_solverBodyIdA; @@ -250,14 +244,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - numRows = im_contactNormal1 * orgBodyA->getInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); - for (int r=0;r<3;r++) + for (int r = 0; r < 3; r++) { - J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); - J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); - JinvM3.setElem(cur,r,normalInvMass[r]); - JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); + J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]); + J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]); + JinvM3.setElem(cur, r, normalInvMass[r]); + JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]); } - J3.setElem(cur,3,0); - JinvM3.setElem(cur,3,0); - J3.setElem(cur,7,0); - JinvM3.setElem(cur,7,0); + J3.setElem(cur, 3, 0); + JinvM3.setElem(cur, 3, 0); + J3.setElem(cur, 7, 0); + JinvM3.setElem(cur, 7, 0); } - } else + } + else { cur += numRows; } if (orgBodyB) { - { - int slotB=-1; + int slotB = -1; //find free jointNode slot for sbA - slotB =jointNodeArray.size(); - jointNodeArray.expand();//NonInitializing(); + slotB = jointNodeArray.size(); + jointNodeArray.expand(); //NonInitializing(); int prevSlot = bodyJointNodeArray[sbB]; bodyJointNodeArray[sbB] = slotB; jointNodeArray[slotB].nextJointNodeIndex = prevSlot; @@ -302,78 +296,74 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) jointNodeArray[slotB].constraintRowIndex = i; } - for (int row=0;rowm_contactNormal2*orgBodyB->getInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); - for (int r=0;r<3;r++) + for (int r = 0; r < 3; r++) { - J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); - J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); - JinvM3.setElem(cur,r,normalInvMassB[r]); - JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); + J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]); + J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]); + JinvM3.setElem(cur, r, normalInvMassB[r]); + JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]); } - J3.setElem(cur,3,0); - JinvM3.setElem(cur,3,0); - J3.setElem(cur,7,0); - JinvM3.setElem(cur,7,0); + J3.setElem(cur, 3, 0); + JinvM3.setElem(cur, 3, 0); + J3.setElem(cur, 7, 0); + JinvM3.setElem(cur, 7, 0); } } else { cur += numRows; } - rowOffset+=numRows; - + rowOffset += numRows; } - } - //compute JinvM = J*invM. const btScalar* JinvM = JinvM3.getBufferPointer(); const btScalar* Jptr = J3.getBufferPointer(); { BT_PROFILE("m_A.resize"); - m_A.resize(n,n); + m_A.resize(n, n); } - + { BT_PROFILE("m_A.setZero"); m_A.setZero(); } - int c=0; + int c = 0; { int numRows = 0; BT_PROFILE("Compute A"); - for (int i=0;im_solverBodyIdA; int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; - // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - numRows = i=0) + while (startJointNodeA >= 0) { int j0 = jointNodeArray[startJointNodeA].jointIndex; int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; - if (j0m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; + size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0; //printf("%d joint i %d and j0: %d: ",count++,i,j0); - m_A.multiplyAdd2_p8r ( JinvMrow, - Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); + m_A.multiplyAdd2_p8r(JinvMrow, + Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]); } startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; } @@ -381,17 +371,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int startJointNodeB = bodyJointNodeArray[sbB]; - while (startJointNodeB>=0) + while (startJointNodeB >= 0) { int j1 = jointNodeArray[startJointNodeB].jointIndex; int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; - if (j1m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; - m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, - Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); + int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; + size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0; + m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows, + Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]); } startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; } @@ -402,27 +392,25 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) BT_PROFILE("compute diagonal"); // compute diagonal blocks of m_A - int row__ = 0; + int row__ = 0; int numJointRows = m_allConstraintPtrArray.size(); - int jj=0; - for (;row__m_solverBodyIdA; int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; - // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; - const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; - - const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; - const btScalar *Jrow = Jptr + 2*8*(size_t)row__; - m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__); - if (orgBodyB) + const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; + const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__; + m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__); + if (orgBodyB) { - m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__); + m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__); } row__ += infom; jj++; @@ -433,12 +421,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) if (1) { // add cfm to the diagonal of m_A - for ( int i=0; im_jacDiagABInv) { - m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv; + m_b[i] = m_allConstraintPtrArray[i]->m_rhs / m_allConstraintPtrArray[i]->m_jacDiagABInv; if (infoGlobal.m_splitImpulse) - m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv; + m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration / m_allConstraintPtrArray[i]->m_jacDiagABInv; } } - - btMatrixXu& Minv = m_scratchMInv; - Minv.resize(6*numBodies,6*numBodies); + + btMatrixXu& Minv = m_scratchMInv; + Minv.resize(6 * numBodies, 6 * numBodies); Minv.setZero(); - for (int i=0;igetInvInertiaTensorWorld()[r][c] : 0); + + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + setElem(Minv, i * 6 + 3 + r, i * 6 + 3 + c, orgBody ? orgBody->getInvInertiaTensorWorld()[r][c] : 0); } - - btMatrixXu& J = m_scratchJ; - J.resize(numConstraintRows,6*numBodies); + + btMatrixXu& J = m_scratchJ; + J.resize(numConstraintRows, 6 * numBodies); J.setZero(); - + m_lo.resize(numConstraintRows); m_hi.resize(numConstraintRows); - - for (int i=0;im_lowerLimit; m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - + int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) { - setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); - setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); - setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); - setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); - setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); - setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); + setElem(J, i, 6 * bodyIndex0 + 0, m_allConstraintPtrArray[i]->m_contactNormal1[0]); + setElem(J, i, 6 * bodyIndex0 + 1, m_allConstraintPtrArray[i]->m_contactNormal1[1]); + setElem(J, i, 6 * bodyIndex0 + 2, m_allConstraintPtrArray[i]->m_contactNormal1[2]); + setElem(J, i, 6 * bodyIndex0 + 3, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); + setElem(J, i, 6 * bodyIndex0 + 4, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); + setElem(J, i, 6 * bodyIndex0 + 5, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); } if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) { - setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); - setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); - setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); - setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); - setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); - setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); + setElem(J, i, 6 * bodyIndex1 + 0, m_allConstraintPtrArray[i]->m_contactNormal2[0]); + setElem(J, i, 6 * bodyIndex1 + 1, m_allConstraintPtrArray[i]->m_contactNormal2[1]); + setElem(J, i, 6 * bodyIndex1 + 2, m_allConstraintPtrArray[i]->m_contactNormal2[2]); + setElem(J, i, 6 * bodyIndex1 + 3, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); + setElem(J, i, 6 * bodyIndex1 + 4, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); + setElem(J, i, 6 * bodyIndex1 + 5, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); } } - - btMatrixXu& J_transpose = m_scratchJTranspose; - J_transpose= J.transpose(); - btMatrixXu& tmp = m_scratchTmp; + btMatrixXu& J_transpose = m_scratchJTranspose; + J_transpose = J.transpose(); + + btMatrixXu& tmp = m_scratchTmp; { { BT_PROFILE("J*Minv"); - tmp = J*Minv; - + tmp = J * Minv; } { BT_PROFILE("J*tmp"); - m_A = tmp*J_transpose; + m_A = tmp * J_transpose; } } if (1) { // add cfm to the diagonal of m_A - for ( int i=0; i m_limitDependencies; - btAlignedObjectArray m_allConstraintPtrArray; + btAlignedObjectArray m_allConstraintPtrArray; btMLCPSolverInterface* m_solver; int m_fallback; - /// 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); + /// 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); virtual void createMLCP(const btContactSolverInfo& infoGlobal); virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); @@ -65,8 +62,7 @@ protected: virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); public: - - btMLCPSolver( btMLCPSolverInterface* solver); + btMLCPSolver(btMLCPSolverInterface* solver); virtual ~btMLCPSolver(); void setMLCPSolver(btMLCPSolverInterface* solver) @@ -83,12 +79,10 @@ public: m_fallback = num; } - virtual btConstraintSolverType getSolverType() const + virtual btConstraintSolverType getSolverType() const { return BT_MLCP_SOLVER; } - }; - -#endif //BT_MLCP_SOLVER_H +#endif //BT_MLCP_SOLVER_H diff --git a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h index 25bb3f6d3..6b0465b88 100644 --- a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h +++ b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h @@ -27,7 +27,7 @@ public: } //return true is it solves the problem successfully - 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)=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) = 0; }; -#endif //BT_MLCP_SOLVER_INTERFACE_H +#endif //BT_MLCP_SOLVER_INTERFACE_H diff --git a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btPATHSolver.h b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btPATHSolver.h index 9ec31a6d4..7f8eec3f6 100644 --- a/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btPATHSolver.h +++ b/Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btPATHSolver.h @@ -14,38 +14,35 @@ subject to the following restrictions: */ ///original version written by Erwin Coumans, October 2013 - #ifndef BT_PATH_SOLVER_H #define BT_PATH_SOLVER_H //#define BT_USE_PATH #ifdef BT_USE_PATH -extern "C" { +extern "C" +{ #include "PATH/SimpleLCP.h" #include "PATH/License.h" #include "PATH/Error_Interface.h" }; - void __stdcall MyError(Void *data, Char *msg) +void __stdcall MyError(Void *data, Char *msg) { - printf("Path Error: %s\n",msg); + printf("Path Error: %s\n", msg); } - void __stdcall MyWarning(Void *data, Char *msg) +void __stdcall MyWarning(Void *data, Char *msg) { - printf("Path Warning: %s\n",msg); + printf("Path Warning: %s\n", msg); } Error_Interface e; - - #include "btMLCPSolverInterface.h" #include "Dantzig/lcp.h" class btPathSolver : public btMLCPSolverInterface { public: - btPathSolver() { License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0"); @@ -55,17 +52,15 @@ public: Error_SetInterface(&e); } - - 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) + 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) { MCP_Termination status; - int numVariables = b.rows(); - if (0==numVariables) + if (0 == numVariables) return true; - /* - variables - the number of variables in the problem + /* - variables - the number of variables in the problem - m_nnz - the number of nonzeros in the M matrix - m_i - a vector of size m_nnz containing the row indices for M - m_j - a vector of size m_nnz containing the column indices for M @@ -78,16 +73,16 @@ public: btAlignedObjectArray rowIndices; btAlignedObjectArray colIndices; - for (int i=0;i rhs; btAlignedObjectArray upperBounds; btAlignedObjectArray lowerBounds; - for (int i=0;i& limitDependency, int numIterations, bool useSparsity = true) + 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()) return true; @@ -46,65 +43,65 @@ public: btAssert(A.rows() == b.rows()); int i, j, numRows = A.rows(); - + btScalar delta; - for (int k = 0; k =0) + if (limitDependency[i] >= 0) { s = x[limitDependency[i]]; - if (s<0) - s=1; + if (s < 0) + s = 1; } - - if (x[i]hi[i]*s) - x[i]=hi[i]*s; + + if (x[i] < lo[i] * s) + 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; + m_leastSquaresResidual += diff * diff; } - btScalar eps = m_leastSquaresResidualThreshold; - if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1))) + 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); + printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual, k); #endif break; } } return true; } - }; -#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H +#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index a7b168846..fc70d8e63 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -24,17 +24,16 @@ #define ROLLING_INFLUENCE_FIX - btRigidBody& btActionInterface::getFixedBody() { - static btRigidBody s_fixed(0, 0,0); - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); + static btRigidBody s_fixed(0, 0, 0); + s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); return s_fixed; } -btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) -:m_vehicleRaycaster(raycaster), -m_pitchControl(btScalar(0.)) +btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster) + : m_vehicleRaycaster(raycaster), + m_pitchControl(btScalar(0.)) { m_chassisBody = chassis; m_indexRightAxis = 0; @@ -43,28 +42,22 @@ m_pitchControl(btScalar(0.)) defaultInit(tuning); } - void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning) { (void)tuning; m_currentVehicleSpeedKmHour = btScalar(0.); m_steeringValue = btScalar(0.); - } - - btRaycastVehicle::~btRaycastVehicle() { } - // // basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed // -btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel) +btWheelInfo& btRaycastVehicle::addWheel(const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0, const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius, const btVehicleTuning& tuning, bool isFrontWheel) { - btWheelInfoConstructionInfo ci; ci.m_chassisConnectionCS = connectionPointCS; @@ -80,76 +73,76 @@ btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm; ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce; - m_wheelInfo.push_back( btWheelInfo(ci)); - - btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1]; - - updateWheelTransformsWS( wheel , false ); - updateWheelTransform(getNumWheels()-1,false); + m_wheelInfo.push_back(btWheelInfo(ci)); + + btWheelInfo& wheel = m_wheelInfo[getNumWheels() - 1]; + + updateWheelTransformsWS(wheel, false); + updateWheelTransform(getNumWheels() - 1, false); return wheel; } - - - -const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const +const btTransform& btRaycastVehicle::getWheelTransformWS(int wheelIndex) const { btAssert(wheelIndex < getNumWheels()); const btWheelInfo& wheel = m_wheelInfo[wheelIndex]; return wheel.m_worldTransform; - } -void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform) +void btRaycastVehicle::updateWheelTransform(int wheelIndex, bool interpolatedTransform) { - - btWheelInfo& wheel = m_wheelInfo[ wheelIndex ]; - updateWheelTransformsWS(wheel,interpolatedTransform); + btWheelInfo& wheel = m_wheelInfo[wheelIndex]; + updateWheelTransformsWS(wheel, interpolatedTransform); btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS; btVector3 fwd = up.cross(right); fwd = fwd.normalize(); -// up = right.cross(fwd); -// up.normalize(); + // up = right.cross(fwd); + // up.normalize(); //rotate around steering over de wheelAxleWS btScalar steering = wheel.m_steering; - - btQuaternion steeringOrn(up,steering);//wheel.m_steering); + + btQuaternion steeringOrn(up, steering); //wheel.m_steering); btMatrix3x3 steeringMat(steeringOrn); - btQuaternion rotatingOrn(right,-wheel.m_rotation); + 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 - ); + wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength); } void btRaycastVehicle::resetSuspension() { - int i; - for (i=0;igetMotionState()->getWorldTransform(chassisTrans); } - wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); - wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; + wheel.m_raycastInfo.m_hardPointWS = chassisTrans(wheel.m_chassisConnectionPointCS); + wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS; wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; } btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) { - updateWheelTransformsWS( wheel,false); + updateWheelTransformsWS(wheel, false); - btScalar depth = -1; - - btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius; + + btScalar raylen = wheel.getSuspensionRestLength() + wheel.m_wheelsRadius; btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; @@ -179,12 +171,12 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; btScalar param = btScalar(0.); - - btVehicleRaycaster::btVehicleRaycasterResult rayResults; + + btVehicleRaycaster::btVehicleRaycasterResult rayResults; btAssert(m_vehicleRaycaster); - void* object = m_vehicleRaycaster->castRay(source,target,rayResults); + void* object = m_vehicleRaycaster->castRay(source, target, rayResults); wheel.m_raycastInfo.m_groundObject = 0; @@ -192,19 +184,18 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) { param = rayResults.m_distFraction; depth = raylen * rayResults.m_distFraction; - wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; + wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; wheel.m_raycastInfo.m_isInContact = true; - - wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!; + + wheel.m_raycastInfo.m_groundObject = &getFixedBody(); ///@todo for driving on dynamic/movable objects!; //wheel.m_raycastInfo.m_groundObject = object; - - btScalar hitDistance = param*raylen; + btScalar hitDistance = param * raylen; wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; //clamp on max suspension travel - btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01); - btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01); + btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm * btScalar(0.01); + btScalar maxSuspensionLength = wheel.getSuspensionRestLength() + wheel.m_maxSuspensionTravelCm * btScalar(0.01); if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) { wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; @@ -216,16 +207,16 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; - btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); + btScalar denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(wheel.m_raycastInfo.m_wheelDirectionWS); btVector3 chassis_velocity_at_contactPoint; - btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); + btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition(); chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); - btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); + btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint); - if ( denominator >= btScalar(-0.1)) + if (denominator >= btScalar(-0.1)) { wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); @@ -236,20 +227,19 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) wheel.m_suspensionRelativeVelocity = projVel * inv; wheel.m_clippedInvContactDotSuspension = inv; } - - } else + } + else { //put wheel info as in rest position wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); wheel.m_suspensionRelativeVelocity = btScalar(0.0); - wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; + wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS; wheel.m_clippedInvContactDotSuspension = btScalar(1.0); } return depth; } - const btTransform& btRaycastVehicle::getChassisWorldTransform() const { /*if (getRigidBody()->getMotionState()) @@ -260,26 +250,23 @@ const btTransform& btRaycastVehicle::getChassisWorldTransform() const } */ - return getRigidBody()->getCenterOfMassTransform(); } - -void btRaycastVehicle::updateVehicle( btScalar step ) +void btRaycastVehicle::updateVehicle(btScalar step) { { - for (int i=0;igetLinearVelocity().length(); - + const btTransform& chassisTrans = getChassisWorldTransform(); - btVector3 forwardW ( + btVector3 forwardW( chassisTrans.getBasis()[0][m_indexForwardAxis], chassisTrans.getBasis()[1][m_indexForwardAxis], chassisTrans.getBasis()[2][m_indexForwardAxis]); @@ -292,52 +279,47 @@ void btRaycastVehicle::updateVehicle( btScalar step ) // // simulate suspension // - - int i=0; - for (i=0;i wheel.m_maxSuspensionForce) { suspensionForce = wheel.m_maxSuspensionForce; } btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition(); - + getRigidBody()->applyImpulse(impulse, relpos); - } - - - updateFriction( step); + updateFriction(step); - - for (i=0;igetCenterOfMassPosition(); - btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos ); + btVector3 vel = getRigidBody()->getVelocityInLocalPoint(relpos); if (wheel.m_raycastInfo.m_isInContact) { - const btTransform& chassisWorldTransform = getChassisWorldTransform(); + const btTransform& chassisWorldTransform = getChassisWorldTransform(); - btVector3 fwd ( + btVector3 fwd( chassisWorldTransform.getBasis()[0][m_indexForwardAxis], chassisWorldTransform.getBasis()[1][m_indexForwardAxis], chassisWorldTransform.getBasis()[2][m_indexForwardAxis]); @@ -346,99 +328,88 @@ void btRaycastVehicle::updateVehicle( btScalar step ) fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; btScalar proj2 = fwd.dot(vel); - + wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius); wheel.m_rotation += wheel.m_deltaRotation; - - } else + } + else { wheel.m_rotation += wheel.m_deltaRotation; } - - wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact + wheel.m_deltaRotation *= btScalar(0.99); //damping of rotation when not in contact } - - - } - -void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel) +void btRaycastVehicle::setSteeringValue(btScalar steering, int wheel) { - btAssert(wheel>=0 && wheel < getNumWheels()); + btAssert(wheel >= 0 && wheel < getNumWheels()); btWheelInfo& wheelInfo = getWheelInfo(wheel); wheelInfo.m_steering = steering; } - - -btScalar btRaycastVehicle::getSteeringValue(int wheel) const +btScalar btRaycastVehicle::getSteeringValue(int wheel) const { return getWheelInfo(wheel).m_steering; } - -void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) +void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) { - btAssert(wheel>=0 && wheel < getNumWheels()); + btAssert(wheel >= 0 && wheel < getNumWheels()); btWheelInfo& wheelInfo = getWheelInfo(wheel); wheelInfo.m_engineForce = force; } - -const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const +const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const { - btAssert((index >= 0) && (index < getNumWheels())); - + btAssert((index >= 0) && (index < getNumWheels())); + return m_wheelInfo[index]; } -btWheelInfo& btRaycastVehicle::getWheelInfo(int index) +btWheelInfo& btRaycastVehicle::getWheelInfo(int index) { - btAssert((index >= 0) && (index < getNumWheels())); - + btAssert((index >= 0) && (index < getNumWheels())); + return m_wheelInfo[index]; } -void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex) +void btRaycastVehicle::setBrake(btScalar brake, int wheelIndex) { - btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels())); + btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels())); getWheelInfo(wheelIndex).m_brake = brake; } - -void btRaycastVehicle::updateSuspension(btScalar deltaTime) +void btRaycastVehicle::updateSuspension(btScalar deltaTime) { (void)deltaTime; btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass(); - - for (int w_it=0; w_itcomputeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); - btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); - btScalar relaxation = 1.f; - m_jacDiagABInv = relaxation/(denom0+denom1); + btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld); + btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld); + btScalar relaxation = 1.f; + m_jacDiagABInv = relaxation / (denom0 + denom1); } - - - }; -btScalar calcRollingFriction(btWheelContactPoint& contactPoint); -btScalar calcRollingFriction(btWheelContactPoint& contactPoint) +btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround); +btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround) { - - btScalar j1=0.f; + btScalar j1 = 0.f; const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld; - btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); + btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition(); - - btScalar maxImpulse = contactPoint.m_maxImpulse; - + + btScalar maxImpulse = contactPoint.m_maxImpulse; + btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; @@ -513,252 +477,225 @@ 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); return j1; } - - - btScalar sideFrictionStiffness2 = btScalar(1.0); -void btRaycastVehicle::updateFriction(btScalar timeStep) +void btRaycastVehicle::updateFriction(btScalar timeStep) { + //calculate the impulse, so that the wheels don't move sidewards + int numWheel = getNumWheels(); + if (!numWheel) + return; - //calculate the impulse, so that the wheels don't move sidewards - int numWheel = getNumWheels(); - if (!numWheel) - return; + m_forwardWS.resize(numWheel); + m_axle.resize(numWheel); + m_forwardImpulse.resize(numWheel); + m_sideImpulse.resize(numWheel); - m_forwardWS.resize(numWheel); - m_axle.resize(numWheel); - m_forwardImpulse.resize(numWheel); - m_sideImpulse.resize(numWheel); - - int numWheelsOnGround = 0; - + int numWheelsOnGround = 0; - //collapse all those loops into one! - for (int i=0;i 0); + rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround); } } //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) - - - m_forwardImpulse[wheel] = btScalar(0.); - m_wheelInfo[wheel].m_skidInfo= btScalar(1.); + m_wheelInfo[wheel].m_skidInfo = btScalar(1.); if (groundObject) { - m_wheelInfo[wheel].m_skidInfo= btScalar(1.); - + m_wheelInfo[wheel].m_skidInfo = btScalar(1.); + btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip; btScalar maximpSide = maximp; btScalar maximpSquared = maximp * maximpSide; - - m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep; + m_forwardImpulse[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep; - btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor; - btScalar y = (m_sideImpulse[wheel] ) * sideFactor; - - btScalar impulseSquared = (x*x + y*y); + btScalar x = (m_forwardImpulse[wheel]) * fwdFactor; + btScalar y = (m_sideImpulse[wheel]) * sideFactor; + + btScalar impulseSquared = (x * x + y * y); if (impulseSquared > maximpSquared) { sliding = true; - + btScalar factor = maximp / btSqrt(impulseSquared); - + m_wheelInfo[wheel].m_skidInfo *= factor; } - } - + } } } - - - - if (sliding) + if (sliding) + { + for (int wheel = 0; wheel < getNumWheels(); wheel++) { - for (int wheel = 0;wheel < getNumWheels(); wheel++) + if (m_sideImpulse[wheel] != btScalar(0.)) { - if (m_sideImpulse[wheel] != btScalar(0.)) + if (m_wheelInfo[wheel].m_skidInfo < btScalar(1.)) { - if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.)) - { - m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; - m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; - } + m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; + m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; } } } + } - // apply the impulses + // apply the impulses + { + for (int wheel = 0; wheel < getNumWheels(); wheel++) { - for (int wheel = 0;wheelgetCenterOfMassPosition(); + + if (m_forwardImpulse[wheel] != btScalar(0.)) { - btWheelInfo& wheelInfo = m_wheelInfo[wheel]; + m_chassisBody->applyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos); + } + if (m_sideImpulse[wheel] != btScalar(0.)) + { + class btRigidBody* groundObject = (class btRigidBody*)m_wheelInfo[wheel].m_raycastInfo.m_groundObject; - btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - - m_chassisBody->getCenterOfMassPosition(); + btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - + groundObject->getCenterOfMassPosition(); - if (m_forwardImpulse[wheel] != btScalar(0.)) - { - m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos); - } - if (m_sideImpulse[wheel] != btScalar(0.)) - { - class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; + btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; - btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - - groundObject->getCenterOfMassPosition(); - - - btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; - -#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT. - btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis); - rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence)); +#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT. + btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis); + rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f - wheelInfo.m_rollInfluence)); #else - rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence; + rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence; #endif - m_chassisBody->applyImpulse(sideImp,rel_pos); + m_chassisBody->applyImpulse(sideImp, rel_pos); - //apply friction impulse on the ground - groundObject->applyImpulse(-sideImp,rel_pos2); - } + //apply friction impulse on the ground + groundObject->applyImpulse(-sideImp, rel_pos2); } } - - + } } - - -void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer) +void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer) { - - for (int v=0;vgetNumWheels();v++) + for (int v = 0; v < this->getNumWheels(); v++) { - btVector3 wheelColor(0,1,1); + btVector3 wheelColor(0, 1, 1); if (getWheelInfo(v).m_raycastInfo.m_isInContact) { - wheelColor.setValue(0,0,1); - } else + wheelColor.setValue(0, 0, 1); + } + else { - wheelColor.setValue(1,0,1); + wheelColor.setValue(1, 0, 1); } btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin(); - btVector3 axle = btVector3( + btVector3 axle = btVector3( getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()], getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()], getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]); //debug wheels (cylinders) - debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); - debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); - + debugDrawer->drawLine(wheelPosWS, wheelPosWS + axle, wheelColor); + debugDrawer->drawLine(wheelPosWS, getWheelInfo(v).m_raycastInfo.m_contactPointWS, wheelColor); } } - -void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) +void* btDefaultVehicleRaycaster::castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result) { -// RayResultCallback& resultCallback; + // RayResultCallback& resultCallback; - btCollisionWorld::ClosestRayResultCallback rayCallback(from,to); + btCollisionWorld::ClosestRayResultCallback rayCallback(from, to); m_dynamicsWorld->rayTest(from, to, rayCallback); if (rayCallback.hasHit()) { - const btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject); - if (body && body->hasContactResponse()) + if (body && body->hasContactResponse()) { result.m_hitPointInWorld = rayCallback.m_hitPointWorld; result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld; @@ -769,4 +706,3 @@ void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& } return 0; } - diff --git a/Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h index 04656b912..99d6894e5 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -24,122 +24,111 @@ class btDynamicsWorld; ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle. class btRaycastVehicle : public btActionInterface { + btAlignedObjectArray m_forwardWS; + btAlignedObjectArray m_axle; + btAlignedObjectArray m_forwardImpulse; + btAlignedObjectArray m_sideImpulse; - btAlignedObjectArray m_forwardWS; - btAlignedObjectArray m_axle; - btAlignedObjectArray m_forwardImpulse; - btAlignedObjectArray m_sideImpulse; - - ///backwards compatibility - int m_userConstraintType; - int m_userConstraintId; + ///backwards compatibility + int m_userConstraintType; + int m_userConstraintId; public: class btVehicleTuning + { + public: + btVehicleTuning() + : m_suspensionStiffness(btScalar(5.88)), + m_suspensionCompression(btScalar(0.83)), + m_suspensionDamping(btScalar(0.88)), + m_maxSuspensionTravelCm(btScalar(500.)), + m_frictionSlip(btScalar(10.5)), + m_maxSuspensionForce(btScalar(6000.)) { - public: + } + btScalar m_suspensionStiffness; + btScalar m_suspensionCompression; + btScalar m_suspensionDamping; + btScalar m_maxSuspensionTravelCm; + btScalar m_frictionSlip; + btScalar m_maxSuspensionForce; + }; - btVehicleTuning() - :m_suspensionStiffness(btScalar(5.88)), - m_suspensionCompression(btScalar(0.83)), - m_suspensionDamping(btScalar(0.88)), - m_maxSuspensionTravelCm(btScalar(500.)), - m_frictionSlip(btScalar(10.5)), - m_maxSuspensionForce(btScalar(6000.)) - { - } - btScalar m_suspensionStiffness; - btScalar m_suspensionCompression; - btScalar m_suspensionDamping; - btScalar m_maxSuspensionTravelCm; - btScalar m_frictionSlip; - btScalar m_maxSuspensionForce; - - }; private: - - btVehicleRaycaster* m_vehicleRaycaster; - btScalar m_pitchControl; - btScalar m_steeringValue; + btVehicleRaycaster* m_vehicleRaycaster; + btScalar m_pitchControl; + btScalar m_steeringValue; btScalar m_currentVehicleSpeedKmHour; btRigidBody* m_chassisBody; int m_indexRightAxis; int m_indexUpAxis; - int m_indexForwardAxis; + int m_indexForwardAxis; void defaultInit(const btVehicleTuning& tuning); public: - //constructor to create a car from an existing rigidbody - btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ); - - virtual ~btRaycastVehicle() ; + btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster); + virtual ~btRaycastVehicle(); ///btActionInterface interface - virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step) + virtual void updateAction(btCollisionWorld* collisionWorld, btScalar step) { - (void) collisionWorld; + (void)collisionWorld; updateVehicle(step); } - ///btActionInterface interface - void debugDraw(btIDebugDraw* debugDrawer); - + void debugDraw(btIDebugDraw* debugDrawer); + const btTransform& getChassisWorldTransform() const; - + btScalar rayCast(btWheelInfo& wheel); virtual void updateVehicle(btScalar step); - - + void resetSuspension(); - btScalar getSteeringValue(int wheel) const; + btScalar getSteeringValue(int wheel) const; - void setSteeringValue(btScalar steering,int wheel); + void setSteeringValue(btScalar steering, int wheel); + void applyEngineForce(btScalar force, int wheel); - void applyEngineForce(btScalar force, int wheel); + const btTransform& getWheelTransformWS(int wheelIndex) const; - const btTransform& getWheelTransformWS( int wheelIndex ) const; + void updateWheelTransform(int wheelIndex, bool interpolatedTransform = true); - void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true ); - -// void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth); + // void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth); - btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel); + btWheelInfo& addWheel(const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0, const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius, const btVehicleTuning& tuning, bool isFrontWheel); - inline int getNumWheels() const { - return int (m_wheelInfo.size()); + inline int getNumWheels() const + { + return int(m_wheelInfo.size()); } - - btAlignedObjectArray m_wheelInfo; + btAlignedObjectArray m_wheelInfo; - const btWheelInfo& getWheelInfo(int index) const; + const btWheelInfo& getWheelInfo(int index) const; - btWheelInfo& getWheelInfo(int index); + btWheelInfo& getWheelInfo(int index); - void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true); + void updateWheelTransformsWS(btWheelInfo& wheel, bool interpolatedTransform = true); - - void setBrake(btScalar brake,int wheelIndex); + void setBrake(btScalar brake, int wheelIndex); - void setPitchControl(btScalar pitch) + void setPitchControl(btScalar pitch) { m_pitchControl = pitch; } - - void updateSuspension(btScalar deltaTime); - - virtual void updateFriction(btScalar timeStep); + void updateSuspension(btScalar deltaTime); + virtual void updateFriction(btScalar timeStep); inline btRigidBody* getRigidBody() { @@ -151,7 +140,7 @@ public: return m_chassisBody; } - inline int getRightAxis() const + inline int getRightAxis() const { return m_indexRightAxis; } @@ -165,46 +154,44 @@ public: return m_indexForwardAxis; } - ///Worldspace forward vector btVector3 getForwardVector() const { - const btTransform& chassisTrans = getChassisWorldTransform(); + const btTransform& chassisTrans = getChassisWorldTransform(); - btVector3 forwardW ( - chassisTrans.getBasis()[0][m_indexForwardAxis], - chassisTrans.getBasis()[1][m_indexForwardAxis], - chassisTrans.getBasis()[2][m_indexForwardAxis]); + btVector3 forwardW( + chassisTrans.getBasis()[0][m_indexForwardAxis], + chassisTrans.getBasis()[1][m_indexForwardAxis], + chassisTrans.getBasis()[2][m_indexForwardAxis]); return forwardW; } ///Velocity of vehicle (positive if velocity vector has same direction as foward vector) - btScalar getCurrentSpeedKmHour() const + btScalar getCurrentSpeedKmHour() const { return m_currentVehicleSpeedKmHour; } - virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex) + virtual void setCoordinateSystem(int rightIndex, int upIndex, int forwardIndex) { m_indexRightAxis = rightIndex; m_indexUpAxis = upIndex; m_indexForwardAxis = forwardIndex; } - ///backwards compatibility int getUserConstraintType() const { - return m_userConstraintType ; + return m_userConstraintType; } - void setUserConstraintType(int userConstraintType) + void setUserConstraintType(int userConstraintType) { m_userConstraintType = userConstraintType; }; - void setUserConstraintId(int uid) + void setUserConstraintId(int uid) { m_userConstraintId = uid; } @@ -213,22 +200,19 @@ public: { return m_userConstraintId; } - }; class btDefaultVehicleRaycaster : public btVehicleRaycaster { - btDynamicsWorld* m_dynamicsWorld; + btDynamicsWorld* m_dynamicsWorld; + public: btDefaultVehicleRaycaster(btDynamicsWorld* world) - :m_dynamicsWorld(world) + : m_dynamicsWorld(world) { } - virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result); - + virtual void* castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result); }; - -#endif //BT_RAYCASTVEHICLE_H - +#endif //BT_RAYCASTVEHICLE_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Vehicle/btVehicleRaycaster.h b/Engine/lib/bullet/src/BulletDynamics/Vehicle/btVehicleRaycaster.h index 3cc909c65..2c44ce546 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Vehicle/btVehicleRaycaster.h +++ b/Engine/lib/bullet/src/BulletDynamics/Vehicle/btVehicleRaycaster.h @@ -16,20 +16,18 @@ /// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting struct btVehicleRaycaster { -virtual ~btVehicleRaycaster() -{ -} + virtual ~btVehicleRaycaster() + { + } struct btVehicleRaycasterResult { - btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){}; - btVector3 m_hitPointInWorld; - btVector3 m_hitNormalInWorld; - btScalar m_distFraction; + btVehicleRaycasterResult() : m_distFraction(btScalar(-1.)){}; + btVector3 m_hitPointInWorld; + btVector3 m_hitNormalInWorld; + btScalar m_distFraction; }; - virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0; - + virtual void* castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result) = 0; }; -#endif //BT_VEHICLE_RAYCASTER_H - +#endif //BT_VEHICLE_RAYCASTER_H diff --git a/Engine/lib/bullet/src/BulletDynamics/Vehicle/btWheelInfo.cpp b/Engine/lib/bullet/src/BulletDynamics/Vehicle/btWheelInfo.cpp index ef93c16ff..d5c12f223 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Vehicle/btWheelInfo.cpp +++ b/Engine/lib/bullet/src/BulletDynamics/Vehicle/btWheelInfo.cpp @@ -9,30 +9,26 @@ * It is provided "as is" without express or implied warranty. */ #include "btWheelInfo.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity - +#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity btScalar btWheelInfo::getSuspensionRestLength() const { - return m_suspensionRestLength1; - } -void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo) +void btWheelInfo::updateWheel(const btRigidBody& chassis, RaycastInfo& raycastInfo) { (void)raycastInfo; - if (m_raycastInfo.m_isInContact) { - btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); - btVector3 chassis_velocity_at_contactPoint; + btScalar project = m_raycastInfo.m_contactNormalWS.dot(m_raycastInfo.m_wheelDirectionWS); + btVector3 chassis_velocity_at_contactPoint; btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); - chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); - btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); - if ( project >= btScalar(-0.1)) + chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint(relpos); + btScalar projVel = m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint); + if (project >= btScalar(-0.1)) { m_suspensionRelativeVelocity = btScalar(0.0); m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); @@ -43,10 +39,9 @@ void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInf m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; } - } - else // Not in contact : position wheel in a nice (rest length) position + else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); m_suspensionRelativeVelocity = btScalar(0.0); diff --git a/Engine/lib/bullet/src/BulletDynamics/Vehicle/btWheelInfo.h b/Engine/lib/bullet/src/BulletDynamics/Vehicle/btWheelInfo.h index f991a57b6..af88b8ff8 100644 --- a/Engine/lib/bullet/src/BulletDynamics/Vehicle/btWheelInfo.h +++ b/Engine/lib/bullet/src/BulletDynamics/Vehicle/btWheelInfo.h @@ -18,20 +18,19 @@ class btRigidBody; struct btWheelInfoConstructionInfo { - btVector3 m_chassisConnectionCS; - btVector3 m_wheelDirectionCS; - btVector3 m_wheelAxleCS; - btScalar m_suspensionRestLength; - btScalar m_maxSuspensionTravelCm; - btScalar m_wheelRadius; - - btScalar m_suspensionStiffness; - btScalar m_wheelsDampingCompression; - btScalar m_wheelsDampingRelaxation; - btScalar m_frictionSlip; - btScalar m_maxSuspensionForce; + btVector3 m_chassisConnectionCS; + btVector3 m_wheelDirectionCS; + btVector3 m_wheelAxleCS; + btScalar m_suspensionRestLength; + btScalar m_maxSuspensionTravelCm; + btScalar m_wheelRadius; + + btScalar m_suspensionStiffness; + btScalar m_wheelsDampingCompression; + btScalar m_wheelsDampingRelaxation; + btScalar m_frictionSlip; + btScalar m_maxSuspensionForce; bool m_bIsFrontWheel; - }; /// btWheelInfo contains information per wheel about friction and suspension. @@ -40,51 +39,50 @@ struct btWheelInfo struct RaycastInfo { //set by raycaster - btVector3 m_contactNormalWS;//contactnormal - btVector3 m_contactPointWS;//raycast hitpoint - btScalar m_suspensionLength; - btVector3 m_hardPointWS;//raycast starting point - btVector3 m_wheelDirectionWS; //direction in worldspace - btVector3 m_wheelAxleWS; // axle in worldspace - bool m_isInContact; - void* m_groundObject; //could be general void* ptr + btVector3 m_contactNormalWS; //contactnormal + btVector3 m_contactPointWS; //raycast hitpoint + btScalar m_suspensionLength; + btVector3 m_hardPointWS; //raycast starting point + btVector3 m_wheelDirectionWS; //direction in worldspace + btVector3 m_wheelAxleWS; // axle in worldspace + bool m_isInContact; + void* m_groundObject; //could be general void* ptr }; - RaycastInfo m_raycastInfo; + RaycastInfo m_raycastInfo; - btTransform m_worldTransform; - - btVector3 m_chassisConnectionPointCS; //const - btVector3 m_wheelDirectionCS;//const - btVector3 m_wheelAxleCS; // const or modified by steering - btScalar m_suspensionRestLength1;//const - btScalar m_maxSuspensionTravelCm; + btTransform m_worldTransform; + + btVector3 m_chassisConnectionPointCS; //const + btVector3 m_wheelDirectionCS; //const + btVector3 m_wheelAxleCS; // const or modified by steering + btScalar m_suspensionRestLength1; //const + btScalar m_maxSuspensionTravelCm; btScalar getSuspensionRestLength() const; - btScalar m_wheelsRadius;//const - btScalar m_suspensionStiffness;//const - btScalar m_wheelsDampingCompression;//const - btScalar m_wheelsDampingRelaxation;//const - btScalar m_frictionSlip; - btScalar m_steering; - btScalar m_rotation; - btScalar m_deltaRotation; - btScalar m_rollInfluence; - btScalar m_maxSuspensionForce; + btScalar m_wheelsRadius; //const + btScalar m_suspensionStiffness; //const + btScalar m_wheelsDampingCompression; //const + btScalar m_wheelsDampingRelaxation; //const + btScalar m_frictionSlip; + btScalar m_steering; + btScalar m_rotation; + btScalar m_deltaRotation; + btScalar m_rollInfluence; + btScalar m_maxSuspensionForce; - btScalar m_engineForce; + btScalar m_engineForce; + + btScalar m_brake; - btScalar m_brake; - bool m_bIsFrontWheel; - - void* m_clientInfo;//can be used to store pointer to sync transforms... + + void* m_clientInfo; //can be used to store pointer to sync transforms... btWheelInfo() {} btWheelInfo(btWheelInfoConstructionInfo& ci) { - m_suspensionRestLength1 = ci.m_suspensionRestLength; m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; @@ -104,18 +102,15 @@ struct btWheelInfo m_rollInfluence = btScalar(0.1); m_bIsFrontWheel = ci.m_bIsFrontWheel; m_maxSuspensionForce = ci.m_maxSuspensionForce; - } - void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); + void updateWheel(const btRigidBody& chassis, RaycastInfo& raycastInfo); - btScalar m_clippedInvContactDotSuspension; - btScalar m_suspensionRelativeVelocity; + btScalar m_clippedInvContactDotSuspension; + btScalar m_suspensionRelativeVelocity; //calculated by suspension - btScalar m_wheelsSuspensionForce; - btScalar m_skidInfo; - + btScalar m_wheelsSuspensionForce; + btScalar m_skidInfo; }; -#endif //BT_WHEEL_INFO_H - +#endif //BT_WHEEL_INFO_H diff --git a/Engine/lib/bullet/src/BulletDynamics/premake4.lua b/Engine/lib/bullet/src/BulletDynamics/premake4.lua index 32414dce3..fdaf0513d 100644 --- a/Engine/lib/bullet/src/BulletDynamics/premake4.lua +++ b/Engine/lib/bullet/src/BulletDynamics/premake4.lua @@ -3,6 +3,9 @@ includedirs { "..", } + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "Dynamics/*.cpp", "Dynamics/*.h", diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/IDConfig.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/IDConfig.hpp index ebb10e7a1..b662b8015 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/IDConfig.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/IDConfig.hpp @@ -32,10 +32,10 @@ #define BT_ID_POW(x, y) btPow(x, y) #define BT_ID_PI SIMD_PI #ifdef _WIN32 - #define BT_ID_SNPRINTF _snprintf +#define BT_ID_SNPRINTF _snprintf #else - #define BT_ID_SNPRINTF snprintf -#endif // +#define BT_ID_SNPRINTF snprintf +#endif // #endif // error messages #include "IDErrorMessages.hpp" @@ -52,8 +52,8 @@ #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) +#define BT_ID_MAX(a, b) std::max(a, b) +#define BT_ID_MIN(a, b) std::min(a, b) #else #define btInverseDynamics btInverseDynamicsBullet3 @@ -62,8 +62,8 @@ #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) +#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 @@ -71,31 +71,31 @@ typedef btScalar idScalar; #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 { +struct idArray +{ typedef b3AlignedObjectArray type; }; typedef int idArrayIdx; #define ID_DECLARE_ALIGNED_ALLOCATOR() B3_DECLARE_ALIGNED_ALLOCATOR() -#else // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 +#else // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 #include "LinearMath/btAlignedObjectArray.h" template -struct idArray { +struct idArray +{ typedef btAlignedObjectArray type; }; typedef int idArrayIdx; #define ID_DECLARE_ALIGNED_ALLOCATOR() BT_DECLARE_ALIGNED_ALLOCATOR() -#endif // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 - +#endif // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 // use bullet's allocator functions #define idMalloc btAllocFunc diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/IDConfigBuiltin.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/IDConfigBuiltin.hpp index 130c19c6d..639236792 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/IDConfigBuiltin.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/IDConfigBuiltin.hpp @@ -14,7 +14,8 @@ typedef float idScalar; // template // using idArray = std::vector; template -struct idArray { +struct idArray +{ typedef std::vector type; }; typedef std::vector::size_type idArrayIdx; @@ -23,14 +24,14 @@ typedef std::vector::size_type idArrayIdx; #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; } \ +#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" diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/IDConfigEigen.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/IDConfigEigen.hpp index cbd7e8a9c..cfb308ee5 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/IDConfigEigen.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/IDConfigEigen.hpp @@ -15,7 +15,8 @@ typedef float idScalar; // template // using idArray = std::vector; template -struct idArray { +struct idArray +{ typedef std::vector type; }; typedef std::vector::size_type idArrayIdx; diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/IDErrorMessages.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/IDErrorMessages.hpp index 1dc22f860..5a98f0149 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/IDErrorMessages.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/IDErrorMessages.hpp @@ -7,22 +7,24 @@ #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 bt_id_error_message(...) b3Error(__VA_ARGS__) +#define bt_id_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__); \ +#define bt_id_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__); \ +#define bt_id_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 diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/IDMath.cpp b/Engine/lib/bullet/src/BulletInverseDynamics/IDMath.cpp index 99fe20e49..2f120ed48 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/IDMath.cpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/IDMath.cpp @@ -3,25 +3,30 @@ #include #include -namespace btInverseDynamics { +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) { +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++) { +void setZero(vecx &v) +{ + for (int i = 0; i < v.size(); i++) + { v(i) = 0; } } -void setZero(mat33 &m) { +void setZero(mat33 &m) +{ m(0, 0) = 0; m(0, 1) = 0; m(0, 2) = 0; @@ -33,7 +38,8 @@ void setZero(mat33 &m) { m(2, 2) = 0; } -void skew(vec3& v, mat33* result) { +void skew(vec3 &v, mat33 *result) +{ (*result)(0, 0) = 0.0; (*result)(0, 1) = -v(2); (*result)(0, 2) = v(1); @@ -45,22 +51,28 @@ void skew(vec3& v, mat33* result) { (*result)(2, 2) = 0.0; } -idScalar maxAbs(const vecx &v) { +idScalar maxAbs(const vecx &v) +{ idScalar result = 0.0; - for (int i = 0; i < v.size(); i++) { + for (int i = 0; i < v.size(); i++) + { const idScalar tmp = BT_ID_FABS(v(i)); - if (tmp > result) { + if (tmp > result) + { result = tmp; } } return result; } -idScalar maxAbs(const vec3 &v) { +idScalar maxAbs(const vec3 &v) +{ idScalar result = 0.0; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { const idScalar tmp = BT_ID_FABS(v(i)); - if (tmp > result) { + if (tmp > result) + { result = tmp; } } @@ -68,60 +80,75 @@ idScalar maxAbs(const vec3 &v) { } #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; +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(); - } +void mul(const mat33 &a, const mat3x &b, mat3x *result) +{ + if (b.cols() != result->cols()) + { + bt_id_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); - } + 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 add(const mat3x &a, const mat3x &b, mat3x *result) +{ + if (a.cols() != b.cols()) + { + bt_id_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); - } - } +void sub(const mat3x &a, const mat3x &b, mat3x *result) +{ + if (a.cols() != b.cols()) + { + bt_id_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 transformX(const idScalar &alpha) +{ mat33 T; const idScalar cos_alpha = BT_ID_COS(alpha); const idScalar sin_alpha = BT_ID_SIN(alpha); @@ -143,7 +170,8 @@ mat33 transformX(const idScalar &alpha) { return T; } -mat33 transformY(const idScalar &beta) { +mat33 transformY(const idScalar &beta) +{ mat33 T; const idScalar cos_beta = BT_ID_COS(beta); const idScalar sin_beta = BT_ID_SIN(beta); @@ -165,7 +193,8 @@ mat33 transformY(const idScalar &beta) { return T; } -mat33 transformZ(const idScalar &gamma) { +mat33 transformZ(const idScalar &gamma) +{ mat33 T; const idScalar cos_gamma = BT_ID_COS(gamma); const idScalar sin_gamma = BT_ID_SIN(gamma); @@ -187,7 +216,8 @@ mat33 transformZ(const idScalar &gamma) { return T; } -mat33 tildeOperator(const vec3 &v) { +mat33 tildeOperator(const vec3 &v) +{ mat33 m; m(0, 0) = 0.0; m(0, 1) = -v(2); @@ -201,7 +231,8 @@ mat33 tildeOperator(const vec3 &v) { return m; } -void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) { +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); @@ -224,7 +255,8 @@ void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec (*T)(2, 2) = ca; } -void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) { +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; @@ -246,192 +278,233 @@ void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) (*T)(2, 2) = z * z * one_m_c + c; } -bool isPositiveDefinite(const mat33 &m) { +bool isPositiveDefinite(const mat33 &m) +{ // test if all upper left determinants are positive - if (m(0, 0) <= 0) { // upper 1x1 + 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 + 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) { + 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) { +bool isPositiveSemiDefinite(const mat33 &m) +{ // test if all upper left determinants are positive - if (m(0, 0) < 0) { // upper 1x1 + 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 + 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) { + 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) { +bool isPositiveSemiDefiniteFuzzy(const mat33 &m) +{ // test if all upper left determinants are positive - if (m(0, 0) < -kIsZero) { // upper 1x1 + 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 + 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) { + 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) { +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) { +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)); + if (!isPositiveSemiDefiniteFuzzy(I)) + { + bt_id_error_message( + "invalid inertia matrix for body %d, not positive definite " + "(fixed joint)\n", + index); + bt_id_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)); + if (!has_fixed_joint) + { + if (I(0, 0) + I(1, 1) < I(2, 2)) + { + bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + bt_id_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)); + if (I(0, 0) + I(1, 1) < I(2, 2)) + { + bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + bt_id_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)); + if (I(1, 1) + I(2, 2) < I(0, 0)) + { + bt_id_error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index); + bt_id_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)); + for (int i = 0; i < 3; i++) + { + if (I(i, i) < 0) + { // accept zero + bt_id_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)); + if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) + { + bt_id_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)); + if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) + { + bt_id_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)); + if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) + { + bt_id_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)) +bool isValidTransformMatrix(const mat33 &m) +{ +#define print_mat(x) \ + bt_id_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++) { + 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); + if (length_minus_1 > kAxisLengthEpsilon) + { + bt_id_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"); + if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) + { + bt_id_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"); + if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) + { + bt_id_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"); + if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) + { + bt_id_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"); + if (determinant(m) <= 0) + { + bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n"); print_mat(m); return false; } return true; } -bool isUnitVector(const vec3 &vector) { +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 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; } -} +} // namespace btInverseDynamics diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/IDMath.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/IDMath.hpp index b355474d4..40bee5302 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/IDMath.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/IDMath.hpp @@ -5,7 +5,8 @@ #define IDMATH_HPP_ #include "IDConfig.hpp" -namespace btInverseDynamics { +namespace btInverseDynamics +{ /// set all elements to zero void setZero(vec3& v); /// set all elements to zero @@ -23,11 +24,11 @@ idScalar maxAbs(const vec3& v); #if (defined BT_ID_HAVE_MAT3X) idScalar maxAbsMat3x(const mat3x& m); -void setZero(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); +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 @@ -94,6 +95,6 @@ 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); -} +vec3 rpyFromMatrix(const mat33& rot); +} // namespace btInverseDynamics #endif // IDMATH_HPP_ diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/MultiBodyTree.cpp b/Engine/lib/bullet/src/BulletInverseDynamics/MultiBodyTree.cpp index c67588d49..9326b0d09 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/MultiBodyTree.cpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -8,69 +8,84 @@ #include "details/MultiBodyTreeImpl.hpp" #include "details/MultiBodyTreeInitCache.hpp" -namespace btInverseDynamics { - +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(0x0) +{ m_init_cache = new InitCache(); } -MultiBodyTree::~MultiBodyTree() { +MultiBodyTree::~MultiBodyTree() +{ delete m_impl; delete m_init_cache; } -void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) { +void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) +{ m_accept_invalid_mass_parameters = flag; } -bool MultiBodyTree::getAcceptInvalidMassProperties() const { +bool MultiBodyTree::getAcceptInvalidMassProperties() const +{ return m_accept_invalid_mass_parameters; } -int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const { +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 { +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 { +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 { +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 { +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 { +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 { +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 { +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::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::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); +int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3 *axis) const +{ + return m_impl->getBodyAxisOfMotion(body_index, axis); } void MultiBodyTree::printTree() { m_impl->printTree(); } @@ -81,13 +96,16 @@ 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"); + vecx *joint_forces) +{ + if (false == m_is_finalized) + { + bt_id_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"); + if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) + { + bt_id_error_message("error in inverse dynamics calculation\n"); return -1; } return 0; @@ -95,142 +113,165 @@ int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const 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"); + const bool set_lower_triangular_matrix, matxx *mass_matrix) +{ + if (false == m_is_finalized) + { + bt_id_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"); + set_lower_triangular_matrix, mass_matrix)) + { + bt_id_error_message("error in mass matrix calculation\n"); return -1; } return 0; } -int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) { +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) + { + bt_id_error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, u, dot_u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) + { + bt_id_error_message("error in kinematics calculation\n"); + return -1; + } -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; + 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"); +int MultiBodyTree::calculatePositionKinematics(const vecx &q) +{ + if (false == m_is_finalized) + { + bt_id_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"); + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) + { + bt_id_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"); +int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx &q, const vecx &u) +{ + if (false == m_is_finalized) + { + bt_id_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"); + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) + { + bt_id_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, const vecx &u) +{ + if (false == m_is_finalized) + { + bt_id_error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateJacobians(q, u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) + { + bt_id_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::calculateJacobians(const vecx &q) +{ + if (false == m_is_finalized) + { + bt_id_error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateJacobians(q, q, + MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) + { + bt_id_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::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::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::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); +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); + const int user_int, void *user_ptr) +{ + if (body_index < 0) + { + bt_id_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) { + switch (joint_type) + { case REVOLUTE: case PRISMATIC: // check if axis is unit vector - if (!isUnitVector(body_axis_of_motion)) { - warning_message( + if (!isUnitVector(body_axis_of_motion)) + { + bt_id_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); + 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())) + { + bt_id_error_message("axis of motion vector too short (%e)\n", length); return -1; } body_axis_of_motion = (1.0 / length) * body_axis_of_motion; @@ -240,29 +281,36 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ break; case FLOATING: break; + case SPHERICAL: + break; default: - error_message("unknown joint type %d\n", joint_type); + bt_id_error_message("unknown joint type %d\n", joint_type); return -1; } // sanity check for mass properties. Zero mass is OK. - if (mass < 0) { + 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) { + bt_id_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)) { + 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) { + if (!m_accept_invalid_mass_parameters) + { return -1; } } - if (!isValidTransformMatrix(body_T_parent_ref)) { + if (!isValidTransformMatrix(body_T_parent_ref)) + { return -1; } @@ -271,52 +319,63 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ body_I_body, user_int, user_ptr); } -int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const { +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 { +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 { +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) { +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) { +int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) +{ return m_impl->setUserPtr(body_index, user_ptr); } -int MultiBodyTree::finalize() { +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; - } + if (num_dofs < 0) + { + bt_id_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()) { + 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++) { + for (int index = 0; index < num_bodies; index++) + { InertiaData inertia; JointData joint; - if (-1 == m_init_cache->getInertiaData(index, &inertia)) { + if (-1 == m_init_cache->getInertiaData(index, &inertia)) + { return -1; } - if (-1 == m_init_cache->getJointData(index, &joint)) { + if (-1 == m_init_cache->getJointData(index, &joint)) + { return -1; } @@ -331,9 +390,30 @@ int MultiBodyTree::finalize() { 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) { + 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); @@ -359,6 +439,16 @@ int MultiBodyTree::finalize() { rigid_body.m_Jac_JT(1) = 0.0; rigid_body.m_Jac_JT(2) = 0.0; break; + case SPHERICAL: + // 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; case FLOATING: // NOTE/TODO: this is not really correct. // the Jacobians should be 3x3 matrices here ! @@ -370,14 +460,15 @@ int MultiBodyTree::finalize() { rigid_body.m_Jac_JT(2) = 0.0; break; default: - error_message("unsupported joint type %d\n", rigid_body.m_joint_type); + bt_id_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"); + if (-1 == m_impl->generateIndexSets()) + { + bt_id_error_message("generating index sets\n"); return -1; } @@ -392,54 +483,66 @@ int MultiBodyTree::finalize() { return 0; } -int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) { +int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) +{ return m_impl->setGravityInWorldFrame(gravity); } -int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const { +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 { +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 { +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) { +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) { +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) { +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 { +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 { +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 { +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) { +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) { +int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) +{ return m_impl->addUserMoment(body_index, body_moment); } -} +} // namespace btInverseDynamics diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/MultiBodyTree.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/MultiBodyTree.hpp index d235aa6e7..7b852f976 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/MultiBodyTree.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/MultiBodyTree.hpp @@ -4,10 +4,11 @@ #include "IDConfig.hpp" #include "IDMath.hpp" -namespace btInverseDynamics { - +namespace btInverseDynamics +{ /// Enumeration of supported joint types -enum JointType { +enum JointType +{ /// no degree of freedom, moves with parent FIXED = 0, /// one rotational degree of freedom relative to parent @@ -15,7 +16,9 @@ enum JointType { /// one translational degree of freedom relative to parent PRISMATIC, /// six degrees of freedom relative to parent - FLOATING + FLOATING, + /// three degrees of freedom, relative to parent + SPHERICAL }; /// Interface class for calculating inverse dynamics for tree structured @@ -30,12 +33,14 @@ enum JointType { /// - PRISMATIC: displacement [m] /// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] /// (in that order) +/// - SPHERICAL: Euler x-y-z angles [rad] /// 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] +// - SPHERICAL: angular velocity [rad/s] /// /// The q and u vectors are obtained by stacking contributions of all bodies in one /// vector in the order of body indices. @@ -46,12 +51,13 @@ enum JointType { /// - PRISMATIC: force [N], along joint axis /// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame /// (in that order) -/// +/// - SPHERICAL: moment vector [Nm] /// TODO - force element interface (friction, springs, dampers, etc) /// - gears and motor inertia -class MultiBodyTree { +class MultiBodyTree +{ public: - ID_DECLARE_ALIGNED_ALLOCATOR(); + ID_DECLARE_ALIGNED_ALLOCATOR(); /// The contructor. /// Initialization & allocation is via addBody and buildSystem calls. MultiBodyTree(); @@ -119,9 +125,9 @@ public: /// print tree data to stdout void printTreeData(); /// Calculate joint forces for given generalized state & derivatives. - /// This also updates kinematic terms computed in calculateKinematics. - /// If gravity is not set to zero, acceleration terms will contain - /// gravitational acceleration. + /// This also updates kinematic terms computed in calculateKinematics. + /// If gravity is not set to zero, acceleration terms will contain + /// gravitational acceleration. /// @param q generalized coordinates /// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u) /// @param dot_u time derivative of u @@ -152,30 +158,28 @@ public: /// @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); + /// 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 - + /// 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 @@ -231,15 +235,15 @@ public: 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 + // 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 @@ -256,21 +260,21 @@ public: /// @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) + /// 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; + 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; + 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; + 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 diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/details/IDEigenInterface.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/details/IDEigenInterface.hpp index 836395cea..fe4f10251 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/details/IDEigenInterface.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/details/IDEigenInterface.hpp @@ -1,8 +1,8 @@ #ifndef INVDYNEIGENINTERFACE_HPP_ #define INVDYNEIGENINTERFACE_HPP_ #include "../IDConfig.hpp" -namespace btInverseDynamics { - +namespace btInverseDynamics +{ #define BT_ID_HAVE_MAT3X #ifdef BT_USE_DOUBLE_PRECISION @@ -19,18 +19,21 @@ typedef Eigen::Matrix m typedef Eigen::Matrix mat3x; #endif -inline void resize(mat3x &m, Eigen::Index size) { - m.resize(3, size); - m.setZero(); +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 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; +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x *m) +{ + (*m)(row, col) = val; } -} +} // namespace btInverseDynamics #endif // INVDYNEIGENINTERFACE_HPP_ diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp index 5bb4a33bd..0c398a372 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -10,32 +10,37 @@ #include "../../LinearMath/btMatrixX.h" #define BT_ID_HAVE_MAT3X -namespace btInverseDynamics { +namespace btInverseDynamics +{ class vec3; class vecx; class mat33; typedef btMatrixX matxx; -class vec3 : public btVector3 { +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) { + const vec3& operator=(const btVector3& rhs) + { *static_cast(this) = rhs; return *this; } }; -class mat33 : public btMatrix3x3 { +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) { + const mat33& operator=(const btMatrix3x3& rhs) + { *static_cast(this) = rhs; return *this; } @@ -47,11 +52,13 @@ 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 { +class vecx : public btVectorX +{ public: - vecx(int size) : btVectorX(size) {} - const vecx& operator=(const btVectorX& rhs) { - *static_cast(this) = rhs; + vecx(int size) : btVectorX(size) {} + const vecx& operator=(const btVectorX& rhs) + { + *static_cast*>(this) = rhs; return *this; } @@ -66,43 +73,53 @@ public: friend vecx operator/(const vecx& a, const idScalar& s); }; -inline 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++) { + 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) { +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()); + if (a.size() != b.size()) + { + bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + 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) { +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()); + if (a.size() != b.size()) + { + bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + 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) { +inline vecx operator/(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result(i) = a(i) / s; } @@ -110,63 +127,76 @@ inline vecx operator/(const vecx& a, const idScalar& s) { } // use btMatrixX to implement 3xX matrix -class mat3x : public matxx { +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(); + 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) + { + bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); + abort(); + } + for (int i = 0; i < rows(); i++) + { + for (int k = 0; k < cols(); k++) + { + setElem(i, k, rhs(i, k)); + } + } + } + void setZero() + { + matxx::setZero(); } - for(int i=0;isetElem(row, col, val); +inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m) +{ + m->setElem(row, col, val); } -inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){ - m->setElem(row, col, val); +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m) +{ + m->setElem(row, col, val); } -} +} // namespace btInverseDynamics #endif // IDLINEARMATHINTERFACE_HPP_ diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/details/IDMatVec.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/details/IDMatVec.hpp index 4d3f6c87e..1c786095e 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/details/IDMatVec.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/details/IDMatVec.hpp @@ -7,7 +7,8 @@ #include "../IDConfig.hpp" #define BT_ID_HAVE_MAT3X -namespace btInverseDynamics { +namespace btInverseDynamics +{ class vec3; class vecx; class mat33; @@ -17,7 +18,8 @@ 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 { +class vec3 +{ public: idScalar& operator()(int i) { return m_data[i]; } const idScalar& operator()(int i) const { return m_data[i]; } @@ -40,7 +42,8 @@ private: idScalar m_data[3]; }; -class mat33 { +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]; } @@ -62,9 +65,11 @@ private: idScalar m_data[9]; }; -class vecx { +class vecx +{ public: - vecx(int size) : m_size(size) { + vecx(int size) : m_size(size) + { m_data = static_cast(idMalloc(sizeof(idScalar) * size)); } ~vecx() { idFree(m_data); } @@ -85,14 +90,17 @@ private: idScalar* m_data; }; -class matxx { +class matxx +{ public: - matxx() { - m_data = 0x0; - m_cols=0; - m_rows=0; - } - matxx(int rows, int cols) : m_rows(rows), m_cols(cols) { + 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); } @@ -107,69 +115,86 @@ private: idScalar* m_data; }; -class mat3x { +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(); + 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) + { + bt_id_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]; + } } - 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 + ~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; + 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(); +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) { +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 { +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]; @@ -178,17 +203,21 @@ inline vec3 vec3::cross(const vec3& b) const { return result; } -inline idScalar vec3::dot(const vec3& b) const { +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) { +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 { +inline mat33 mat33::transpose() const +{ mat33 result; result.m_data[0] = m_data[0]; result.m_data[1] = m_data[3]; @@ -203,7 +232,8 @@ inline mat33 mat33::transpose() const { return result; } -inline mat33 operator*(const mat33& a, const mat33& b) { +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]; @@ -227,22 +257,27 @@ inline mat33 operator*(const mat33& a, const mat33& b) { return result; } -inline const mat33& mat33::operator+=(const mat33& b) { - for (int i = 0; i < 9; i++) { +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++) { +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) { +inline vec3 operator*(const mat33& a, const vec3& b) +{ vec3 result; result.m_data[0] = @@ -255,23 +290,29 @@ inline vec3 operator*(const mat33& a, const vec3& b) { return result; } -inline const vec3& vec3::operator+=(const vec3& b) { - for (int i = 0; i < 3; i++) { +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++) { +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) { +inline mat33 operator*(const mat33& a, const idScalar& s) +{ mat33 result; - for (int i = 0; i < 9; i++) { + for (int i = 0; i < 9; i++) + { result.m_data[i] = a.m_data[i] * s; } return result; @@ -279,137 +320,170 @@ inline mat33 operator*(const mat33& a, const idScalar& s) { inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } -inline vec3 operator*(const vec3& a, const idScalar& s) { +inline vec3 operator*(const vec3& a, const idScalar& s) +{ vec3 result; - for (int i = 0; i < 3; i++) { + 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) { +inline mat33 operator+(const mat33& a, const mat33& b) +{ mat33 result; - for (int i = 0; i < 9; i++) { + 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) { +inline vec3 operator+(const vec3& a, const vec3& b) +{ vec3 result; - for (int i = 0; i < 3; i++) { + 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) { +inline mat33 operator-(const mat33& a, const mat33& b) +{ mat33 result; - for (int i = 0; i < 9; i++) { + 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) { +inline vec3 operator-(const vec3& a, const vec3& b) +{ vec3 result; - for (int i = 0; i < 3; i++) { + 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) { +inline mat33 operator/(const mat33& a, const idScalar& s) +{ mat33 result; - for (int i = 0; i < 9; i++) { + 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) { +inline vec3 operator/(const vec3& a, const idScalar& s) +{ vec3 result; - for (int i = 0; i < 3; i++) { + 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()); +inline const vecx& vecx::operator=(const vecx& rhs) +{ + if (size() != rhs.size()) + { + bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); abort(); } - if (&rhs != this) { + if (&rhs != this) + { memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); } return *this; } -inline 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++) { + 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) { +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()); + if (a.size() != b.size()) + { + bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + 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) { +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()); + if (a.size() != b.size()) + { + bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + 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) { +inline vecx operator/(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + 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)); @@ -59,19 +67,22 @@ void MultiBodyTree::MultiBodyImpl::printTreeData() { 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( + "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) { +int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const +{ + switch (type) + { case FIXED: return 0; case REVOLUTE: @@ -79,12 +90,15 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { return 1; case FLOATING: return 6; + case SPHERICAL: + return 3; } - error_message("unknown joint type %d\n", type); + bt_id_error_message("unknown joint type %d\n", type); return 0; } -void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { +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(); @@ -92,7 +106,8 @@ void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { indentation += 2; int count = 0; - for (int i = 0; i < num_children; i++) { + 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, @@ -104,19 +119,23 @@ void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { } } -int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) { +int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) +{ m_world_gravity = gravity; return 0; } -int MultiBodyTree::MultiBodyImpl::generateIndexSets() { +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++) { + 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) { + switch (body.m_joint_type) + { case REVOLUTE: m_body_revolute_list.push_back(i); body.m_q_index = q_index; @@ -135,30 +154,43 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() { body.m_q_index = q_index; q_index += 6; break; + case SPHERICAL: + m_body_spherical_list.push_back(i); + body.m_q_index = q_index; + q_index += 3; + break; default: - error_message("unsupported joint type %d\n", body.m_joint_type); + bt_id_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); + if (q_index != m_num_dofs) + { + bt_id_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++) { + 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)) { + if (parent >= 0 && parent < (static_cast(m_parent_index.size()) - 1)) + { m_child_indices[parent].push_back(child); - } else { - if (-1 == parent) { + } + 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 { + bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); + } + else + { // should never happen - error_message( + bt_id_error_message( "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", child, parent, static_cast(m_parent_index.size())); } @@ -169,11 +201,14 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() { return 0; } -void MultiBodyTree::MultiBodyImpl::calculateStaticData() { +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++) { + for (idArrayIdx i = 0; i < m_body_list.size(); i++) + { RigidBody &body = m_body_list[i]; - switch (body.m_joint_type) { + switch (body.m_joint_type) + { case REVOLUTE: body.m_parent_vel_rel(0) = 0; body.m_parent_vel_rel(1) = 0; @@ -212,41 +247,56 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData() { case FLOATING: // no static data break; + case SPHERICAL: + //todo: review + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + 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; + break; } - // resize & initialize jacobians to zero. + // 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 // + 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) { + 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())); + joint_forces->size() != m_num_dofs) + { + bt_id_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++) { + if (-1 == calculateKinematics(q, u, dot_u, POSITION_VELOCITY_ACCELERATION)) + { + bt_id_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 = @@ -268,14 +318,16 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const // 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--) { + 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++) { + 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; @@ -293,19 +345,22 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const // 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++) { + 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++) { + 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++) { + 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); @@ -316,84 +371,141 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); } + // 4.4 spherical bodies (3-DoF joints) + for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) + { + //todo: review + RigidBody &body = m_body_list[m_body_spherical_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); + } 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())); +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) + { + bt_id_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) + { + bt_id_error_message("invalid type %d\n", type); 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++) { + 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); - } + 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++) { + 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); - } + 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++) { + 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)); + 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); + 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(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_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(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; - } + body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } + } + + for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) + { + //todo: review + RigidBody &body = m_body_list[m_body_spherical_list[i]]; + + mat33 T; + + T = transformX(q(body.m_q_index)) * + transformY(q(body.m_q_index + 1)) * + transformZ(q(body.m_q_index + 2)); + body.m_body_T_parent = T * body.m_body_T_parent_ref; + + body.m_parent_pos_parent_body(0)=0; + body.m_parent_pos_parent_body(1)=0; + body.m_parent_pos_parent_body(2)=0; + + 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 = 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 = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } } // 2. absolute kinematic quantities (vector valued) @@ -410,26 +522,29 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx 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; - } + 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++) { + 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: @@ -439,121 +554,159 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx 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; + 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); - } + 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; + 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); +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(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); + 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; - } + break; + case SPHERICAL: + //todo: review + 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); + 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; - } +int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &u, const KinUpdateType type) +{ + if (q.size() != m_num_dofs || u.size() != m_num_dofs) + { + bt_id_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) + { + bt_id_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]]; + 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); + 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); + 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; + 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) { +static inline void setThreeDoFJacobians(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; + } +} + +static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) +{ + switch (dof) + { // rotational part case 0: Jac_JR(0) = 1; @@ -595,8 +748,10 @@ static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) } } -static inline int jointNumDoFs(const JointType &type) { - switch (type) { +static inline int jointNumDoFs(const JointType &type) +{ + switch (type) + { case FIXED: return 0; case REVOLUTE: @@ -604,9 +759,11 @@ static inline int jointNumDoFs(const JointType &type) { return 1; case FLOATING: return 6; + case SPHERICAL: + return 3; } // this should never happen - error_message("invalid joint type\n"); + bt_id_error_message("invalid joint type\n"); // TODO add configurable abort/crash function abort(); return 0; @@ -615,37 +772,45 @@ static inline int jointNumDoFs(const JointType &type) { 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. + 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())); + mass_matrix->cols() != m_num_dofs) + { + bt_id_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 (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) { + if (update_kinematics) + { // 1. update relative kinematics // 1.1 for revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + 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; @@ -653,7 +818,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool 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++) { + 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 = @@ -661,7 +827,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } // 1.3 fixed joints: nothing to do // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + 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)) * @@ -673,8 +840,28 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; } + + for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) + { + //todo: review + RigidBody &body = m_body_list[m_body_spherical_list[i]]; + + mat33 T; + + T = transformX(q(body.m_q_index)) * + transformY(q(body.m_q_index + 1)) * + transformZ(q(body.m_q_index + 2)); + body.m_body_T_parent = T * body.m_body_T_parent_ref; + + body.m_parent_pos_parent_body(0)=0; + body.m_parent_pos_parent_body(1)=0; + body.m_parent_pos_parent_body(2)=0; + + 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--) { + 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 @@ -682,7 +869,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool 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++) { + 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(); @@ -692,7 +880,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool 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) { + 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 @@ -707,7 +896,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } } - for (int i = m_body_list.size() - 1; i >= 0; i--) { + for (int i = m_body_list.size() - 1; i >= 0; i--) + { const RigidBody &body = m_body_list[i]; // determine DoF-range for body @@ -717,11 +907,18 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool // 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--) { + for (int col = q_index_max; col >= q_index_min; col--) + { // set jacobians for 6-DoF joints - if (FLOATING == body.m_joint_type) { + if (FLOATING == body.m_joint_type) + { setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); } + if (SPHERICAL == body.m_joint_type) + { + //todo: review + setThreeDoFJacobians(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); @@ -732,19 +929,27 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool // 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; + for (int row = col - 1; row >= q_index_min; row--) + { + if (SPHERICAL == body.m_joint_type) + { + //todo: review + setThreeDoFJacobians(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); + } + if (FLOATING == body.m_joint_type) + { + 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); } - 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) { + while (parent_idx >= 0) + { const RigidBody &child_body = m_body_list[child_idx]; const RigidBody &parent_body = m_body_list[parent_idx]; @@ -758,9 +963,16 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool 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--) { + for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) + { + if (SPHERICAL == parent_body.m_joint_type) + { + //todo: review + setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); + } // set jacobians for 6-DoF joints - if (FLOATING == parent_body.m_joint_type) { + 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); @@ -774,10 +986,13 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } } - 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); + 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); } } } @@ -785,76 +1000,91 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } // 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; \ - } \ +#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ + do \ + { \ + if (index < 0 || index >= m_num_bodies) \ + { \ + bt_id_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) { +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 { +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 { +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) { +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) { +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 { +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 { +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) { + 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 { + } + 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 { +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 { +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 { + 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; @@ -862,13 +1092,17 @@ int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, } int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, - vec3 *world_velocity) const { + 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) { + if (body.m_mass > 0) + { com = body.m_body_mass_com / body.m_mass; - } else { + } + else + { com(0) = 0; com(1) = 0; com(2) = 0; @@ -880,149 +1114,173 @@ int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, } int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index, - vec3 *world_dot_omega) const { + 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 { + 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 { +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 { + 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::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::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::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 { +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) { +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) { + 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) { + 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 { +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 { + 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 { + 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++) { +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) { +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) { +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::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::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::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; +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 -} +} // namespace btInverseDynamics diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp index 3efe9d049..eabdbe161 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -8,12 +8,13 @@ #include "../IDConfig.hpp" #include "../MultiBodyTree.hpp" -namespace btInverseDynamics { - +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 { +struct RigidBody +{ ID_DECLARE_ALIGNED_ALLOCATOR(); // 1 Inertial properties /// Mass @@ -112,31 +113,33 @@ struct RigidBody { 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; + /// 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 { +class MultiBodyTree::MultiBodyImpl +{ friend class MultiBodyTree; public: ID_DECLARE_ALIGNED_ALLOCATOR(); - enum KinUpdateType { - POSITION_ONLY, - POSITION_VELOCITY, - POSITION_VELOCITY_ACCELERATION - }; + enum KinUpdateType + { + POSITION_ONLY, + POSITION_VELOCITY, + POSITION_VELOCITY_ACCELERATION + }; /// constructor /// @param num_bodies the number of bodies in the system @@ -150,24 +153,24 @@ public: 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); + /// 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); + /// 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 @@ -190,12 +193,12 @@ public: 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::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 @@ -271,13 +274,15 @@ private: idArray::type m_body_prismatic_list; // Indices of floating joints idArray::type m_body_floating_list; + // Indices of spherical joints + idArray::type m_body_spherical_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; + mat3x m_m3x; #endif }; -} +} // namespace btInverseDynamics #endif diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp index 47b4ab389..a718db051 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -1,12 +1,13 @@ #include "MultiBodyTreeInitCache.hpp" -namespace btInverseDynamics { - -MultiBodyTree::InitCache::InitCache() { +namespace btInverseDynamics +{ +MultiBodyTree::InitCache::InitCache() +{ m_inertias.resize(0); m_joints.resize(0); m_num_dofs = 0; - m_root_index=-1; + m_root_index = -1; } int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index, @@ -15,8 +16,10 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind 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) { + const int user_int, void* user_ptr) +{ + switch (joint_type) + { case REVOLUTE: case PRISMATIC: m_num_dofs += 1; @@ -25,21 +28,26 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind // does not add a degree of freedom // m_num_dofs+=0; break; + case SPHERICAL: + m_num_dofs += 3; + break; case FLOATING: m_num_dofs += 6; break; default: - error_message("unknown joint type %d\n", joint_type); + bt_id_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); + if (-1 == parent_index) + { + if (m_root_index >= 0) + { + bt_id_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; + m_root_index = body_index; } JointData joint; @@ -61,9 +69,11 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind 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"); +int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const +{ + if (index < 0 || index > static_cast(m_inertias.size())) + { + bt_id_error_message("index out of range\n"); return -1; } @@ -71,43 +81,51 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert 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"); +int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const +{ + if (index < 0 || index > static_cast(m_user_int.size())) + { + bt_id_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"); +int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const +{ + if (index < 0 || index > static_cast(m_user_ptr.size())) + { + bt_id_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"); +int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const +{ + if (index < 0 || index > static_cast(m_joints.size())) + { + bt_id_error_message("index out of range\n"); return -1; } *joint = m_joints[index]; return 0; } -int MultiBodyTree::InitCache::buildIndexSets() { +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++) { + 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; } -} +} // namespace btInverseDynamics diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp index 0d2aa4a07..dbdb3ff60 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp +++ b/Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp @@ -5,9 +5,11 @@ #include "../IDMath.hpp" #include "../MultiBodyTree.hpp" -namespace btInverseDynamics { +namespace btInverseDynamics +{ /// Mass properties of a rigid body -struct InertiaData { +struct InertiaData +{ ID_DECLARE_ALIGNED_ALLOCATOR(); /// mass @@ -21,7 +23,8 @@ struct InertiaData { }; /// Joint properties -struct JointData { +struct JointData +{ ID_DECLARE_ALIGNED_ALLOCATOR(); /// type of joint @@ -48,7 +51,8 @@ struct JointData { /// Data structure to store data passed by the user. /// This is used in MultiBodyTree::finalize to build internal data structures. -class MultiBodyTree::InitCache { +class MultiBodyTree::InitCache +{ public: ID_DECLARE_ALIGNED_ALLOCATOR(); /// constructor @@ -105,5 +109,5 @@ private: // index of root body (or -1 if not set) int m_root_index; }; -} +} // namespace btInverseDynamics #endif // MULTIBODYTREEINITCACHE_HPP_ diff --git a/Engine/lib/bullet/src/BulletInverseDynamics/premake4.lua b/Engine/lib/bullet/src/BulletInverseDynamics/premake4.lua index 774e037b3..fdd176b09 100644 --- a/Engine/lib/bullet/src/BulletInverseDynamics/premake4.lua +++ b/Engine/lib/bullet/src/BulletInverseDynamics/premake4.lua @@ -1,6 +1,9 @@ project "BulletInverseDynamics" kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end includedirs { "..", } diff --git a/Engine/lib/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.cpp b/Engine/lib/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.cpp index 9c2040307..8b7ff9abc 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.cpp +++ b/Engine/lib/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.cpp @@ -21,11 +21,10 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCapsuleShape.h" #include "BulletSoftBody/btSoftBody.h" - btDefaultSoftBodySolver::btDefaultSoftBodySolver() { // Initial we will clearly need to update solver constants - // For now this is global for the cloths linked with this solver - we should probably make this body specific + // For now this is global for the cloths linked with this solver - we should probably make this body specific // for performance in future once we understand more clearly when constants need to be updated m_updateSolverConstants = true; } @@ -37,67 +36,65 @@ btDefaultSoftBodySolver::~btDefaultSoftBodySolver() // In this case the data is already in the soft bodies so there is no need for us to do anything void btDefaultSoftBodySolver::copyBackToSoftBodies(bool bMove) { - } -void btDefaultSoftBodySolver::optimize( btAlignedObjectArray< btSoftBody * > &softBodies , bool forceUpdate) +void btDefaultSoftBodySolver::optimize(btAlignedObjectArray &softBodies, bool forceUpdate) { - m_softBodySet.copyFromArray( softBodies ); + m_softBodySet.copyFromArray(softBodies); } -void btDefaultSoftBodySolver::updateSoftBodies( ) +void btDefaultSoftBodySolver::updateSoftBodies() { - for ( int i=0; i < m_softBodySet.size(); i++) + for (int i = 0; i < m_softBodySet.size(); i++) { - btSoftBody* psb=(btSoftBody*)m_softBodySet[i]; + btSoftBody *psb = (btSoftBody *)m_softBodySet[i]; if (psb->isActive()) { - psb->integrateMotion(); + psb->integrateMotion(); } } -} // updateSoftBodies +} // updateSoftBodies bool btDefaultSoftBodySolver::checkInitialized() { return true; } -void btDefaultSoftBodySolver::solveConstraints( float solverdt ) +void btDefaultSoftBodySolver::solveConstraints(float solverdt) { // Solve constraints for non-solver softbodies - for(int i=0; i < m_softBodySet.size(); ++i) + for (int i = 0; i < m_softBodySet.size(); ++i) { - btSoftBody* psb = static_cast(m_softBodySet[i]); + btSoftBody *psb = static_cast(m_softBodySet[i]); if (psb->isActive()) { psb->solveConstraints(); } - } -} // btDefaultSoftBodySolver::solveConstraints + } +} // btDefaultSoftBodySolver::solveConstraints - -void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ) +void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) { // Currently only support CPU output buffers // TODO: check for DX11 buffers. Take all offsets into the same DX11 buffer // and use them together on a single kernel call if possible by setting up a // per-cloth target buffer array for the copy kernel. - if( vertexBuffer->getBufferType() == btVertexBufferDescriptor::CPU_BUFFER ) + if (vertexBuffer->getBufferType() == btVertexBufferDescriptor::CPU_BUFFER) { - const btAlignedObjectArray &clothVertices( softBody->m_nodes ); + const btAlignedObjectArray &clothVertices(softBody->m_nodes); int numVertices = clothVertices.size(); - const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast< btCPUVertexBufferDescriptor* >(vertexBuffer); - float *basePointer = cpuVertexBuffer->getBasePointer(); + const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast(vertexBuffer); + float *basePointer = cpuVertexBuffer->getBasePointer(); - if( vertexBuffer->hasVertexPositions() ) + if (vertexBuffer->hasVertexPositions()) { const int vertexOffset = cpuVertexBuffer->getVertexOffset(); const int vertexStride = cpuVertexBuffer->getVertexStride(); float *vertexPointer = basePointer + vertexOffset; - for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) + for (int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex) { btVector3 position = clothVertices[vertexIndex].m_x; *(vertexPointer + 0) = (float)position.getX(); @@ -106,13 +103,13 @@ void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *cons vertexPointer += vertexStride; } } - if( vertexBuffer->hasNormals() ) + if (vertexBuffer->hasNormals()) { const int normalOffset = cpuVertexBuffer->getNormalOffset(); const int normalStride = cpuVertexBuffer->getNormalStride(); float *normalPointer = basePointer + normalOffset; - for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) + for (int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex) { btVector3 normal = clothVertices[vertexIndex].m_n; *(normalPointer + 0) = (float)normal.getX(); @@ -122,30 +119,28 @@ void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *cons } } } -} // btDefaultSoftBodySolver::copySoftBodyToVertexBuffer +} // btDefaultSoftBodySolver::copySoftBodyToVertexBuffer -void btDefaultSoftBodySolver::processCollision( btSoftBody* softBody, btSoftBody* otherSoftBody) +void btDefaultSoftBodySolver::processCollision(btSoftBody *softBody, btSoftBody *otherSoftBody) { - softBody->defaultCollisionHandler( otherSoftBody); + softBody->defaultCollisionHandler(otherSoftBody); } // For the default solver just leave the soft body to do its collision processing -void btDefaultSoftBodySolver::processCollision( btSoftBody *softBody, const btCollisionObjectWrapper* collisionObjectWrap ) +void btDefaultSoftBodySolver::processCollision(btSoftBody *softBody, const btCollisionObjectWrapper *collisionObjectWrap) { - softBody->defaultCollisionHandler( collisionObjectWrap ); -} // btDefaultSoftBodySolver::processCollision + softBody->defaultCollisionHandler(collisionObjectWrap); +} // btDefaultSoftBodySolver::processCollision - -void btDefaultSoftBodySolver::predictMotion( float timeStep ) +void btDefaultSoftBodySolver::predictMotion(float timeStep) { - for ( int i=0; i < m_softBodySet.size(); ++i) + for (int i = 0; i < m_softBodySet.size(); ++i) { - btSoftBody* psb = m_softBodySet[i]; + btSoftBody *psb = m_softBodySet[i]; if (psb->isActive()) { - psb->predictMotion(timeStep); + psb->predictMotion(timeStep); } } } - diff --git a/Engine/lib/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.h b/Engine/lib/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.h index 1c17ffcbb..50bd73516 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.h @@ -16,25 +16,23 @@ subject to the following restrictions: #ifndef BT_SOFT_BODY_DEFAULT_SOLVER_H #define BT_SOFT_BODY_DEFAULT_SOLVER_H - #include "BulletSoftBody/btSoftBodySolvers.h" #include "btSoftBodySolverVertexBuffer.h" struct btCollisionObjectWrapper; class btDefaultSoftBodySolver : public btSoftBodySolver { -protected: +protected: /** Variable to define whether we need to update solver constants on the next iteration */ bool m_updateSolverConstants; - btAlignedObjectArray< btSoftBody * > m_softBodySet; - + btAlignedObjectArray m_softBodySet; public: btDefaultSoftBodySolver(); - + virtual ~btDefaultSoftBodySolver(); - + virtual SolverTypes getSolverType() const { return DEFAULT_SOLVER; @@ -42,22 +40,21 @@ public: virtual bool checkInitialized(); - virtual void updateSoftBodies( ); + virtual void updateSoftBodies(); - virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies,bool forceUpdate=false ); + virtual void optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false); virtual void copyBackToSoftBodies(bool bMove = true); - virtual void solveConstraints( float solverdt ); + virtual void solveConstraints(float solverdt); - virtual void predictMotion( float solverdt ); + virtual void predictMotion(float solverdt); - virtual void copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ); + virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer); - virtual void processCollision( btSoftBody *, const btCollisionObjectWrapper* ); - - virtual void processCollision( btSoftBody*, btSoftBody* ); + virtual void processCollision(btSoftBody *, const btCollisionObjectWrapper *); + virtual void processCollision(btSoftBody *, btSoftBody *); }; -#endif // #ifndef BT_ACCELERATED_SOFT_BODY_CPU_SOLVER_H +#endif // #ifndef BT_ACCELERATED_SOFT_BODY_CPU_SOLVER_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBody.cpp b/Engine/lib/bullet/src/BulletSoftBody/btSoftBody.cpp index 48efb0d8d..58796a88d 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBody.cpp +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBody.cpp @@ -21,97 +21,94 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" - // -btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m) -:m_softBodySolver(0),m_worldInfo(worldInfo) -{ - /* Init */ +btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) + : m_softBodySolver(0), m_worldInfo(worldInfo) +{ + /* Init */ initDefaults(); - /* Default material */ - Material* pm=appendMaterial(); - pm->m_kLST = 1; - pm->m_kAST = 1; - pm->m_kVST = 1; - pm->m_flags = fMaterial::Default; + /* Default material */ + Material* pm = appendMaterial(); + pm->m_kLST = 1; + pm->m_kAST = 1; + pm->m_kVST = 1; + pm->m_flags = fMaterial::Default; - /* Nodes */ - const btScalar margin=getCollisionShape()->getMargin(); + /* Nodes */ + const btScalar margin = getCollisionShape()->getMargin(); m_nodes.resize(node_count); - for(int i=0,ni=node_count;i0?1/n.m_im:0; - n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); - n.m_material= pm; + n.m_x = x ? *x++ : btVector3(0, 0, 0); + n.m_q = n.m_x; + n.m_im = m ? *m++ : 1; + n.m_im = n.m_im > 0 ? 1 / n.m_im : 0; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n); + n.m_material = pm; } - updateBounds(); - + updateBounds(); } -btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) -:m_worldInfo(worldInfo) +btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) + : m_worldInfo(worldInfo) { initDefaults(); } - -void btSoftBody::initDefaults() +void btSoftBody::initDefaults() { - m_internalType = CO_SOFT_BODY; - m_cfg.aeromodel = eAeroModel::V_Point; - m_cfg.kVCF = 1; - m_cfg.kDG = 0; - m_cfg.kLF = 0; - m_cfg.kDP = 0; - m_cfg.kPR = 0; - m_cfg.kVC = 0; - m_cfg.kDF = (btScalar)0.2; - m_cfg.kMT = 0; - m_cfg.kCHR = (btScalar)1.0; - m_cfg.kKHR = (btScalar)0.1; - m_cfg.kSHR = (btScalar)1.0; - m_cfg.kAHR = (btScalar)0.7; - m_cfg.kSRHR_CL = (btScalar)0.1; - m_cfg.kSKHR_CL = (btScalar)1; - m_cfg.kSSHR_CL = (btScalar)0.5; - m_cfg.kSR_SPLT_CL = (btScalar)0.5; - m_cfg.kSK_SPLT_CL = (btScalar)0.5; - m_cfg.kSS_SPLT_CL = (btScalar)0.5; - m_cfg.maxvolume = (btScalar)1; - m_cfg.timescale = 1; - m_cfg.viterations = 0; - m_cfg.piterations = 1; - m_cfg.diterations = 0; - m_cfg.citerations = 4; - m_cfg.collisions = fCollision::Default; - m_pose.m_bvolume = false; - m_pose.m_bframe = false; - m_pose.m_volume = 0; - m_pose.m_com = btVector3(0,0,0); + m_internalType = CO_SOFT_BODY; + m_cfg.aeromodel = eAeroModel::V_Point; + m_cfg.kVCF = 1; + m_cfg.kDG = 0; + m_cfg.kLF = 0; + m_cfg.kDP = 0; + m_cfg.kPR = 0; + m_cfg.kVC = 0; + m_cfg.kDF = (btScalar)0.2; + m_cfg.kMT = 0; + m_cfg.kCHR = (btScalar)1.0; + m_cfg.kKHR = (btScalar)0.1; + m_cfg.kSHR = (btScalar)1.0; + m_cfg.kAHR = (btScalar)0.7; + m_cfg.kSRHR_CL = (btScalar)0.1; + m_cfg.kSKHR_CL = (btScalar)1; + m_cfg.kSSHR_CL = (btScalar)0.5; + m_cfg.kSR_SPLT_CL = (btScalar)0.5; + m_cfg.kSK_SPLT_CL = (btScalar)0.5; + m_cfg.kSS_SPLT_CL = (btScalar)0.5; + m_cfg.maxvolume = (btScalar)1; + m_cfg.timescale = 1; + m_cfg.viterations = 0; + m_cfg.piterations = 1; + m_cfg.diterations = 0; + m_cfg.citerations = 4; + m_cfg.collisions = fCollision::Default; + m_pose.m_bvolume = false; + m_pose.m_bframe = false; + m_pose.m_volume = 0; + m_pose.m_com = btVector3(0, 0, 0); m_pose.m_rot.setIdentity(); m_pose.m_scl.setIdentity(); - m_tag = 0; - m_timeacc = 0; - m_bUpdateRtCst = true; - m_bounds[0] = btVector3(0,0,0); - m_bounds[1] = btVector3(0,0,0); + m_tag = 0; + m_timeacc = 0; + m_bUpdateRtCst = true; + m_bounds[0] = btVector3(0, 0, 0); + m_bounds[1] = btVector3(0, 0, 0); m_worldTransform.setIdentity(); setSolver(eSolverPresets::Positions); - - /* Collision shape */ + + /* Collision shape */ ///for now, create a collision shape internally m_collisionShape = new btSoftBodyCollisionShape(this); m_collisionShape->setMargin(0.25f); - + m_initialWorldTransform.setIdentity(); - m_windVelocity = btVector3(0,0,0); + m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); } @@ -119,343 +116,361 @@ void btSoftBody::initDefaults() btSoftBody::~btSoftBody() { //for now, delete the internal shape - delete m_collisionShape; + delete m_collisionShape; int i; releaseClusters(); - for(i=0;i0) - *pm=*m_materials[0]; + Material* pm = new (btAlignedAlloc(sizeof(Material), 16)) Material(); + if (m_materials.size() > 0) + *pm = *m_materials[0]; else ZeroInitialize(*pm); m_materials.push_back(pm); - return(pm); + return (pm); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - const btVector4& c, - Node* n0, - Node* n1, - Node* n2, - Node* n3) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + const btVector4& c, + Node* n0, + Node* n1, + Node* n2, + Node* n3) { - Note n; + Note n; ZeroInitialize(n); - n.m_rank = 0; - n.m_text = text; - n.m_offset = o; - n.m_coords[0] = c.x(); - n.m_coords[1] = c.y(); - n.m_coords[2] = c.z(); - n.m_coords[3] = c.w(); - n.m_nodes[0] = n0;n.m_rank+=n0?1:0; - n.m_nodes[1] = n1;n.m_rank+=n1?1:0; - n.m_nodes[2] = n2;n.m_rank+=n2?1:0; - n.m_nodes[3] = n3;n.m_rank+=n3?1:0; + n.m_rank = 0; + n.m_text = text; + n.m_offset = o; + n.m_coords[0] = c.x(); + n.m_coords[1] = c.y(); + n.m_coords[2] = c.z(); + n.m_coords[3] = c.w(); + n.m_nodes[0] = n0; + n.m_rank += n0 ? 1 : 0; + n.m_nodes[1] = n1; + n.m_rank += n1 ? 1 : 0; + n.m_nodes[2] = n2; + n.m_rank += n2 ? 1 : 0; + n.m_nodes[3] = n3; + n.m_rank += n3 ? 1 : 0; m_notes.push_back(n); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Node* feature) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + Node* feature) { - appendNote(text,o,btVector4(1,0,0,0),feature); + appendNote(text, o, btVector4(1, 0, 0, 0), feature); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Link* feature) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + Link* feature) { - static const btScalar w=1/(btScalar)2; - appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0], - feature->m_n[1]); + static const btScalar w = 1 / (btScalar)2; + appendNote(text, o, btVector4(w, w, 0, 0), feature->m_n[0], + feature->m_n[1]); } // -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Face* feature) +void btSoftBody::appendNote(const char* text, + const btVector3& o, + Face* feature) { - static const btScalar w=1/(btScalar)3; - appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0], - feature->m_n[1], - feature->m_n[2]); + static const btScalar w = 1 / (btScalar)3; + appendNote(text, o, btVector4(w, w, w, 0), feature->m_n[0], + feature->m_n[1], + feature->m_n[2]); } // -void btSoftBody::appendNode( const btVector3& x,btScalar m) +void btSoftBody::appendNode(const btVector3& x, btScalar m) { - if(m_nodes.capacity()==m_nodes.size()) + if (m_nodes.capacity() == m_nodes.size()) { pointersToIndices(); - m_nodes.reserve(m_nodes.size()*2+1); + m_nodes.reserve(m_nodes.size() * 2 + 1); indicesToPointers(); } - const btScalar margin=getCollisionShape()->getMargin(); + const btScalar margin = getCollisionShape()->getMargin(); m_nodes.push_back(Node()); - Node& n=m_nodes[m_nodes.size()-1]; + Node& n = m_nodes[m_nodes.size() - 1]; ZeroInitialize(n); - n.m_x = x; - n.m_q = n.m_x; - n.m_im = m>0?1/m:0; - n.m_material = m_materials[0]; - n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); + n.m_x = x; + n.m_q = n.m_x; + n.m_im = m > 0 ? 1 / m : 0; + n.m_material = m_materials[0]; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n); } // -void btSoftBody::appendLink(int model,Material* mat) +void btSoftBody::appendLink(int model, Material* mat) { - Link l; - if(model>=0) - l=m_links[model]; + Link l; + if (model >= 0) + l = m_links[model]; else - { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; } + { + ZeroInitialize(l); + l.m_material = mat ? mat : m_materials[0]; + } m_links.push_back(l); } // -void btSoftBody::appendLink( int node0, - int node1, - Material* mat, - bool bcheckexist) +void btSoftBody::appendLink(int node0, + int node1, + Material* mat, + bool bcheckexist) { - appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist); + appendLink(&m_nodes[node0], &m_nodes[node1], mat, bcheckexist); } // -void btSoftBody::appendLink( Node* node0, - Node* node1, - Material* mat, - bool bcheckexist) +void btSoftBody::appendLink(Node* node0, + Node* node1, + Material* mat, + bool bcheckexist) { - if((!bcheckexist)||(!checkLink(node0,node1))) + if ((!bcheckexist) || (!checkLink(node0, node1))) { - appendLink(-1,mat); - Link& l=m_links[m_links.size()-1]; - l.m_n[0] = node0; - l.m_n[1] = node1; - l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); - m_bUpdateRtCst=true; + appendLink(-1, mat); + Link& l = m_links[m_links.size() - 1]; + l.m_n[0] = node0; + l.m_n[1] = node1; + l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length(); + m_bUpdateRtCst = true; } } // -void btSoftBody::appendFace(int model,Material* mat) +void btSoftBody::appendFace(int model, Material* mat) { - Face f; - if(model>=0) - { f=m_faces[model]; } + Face f; + if (model >= 0) + { + f = m_faces[model]; + } else - { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; } + { + ZeroInitialize(f); + f.m_material = mat ? mat : m_materials[0]; + } m_faces.push_back(f); } // -void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) +void btSoftBody::appendFace(int node0, int node1, int node2, Material* mat) { - if (node0==node1) + if (node0 == node1) return; - if (node1==node2) + if (node1 == node2) return; - if (node2==node0) + if (node2 == node0) return; - appendFace(-1,mat); - Face& f=m_faces[m_faces.size()-1]; - btAssert(node0!=node1); - btAssert(node1!=node2); - btAssert(node2!=node0); - f.m_n[0] = &m_nodes[node0]; - f.m_n[1] = &m_nodes[node1]; - f.m_n[2] = &m_nodes[node2]; - f.m_ra = AreaOf( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); - m_bUpdateRtCst=true; + appendFace(-1, mat); + Face& f = m_faces[m_faces.size() - 1]; + btAssert(node0 != node1); + btAssert(node1 != node2); + btAssert(node2 != node0); + f.m_n[0] = &m_nodes[node0]; + f.m_n[1] = &m_nodes[node1]; + f.m_n[2] = &m_nodes[node2]; + f.m_ra = AreaOf(f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x); + m_bUpdateRtCst = true; } // -void btSoftBody::appendTetra(int model,Material* mat) +void btSoftBody::appendTetra(int model, Material* mat) { -Tetra t; -if(model>=0) - t=m_tetras[model]; + Tetra t; + if (model >= 0) + t = m_tetras[model]; else - { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; } -m_tetras.push_back(t); + { + ZeroInitialize(t); + t.m_material = mat ? mat : m_materials[0]; + } + m_tetras.push_back(t); } // -void btSoftBody::appendTetra(int node0, - int node1, - int node2, - int node3, - Material* mat) +void btSoftBody::appendTetra(int node0, + int node1, + int node2, + int node3, + Material* mat) { - appendTetra(-1,mat); - Tetra& t=m_tetras[m_tetras.size()-1]; - t.m_n[0] = &m_nodes[node0]; - t.m_n[1] = &m_nodes[node1]; - t.m_n[2] = &m_nodes[node2]; - t.m_n[3] = &m_nodes[node3]; - t.m_rv = VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x); - m_bUpdateRtCst=true; + appendTetra(-1, mat); + Tetra& t = m_tetras[m_tetras.size() - 1]; + t.m_n[0] = &m_nodes[node0]; + t.m_n[1] = &m_nodes[node1]; + t.m_n[2] = &m_nodes[node2]; + t.m_n[3] = &m_nodes[node3]; + t.m_rv = VolumeOf(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x); + m_bUpdateRtCst = true; } // -void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies,btScalar influence) +void btSoftBody::appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies, btScalar influence) { - btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x; - appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies,influence); + btVector3 local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + appendAnchor(node, body, local, disableCollisionBetweenLinkedBodies, influence); } // -void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence) +void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies, btScalar influence) { if (disableCollisionBetweenLinkedBodies) { - if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size()) + if (m_collisionDisabledObjects.findLinearSearch(body) == m_collisionDisabledObjects.size()) { m_collisionDisabledObjects.push_back(body); } } - Anchor a; - a.m_node = &m_nodes[node]; - a.m_body = body; - a.m_local = localPivot; - a.m_node->m_battach = 1; + Anchor a; + a.m_node = &m_nodes[node]; + a.m_body = body; + a.m_local = localPivot; + a.m_node->m_battach = 1; a.m_influence = influence; m_anchors.push_back(a); } // -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1) +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1) { - LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); - pj->m_bodies[0] = body0; - pj->m_bodies[1] = body1; - pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; - pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; - pj->m_cfm = specs.cfm; - pj->m_erp = specs.erp; - pj->m_split = specs.split; + LJoint* pj = new (btAlignedAlloc(sizeof(LJoint), 16)) LJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse() * specs.position; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse() * specs.position; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; m_joints.push_back(pj); } // -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body) +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Body body) { - appendLinearJoint(specs,m_clusters[0],body); + appendLinearJoint(specs, m_clusters[0], body); } // -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, btSoftBody* body) { - appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); + appendLinearJoint(specs, m_clusters[0], body->m_clusters[0]); } // -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1) +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1) { - AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); - pj->m_bodies[0] = body0; - pj->m_bodies[1] = body1; - pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; - pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; - pj->m_cfm = specs.cfm; - pj->m_erp = specs.erp; - pj->m_split = specs.split; - pj->m_icontrol = specs.icontrol; + AJoint* pj = new (btAlignedAlloc(sizeof(AJoint), 16)) AJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis() * specs.axis; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis() * specs.axis; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; + pj->m_icontrol = specs.icontrol; m_joints.push_back(pj); } // -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body) +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Body body) { - appendAngularJoint(specs,m_clusters[0],body); + appendAngularJoint(specs, m_clusters[0], body); } // -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, btSoftBody* body) { - appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); + appendAngularJoint(specs, m_clusters[0], body->m_clusters[0]); } // -void btSoftBody::addForce(const btVector3& force) +void btSoftBody::addForce(const btVector3& force) { - for(int i=0,ni=m_nodes.size();i0) + Node& n = m_nodes[node]; + if (n.m_im > 0) { - n.m_f += force; + n.m_f += force; } } -void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex) +void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex) { btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size()); @@ -464,51 +479,51 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde const btScalar kDG = m_cfg.kDG; //const btScalar kPR = m_cfg.kPR; //const btScalar kVC = m_cfg.kVC; - const bool as_lift = kLF>0; - const bool as_drag = kDG>0; + const bool as_lift = kLF > 0; + const bool as_drag = kDG > 0; const bool as_aero = as_lift || as_drag; const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); Node& n = m_nodes[nodeIndex]; - if( n.m_im>0 ) + if (n.m_im > 0) { - btSoftBody::sMedium medium; + btSoftBody::sMedium medium; EvaluateMedium(m_worldInfo, n.m_x, medium); medium.m_velocity = windVelocity; medium.m_density = m_worldInfo->air_density; - /* Aerodynamics */ - if(as_vaero) - { - const btVector3 rel_v = n.m_v - medium.m_velocity; + /* Aerodynamics */ + if (as_vaero) + { + const btVector3 rel_v = n.m_v - medium.m_velocity; const btScalar rel_v_len = rel_v.length(); - const btScalar rel_v2 = rel_v.length2(); + const btScalar rel_v2 = rel_v.length2(); - if(rel_v2>SIMD_EPSILON) + if (rel_v2 > SIMD_EPSILON) { const btVector3 rel_v_nrm = rel_v.normalized(); - btVector3 nrm = n.m_n; + btVector3 nrm = n.m_n; if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag) { - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); btVector3 fDrag(0, 0, 0); btVector3 fLift(0, 0, 0); btScalar n_dot_v = nrm.dot(rel_v_nrm); btScalar tri_area = 0.5f * n.m_area; - + fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); - + // Check angle of attack // cos(10º) = 0.98480 - if ( 0 < n_dot_v && n_dot_v < 0.98480f) - fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); + if (0 < n_dot_v && n_dot_v < 0.98480f) + fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node. - btVector3 del_v_by_fDrag = fDrag*n.m_im*m_sst.sdt; + btVector3 del_v_by_fDrag = fDrag * n.m_im * m_sst.sdt; btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); btScalar v_len2 = n.m_v.length2(); @@ -516,7 +531,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde { btScalar del_v_by_fDrag_len = del_v_by_fDrag.length(); btScalar v_len = n.m_v.length(); - fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len); + fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len); } n.m_f += fDrag; @@ -525,83 +540,83 @@ 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 (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); - const btScalar dvn = btDot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) + const btScalar dvn = btDot(rel_v, nrm); + /* Compute forces */ + if (dvn > 0) { - btVector3 force(0,0,0); - const btScalar c0 = n.m_area * dvn * rel_v2/2; - const btScalar c1 = c0 * medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized() * (-c1 * kDG); + btVector3 force(0, 0, 0); + const btScalar c0 = n.m_area * dvn * rel_v2 / 2; + const btScalar c1 = c0 * medium.m_density; + force += nrm * (-c1 * kLF); + force += rel_v.normalized() * (-c1 * kDG); ApplyClampedForce(n, force, dt); } - } + } } } } } -void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceIndex) +void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex) { const btScalar dt = m_sst.sdt; const btScalar kLF = m_cfg.kLF; const btScalar kDG = m_cfg.kDG; -// const btScalar kPR = m_cfg.kPR; -// const btScalar kVC = m_cfg.kVC; - const bool as_lift = kLF>0; - const bool as_drag = kDG>0; + // const btScalar kPR = m_cfg.kPR; + // const btScalar kVC = m_cfg.kVC; + const bool as_lift = kLF > 0; + const bool as_drag = kDG > 0; const bool as_aero = as_lift || as_drag; const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); - if(as_faero) + if (as_faero) { - btSoftBody::Face& f=m_faces[faceIndex]; + btSoftBody::Face& f = m_faces[faceIndex]; - btSoftBody::sMedium medium; - - const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; - const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; - EvaluateMedium(m_worldInfo,x,medium); + btSoftBody::sMedium medium; + + const btVector3 v = (f.m_n[0]->m_v + f.m_n[1]->m_v + f.m_n[2]->m_v) / 3; + const btVector3 x = (f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3; + EvaluateMedium(m_worldInfo, x, medium); medium.m_velocity = windVelocity; medium.m_density = m_worldInfo->air_density; - const btVector3 rel_v=v-medium.m_velocity; + const btVector3 rel_v = v - medium.m_velocity; const btScalar rel_v_len = rel_v.length(); - const btScalar rel_v2=rel_v.length2(); + const btScalar rel_v2 = rel_v.length2(); - if(rel_v2>SIMD_EPSILON) + if (rel_v2 > SIMD_EPSILON) { const btVector3 rel_v_nrm = rel_v.normalized(); - btVector3 nrm = f.m_normal; + btVector3 nrm = f.m_normal; if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag) { - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); btVector3 fDrag(0, 0, 0); btVector3 fLift(0, 0, 0); btScalar n_dot_v = nrm.dot(rel_v_nrm); btScalar tri_area = 0.5f * f.m_ra; - + fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); // Check angle of attack // cos(10º) = 0.98480 - if ( 0 < n_dot_v && n_dot_v < 0.98480f) - fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); + if (0 < n_dot_v && n_dot_v < 0.98480f) + fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); fDrag /= 3; fLift /= 3; - for(int j=0;j<3;++j) + for (int j = 0; j < 3; ++j) { - if (f.m_n[j]->m_im>0) + if (f.m_n[j]->m_im > 0) { // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node. - btVector3 del_v_by_fDrag = fDrag*f.m_n[j]->m_im*m_sst.sdt; + btVector3 del_v_by_fDrag = fDrag * f.m_n[j]->m_im * m_sst.sdt; btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); btScalar v_len2 = f.m_n[j]->m_v.length2(); @@ -609,10 +624,10 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceInde { btScalar del_v_by_fDrag_len = del_v_by_fDrag.length(); btScalar v_len = f.m_n[j]->m_v.length(); - fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len); + fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len); } - f.m_n[j]->m_f += fDrag; + f.m_n[j]->m_f += fDrag; f.m_n[j]->m_f += fLift; } } @@ -620,183 +635,181 @@ 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 (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1); - const btScalar dvn=btDot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) + const btScalar dvn = btDot(rel_v, nrm); + /* Compute forces */ + if (dvn > 0) { - btVector3 force(0,0,0); - const btScalar c0 = f.m_ra*dvn*rel_v2; - const btScalar c1 = c0*medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized()*(-c1*kDG); - force /= 3; - for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt); + btVector3 force(0, 0, 0); + const btScalar c0 = f.m_ra * dvn * rel_v2; + const btScalar c1 = c0 * medium.m_density; + force += nrm * (-c1 * kLF); + force += rel_v.normalized() * (-c1 * kDG); + force /= 3; + for (int j = 0; j < 3; ++j) ApplyClampedForce(*f.m_n[j], force, dt); } } } } - } // -void btSoftBody::addVelocity(const btVector3& velocity) +void btSoftBody::addVelocity(const btVector3& velocity) { - for(int i=0,ni=m_nodes.size();i0) + Node& n = m_nodes[i]; + if (n.m_im > 0) { - n.m_v = velocity; + n.m_v = velocity; } } } - // -void btSoftBody::addVelocity(const btVector3& velocity,int node) +void btSoftBody::addVelocity(const btVector3& velocity, int node) { - Node& n=m_nodes[node]; - if(n.m_im>0) + Node& n = m_nodes[node]; + if (n.m_im > 0) { - n.m_v += velocity; + n.m_v += velocity; } } // -void btSoftBody::setMass(int node,btScalar mass) +void btSoftBody::setMass(int node, btScalar mass) { - m_nodes[node].m_im=mass>0?1/mass:0; - m_bUpdateRtCst=true; + m_nodes[node].m_im = mass > 0 ? 1 / mass : 0; + m_bUpdateRtCst = true; } // -btScalar btSoftBody::getMass(int node) const +btScalar btSoftBody::getMass(int node) const { - return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0); + return (m_nodes[node].m_im > 0 ? 1 / m_nodes[node].m_im : 0); } // -btScalar btSoftBody::getTotalMass() const +btScalar btSoftBody::getTotalMass() const { - btScalar mass=0; - for(int i=0;im_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); - for(int j=0;j<3;++j) + const Face& f = m_faces[i]; + const btScalar twicearea = AreaOf(f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x); + for (int j = 0; j < 3; ++j) { - f.m_n[j]->m_im+=twicearea; + f.m_n[j]->m_im += twicearea; } } - for( i=0;i ranks; -ranks.resize(m_nodes.size(),0); -int i; + btAlignedObjectArray ranks; + ranks.resize(m_nodes.size(), 0); + int i; -for(i=0;im_im+=btFabs(t.m_rv); - ranks[int(t.m_n[j]-&m_nodes[0])]+=1; + t.m_n[j]->m_im += btFabs(t.m_rv); + ranks[int(t.m_n[j] - &m_nodes[0])] += 1; } } -for( i=0;i0) + if (m_nodes[i].m_im > 0) { - m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im; + m_nodes[i].m_im = ranks[i] / m_nodes[i].m_im; } } -setTotalMass(mass,false); + setTotalMass(mass, false); } // -void btSoftBody::setVolumeDensity(btScalar density) +void btSoftBody::setVolumeDensity(btScalar density) { -btScalar volume=0; -for(int i=0;igetMargin(); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - - for(int i=0,ni=m_nodes.size();igetMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { - Node& n=m_nodes[i]; - n.m_x=trs*n.m_x; - n.m_q=trs*n.m_q; - n.m_n=trs.getBasis()*n.m_n; - vol = btDbvtVolume::FromCR(n.m_x,margin); - - m_ndbvt.update(n.m_leaf,vol); + Node& n = m_nodes[i]; + n.m_x = trs * n.m_x; + n.m_q = trs * n.m_q; + n.m_n = trs.getBasis() * n.m_n; + vol = btDbvtVolume::FromCR(n.m_x, margin); + + m_ndbvt.update(n.m_leaf, vol); } updateNormals(); updateBounds(); @@ -805,37 +818,37 @@ void btSoftBody::transform(const btTransform& trs) } // -void btSoftBody::translate(const btVector3& trs) +void btSoftBody::translate(const btVector3& trs) { - btTransform t; + btTransform t; t.setIdentity(); t.setOrigin(trs); transform(t); } // -void btSoftBody::rotate( const btQuaternion& rot) +void btSoftBody::rotate(const btQuaternion& rot) { - btTransform t; + btTransform t; t.setIdentity(); t.setRotation(rot); transform(t); } // -void btSoftBody::scale(const btVector3& scl) +void btSoftBody::scale(const btVector3& scl) { + const btScalar margin = getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; - const btScalar margin=getCollisionShape()->getMargin(); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - - for(int i=0,ni=m_nodes.size();i0 ? - 1/(m_nodes[i].m_im*tmass) : - kmass/tmass; + Node& n = m_nodes[i]; + m_pose.m_wgh[i] = n.m_im > 0 ? 1 / (m_nodes[i].m_im * tmass) : kmass / tmass; } - /* Pos */ - const btVector3 com=evaluateCom(); + /* Pos */ + const btVector3 com = evaluateCom(); m_pose.m_pos.resize(m_nodes.size()); - for( i=0,ni=m_nodes.size();im_x-l.m_n[1]->m_x).length(); - l.m_c1 = l.m_rl*l.m_rl; + Link& l = m_links[i]; + l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length(); + l.m_c1 = l.m_rl * l.m_rl; } } // -btScalar btSoftBody::getVolume() const +btScalar btSoftBody::getVolume() const { - btScalar vol=0; - if(m_nodes.size()>0) + btScalar vol = 0; + if (m_nodes.size() > 0) { - int i,ni; + int i, ni; - const btVector3 org=m_nodes[0].m_x; - for(i=0,ni=m_faces.size();im_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); + const Face& f = m_faces[i]; + vol += btDot(f.m_n[0]->m_x - org, btCross(f.m_n[1]->m_x - org, f.m_n[2]->m_x - org)); } - vol/=(btScalar)6; + vol /= (btScalar)6; } - return(vol); + return (vol); } // -int btSoftBody::clusterCount() const +int btSoftBody::clusterCount() const { - return(m_clusters.size()); + return (m_clusters.size()); } // -btVector3 btSoftBody::clusterCom(const Cluster* cluster) +btVector3 btSoftBody::clusterCom(const Cluster* cluster) { - btVector3 com(0,0,0); - for(int i=0,ni=cluster->m_nodes.size();im_nodes.size(); i < ni; ++i) { - com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i]; + com += cluster->m_nodes[i]->m_x * cluster->m_masses[i]; } - return(com*cluster->m_imass); + return (com * cluster->m_imass); } // -btVector3 btSoftBody::clusterCom(int cluster) const +btVector3 btSoftBody::clusterCom(int cluster) const { - return(clusterCom(m_clusters[cluster])); + return (clusterCom(m_clusters[cluster])); } // -btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) +btVector3 btSoftBody::clusterVelocity(const Cluster* cluster, const btVector3& rpos) { - return(cluster->m_lv+btCross(cluster->m_av,rpos)); + return (cluster->m_lv + btCross(cluster->m_av, rpos)); } // -void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) +void btSoftBody::clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse) { - const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse); - cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; - cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + const btVector3 li = cluster->m_imass * impulse; + const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse); + cluster->m_vimpulses[0] += li; + cluster->m_lv += li; + cluster->m_vimpulses[1] += ai; + cluster->m_av += ai; cluster->m_nvimpulses++; } // -void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) +void btSoftBody::clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse) { - const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse); - cluster->m_dimpulses[0]+=li; - cluster->m_dimpulses[1]+=ai; + const btVector3 li = cluster->m_imass * impulse; + const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse); + cluster->m_dimpulses[0] += li; + cluster->m_dimpulses[1] += ai; cluster->m_ndimpulses++; } // -void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse) +void btSoftBody::clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse) { - if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); - if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); + if (impulse.m_asVelocity) clusterVImpulse(cluster, rpos, impulse.m_velocity); + if (impulse.m_asDrift) clusterDImpulse(cluster, rpos, impulse.m_drift); } // -void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse) +void btSoftBody::clusterVAImpulse(Cluster* cluster, const btVector3& impulse) { - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + const btVector3 ai = cluster->m_invwi * impulse; + cluster->m_vimpulses[1] += ai; + cluster->m_av += ai; cluster->m_nvimpulses++; } // -void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse) +void btSoftBody::clusterDAImpulse(Cluster* cluster, const btVector3& impulse) { - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_dimpulses[1]+=ai; + const btVector3 ai = cluster->m_invwi * impulse; + cluster->m_dimpulses[1] += ai; cluster->m_ndimpulses++; } // -void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse) +void btSoftBody::clusterAImpulse(Cluster* cluster, const Impulse& impulse) { - if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); - if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); + if (impulse.m_asVelocity) clusterVAImpulse(cluster, impulse.m_velocity); + if (impulse.m_asDrift) clusterDAImpulse(cluster, impulse.m_drift); } // -void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) +void btSoftBody::clusterDCImpulse(Cluster* cluster, const btVector3& impulse) { - cluster->m_dimpulses[0]+=impulse*cluster->m_imass; + cluster->m_dimpulses[0] += impulse * cluster->m_imass; cluster->m_ndimpulses++; } struct NodeLinks { - btAlignedObjectArray m_links; + btAlignedObjectArray m_links; }; - - // -int btSoftBody::generateBendingConstraints(int distance,Material* mat) +int btSoftBody::generateBendingConstraints(int distance, Material* mat) { - int i,j; + int i, j; - if(distance>1) + if (distance > 1) { - /* Build graph */ - const int n=m_nodes.size(); - const unsigned inf=(~(unsigned)0)>>1; - unsigned* adj=new unsigned[n*n]; - + /* Build graph */ + const int n = m_nodes.size(); + const unsigned inf = (~(unsigned)0) >> 1; + unsigned* adj = new unsigned[n * n]; -#define IDX(_x_,_y_) ((_y_)*n+(_x_)) - for(j=0;j nodeLinks; - /* Build node links */ nodeLinks.resize(m_nodes.size()); - for( i=0;isum) + const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)]; + btAssert(sum == 2); + if (adj[IDX(i, j)] > sum) { - adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + adj[IDX(i, j)] = adj[IDX(j, i)] = sum; } } - } } } - } else + } + else { ///generic Floyd's algorithm - for(int k=0;ksum) + const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)]; + if (adj[IDX(i, j)] > sum) { - adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + adj[IDX(i, j)] = adj[IDX(j, i)] = sum; } } } } } - - /* Build links */ - int nlinks=0; - for(j=0;jm_leaf) m_cdbvt.remove(c->m_leaf); + Cluster* c = m_clusters[index]; + if (c->m_leaf) m_cdbvt.remove(c->m_leaf); c->~Cluster(); btAlignedFree(c); m_clusters.remove(c); } // -void btSoftBody::releaseClusters() +void btSoftBody::releaseClusters() { - while(m_clusters.size()>0) releaseCluster(0); + while (m_clusters.size() > 0) releaseCluster(0); } // -int btSoftBody::generateClusters(int k,int maxiterations) +int btSoftBody::generateClusters(int k, int maxiterations) { int i; releaseClusters(); - m_clusters.resize(btMin(k,m_nodes.size())); - for(i=0;im_collide= true; + m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + m_clusters[i]->m_collide = true; } - k=m_clusters.size(); - if(k>0) + k = m_clusters.size(); + if (k > 0) { - /* Initialize */ - btAlignedObjectArray centers; - btVector3 cog(0,0,0); - int i; - for(i=0;i centers; + btVector3 cog(0, 0, 0); + int i; + for (i = 0; i < m_nodes.size(); ++i) { - cog+=m_nodes[i].m_x; - m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]); + cog += m_nodes[i].m_x; + m_clusters[(i * 29873) % m_clusters.size()]->m_nodes.push_back(&m_nodes[i]); } - cog/=(btScalar)m_nodes.size(); - centers.resize(k,cog); - /* Iterate */ - const btScalar slope=16; - bool changed; - int iterations=0; - do { - const btScalar w=2-btMin(1,iterations/slope); - changed=false; - iterations++; + cog /= (btScalar)m_nodes.size(); + centers.resize(k, cog); + /* Iterate */ + const btScalar slope = 16; + bool changed; + int iterations = 0; + do + { + const btScalar w = 2 - btMin(1, iterations / slope); + changed = false; + iterations++; int i; - for(i=0;im_nodes.size();++j) + btVector3 c(0, 0, 0); + for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j) { - c+=m_clusters[i]->m_nodes[j]->m_x; + c += m_clusters[i]->m_nodes[j]->m_x; } - if(m_clusters[i]->m_nodes.size()) + if (m_clusters[i]->m_nodes.size()) { - c /= (btScalar)m_clusters[i]->m_nodes.size(); - c = centers[i]+(c-centers[i])*w; - changed |= ((c-centers[i]).length2()>SIMD_EPSILON); - centers[i] = c; + c /= (btScalar)m_clusters[i]->m_nodes.size(); + c = centers[i] + (c - centers[i]) * w; + changed |= ((c - centers[i]).length2() > SIMD_EPSILON); + centers[i] = c; m_clusters[i]->m_nodes.resize(0); - } + } } - for(i=0;im_nodes.push_back(&m_nodes[i]); - } - } while(changed&&(iterations cids; - cids.resize(m_nodes.size(),-1); - for(i=0;i cids; + cids.resize(m_nodes.size(), -1); + for (i = 0; i < m_clusters.size(); ++i) { - for(int j=0;jm_nodes.size();++j) + for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j) { - cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; + cids[int(m_clusters[i]->m_nodes[j] - &m_nodes[0])] = i; } } - for(i=0;im_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size()) + if (m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid]) == m_clusters[cid]->m_nodes.size()) { m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); } @@ -1289,55 +1297,56 @@ int btSoftBody::generateClusters(int k,int maxiterations) } } } - /* Master */ - if(m_clusters.size()>1) + /* Master */ + if (m_clusters.size() > 1) { - Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - pmaster->m_collide = false; + Cluster* pmaster = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + pmaster->m_collide = false; pmaster->m_nodes.reserve(m_nodes.size()); - for(int i=0;im_nodes.push_back(&m_nodes[i]); + for (int i = 0; i < m_nodes.size(); ++i) pmaster->m_nodes.push_back(&m_nodes[i]); m_clusters.push_back(pmaster); - btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); + btSwap(m_clusters[0], m_clusters[m_clusters.size() - 1]); } - /* Terminate */ - for(i=0;im_nodes.size()==0) + if (m_clusters[i]->m_nodes.size() == 0) { releaseCluster(i--); } } - } else + } + else { //create a cluster for each tetrahedron (if tetrahedra exist) or each face if (m_tetras.size()) { m_clusters.resize(m_tetras.size()); - for(i=0;im_collide= true; + m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + m_clusters[i]->m_collide = true; } - for (i=0;im_nodes.push_back(m_tetras[i].m_n[j]); } } - - } else + } + else { m_clusters.resize(m_faces.size()); - for(i=0;im_collide= true; + m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster(); + m_clusters[i]->m_collide = true; } - for(i=0;im_nodes.push_back(m_faces[i].m_n[j]); } @@ -1350,261 +1359,272 @@ int btSoftBody::generateClusters(int k,int maxiterations) initializeClusters(); updateClusters(); - //for self-collision - m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size()); + m_clusterConnectivity.resize(m_clusters.size() * m_clusters.size()); { - for (int c0=0;c0m_clusterIndex=c0; - for (int c1=0;c1m_clusterIndex = c0; + for (int c1 = 0; c1 < m_clusters.size(); c1++) { - - bool connected=false; + bool connected = false; Cluster* cla = m_clusters[c0]; Cluster* clb = m_clusters[c1]; - for (int i=0;!connected&&im_nodes.size();i++) + for (int i = 0; !connected && i < cla->m_nodes.size(); i++) { - for (int j=0;jm_nodes.size();j++) + for (int j = 0; j < clb->m_nodes.size(); j++) { if (cla->m_nodes[i] == clb->m_nodes[j]) { - connected=true; + connected = true; break; } } } - m_clusterConnectivity[c0+c1*m_clusters.size()]=connected; + m_clusterConnectivity[c0 + c1 * m_clusters.size()] = connected; } } } } - return(m_clusters.size()); + return (m_clusters.size()); } // -void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) +void btSoftBody::refine(ImplicitFn* ifn, btScalar accurary, bool cut) { - const Node* nbase = &m_nodes[0]; - int ncount = m_nodes.size(); - btSymMatrix edges(ncount,-2); - int newnodes=0; - int i,j,k,ni; + const Node* nbase = &m_nodes[0]; + int ncount = m_nodes.size(); + btSymMatrix edges(ncount, -2); + int newnodes = 0; + int i, j, k, ni; - /* Filter out */ - for(i=0;iEval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x))) + if (!SameSign(ifn->Eval(l.m_n[0]->m_x), ifn->Eval(l.m_n[1]->m_x))) { - btSwap(m_links[i],m_links[m_links.size()-1]); - m_links.pop_back();--i; + btSwap(m_links[i], m_links[m_links.size() - 1]); + m_links.pop_back(); + --i; } - } + } } - /* Fill edges */ - for(i=0;i0) + Node& a = m_nodes[i]; + Node& b = m_nodes[j]; + const btScalar t = ImplicitSolve(ifn, a.m_x, b.m_x, accurary); + if (t > 0) { - const btVector3 x=Lerp(a.m_x,b.m_x,t); - const btVector3 v=Lerp(a.m_v,b.m_v,t); - btScalar m=0; - if(a.m_im>0) + const btVector3 x = Lerp(a.m_x, b.m_x, t); + const btVector3 v = Lerp(a.m_v, b.m_v, t); + btScalar m = 0; + if (a.m_im > 0) { - if(b.m_im>0) + if (b.m_im > 0) { - const btScalar ma=1/a.m_im; - const btScalar mb=1/b.m_im; - const btScalar mc=Lerp(ma,mb,t); - const btScalar f=(ma+mb)/(ma+mb+mc); - a.m_im=1/(ma*f); - b.m_im=1/(mb*f); - m=mc*f; + const btScalar ma = 1 / a.m_im; + const btScalar mb = 1 / b.m_im; + const btScalar mc = Lerp(ma, mb, t); + const btScalar f = (ma + mb) / (ma + mb + mc); + a.m_im = 1 / (ma * f); + b.m_im = 1 / (mb * f); + m = mc * f; } else - { a.m_im/=0.5f;m=1/a.m_im; } + { + a.m_im /= 0.5f; + m = 1 / a.m_im; + } } else { - if(b.m_im>0) - { b.m_im/=0.5f;m=1/b.m_im; } + if (b.m_im > 0) + { + b.m_im /= 0.5f; + m = 1 / b.m_im; + } else - m=0; + m = 0; } - appendNode(x,m); - edges(i,j)=m_nodes.size()-1; - m_nodes[edges(i,j)].m_v=v; + appendNode(x, m); + edges(i, j) = m_nodes.size() - 1; + m_nodes[edges(i, j)].m_v = v; ++newnodes; } } } } - nbase=&m_nodes[0]; - /* Refine links */ - for(i=0,ni=m_links.size();i0) + const int ni = edges(idx[0], idx[1]); + if (ni > 0) { appendLink(i); - Link* pft[]={ &m_links[i], - &m_links[m_links.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[0]]; - pft[0]->m_n[1]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[1]]; + Link* pft[] = {&m_links[i], + &m_links[m_links.size() - 1]}; + pft[0]->m_n[0] = &m_nodes[idx[0]]; + pft[0]->m_n[1] = &m_nodes[ni]; + pft[1]->m_n[0] = &m_nodes[ni]; + pft[1]->m_n[1] = &m_nodes[idx[1]]; } } } - /* Refine faces */ - for(i=0;i0) + const int ni = edges(idx[j], idx[k]); + if (ni > 0) { appendFace(i); - const int l=(k+1)%3; - Face* pft[]={ &m_faces[i], - &m_faces[m_faces.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[l]]; - pft[0]->m_n[1]=&m_nodes[idx[j]]; - pft[0]->m_n[2]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[k]]; - pft[1]->m_n[2]=&m_nodes[idx[l]]; - appendLink(ni,idx[l],pft[0]->m_material); - --i;break; + const int l = (k + 1) % 3; + Face* pft[] = {&m_faces[i], + &m_faces[m_faces.size() - 1]}; + pft[0]->m_n[0] = &m_nodes[idx[l]]; + pft[0]->m_n[1] = &m_nodes[idx[j]]; + pft[0]->m_n[2] = &m_nodes[ni]; + pft[1]->m_n[0] = &m_nodes[ni]; + pft[1]->m_n[1] = &m_nodes[idx[k]]; + pft[1]->m_n[2] = &m_nodes[idx[l]]; + appendLink(ni, idx[l], pft[0]->m_material); + --i; + break; } } } } - /* Cut */ - if(cut) - { - btAlignedObjectArray cnodes; - const int pcount=ncount; - int i; - ncount=m_nodes.size(); - cnodes.resize(ncount,0); - /* Nodes */ - for(i=0;i cnodes; + const int pcount = ncount; + int i; + ncount = m_nodes.size(); + cnodes.resize(ncount, 0); + /* Nodes */ + for (i = 0; i < ncount; ++i) { - const btVector3 x=m_nodes[i].m_x; - if((i>=pcount)||(btFabs(ifn->Eval(x))= pcount) || (btFabs(ifn->Eval(x)) < accurary)) { - const btVector3 v=m_nodes[i].m_v; - btScalar m=getMass(i); - if(m>0) { m*=0.5f;m_nodes[i].m_im/=0.5f; } - appendNode(x,m); - cnodes[i]=m_nodes.size()-1; - m_nodes[cnodes[i]].m_v=v; + const btVector3 v = m_nodes[i].m_v; + btScalar m = getMass(i); + if (m > 0) + { + m *= 0.5f; + m_nodes[i].m_im /= 0.5f; + } + appendNode(x, m); + cnodes[i] = m_nodes.size() - 1; + m_nodes[cnodes[i]].m_v = v; } } - nbase=&m_nodes[0]; - /* Links */ - for(i=0,ni=m_links.size();iEval(m_nodes[id[0]].m_x)Eval(m_nodes[id[1]].m_x)Eval(m_nodes[id[0]].m_x) < accurary) && + (ifn->Eval(m_nodes[id[1]].m_x) < accurary))) + todetach = i; } - if(todetach) + if (todetach) { - Link& l=m_links[todetach]; - for(int j=0;j<2;++j) + Link& l = m_links[todetach]; + for (int j = 0; j < 2; ++j) { - int cn=cnodes[int(l.m_n[j]-nbase)]; - if(cn) l.m_n[j]=&m_nodes[cn]; - } - } - } - /* Faces */ - for(i=0,ni=m_faces.size();iEval(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x) ranks; - btAlignedObjectArray todelete; - ranks.resize(nnodes,0); - for(i=0,ni=m_links.size();iEval(n[0]->m_x) < accurary) && + (ifn->Eval(n[1]->m_x) < accurary) && + (ifn->Eval(n[2]->m_x) < accurary)) + { + for (int j = 0; j < 3; ++j) + { + int cn = cnodes[int(n[j] - nbase)]; + if (cn) n[j] = &m_nodes[cn]; + } + } } - for(i=0,ni=m_faces.size();i ranks; + btAlignedObjectArray todelete; + ranks.resize(nnodes, 0); + for (i = 0, ni = m_links.size(); i < ni; ++i) { - for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++; + for (int j = 0; j < 2; ++j) ranks[int(m_links[i].m_n[j] - nbase)]++; } - for(i=0;im_v=v; - pn[1]->m_v=v; - for(i=0,ni=m_links.size();im_v = v; + pn[1]->m_v = v; + for (i = 0, ni = m_links.size(); i < ni; ++i) { - const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb); - if(mtch!=-1) + const int mtch = MatchEdge(m_links[i].m_n[0], m_links[i].m_n[1], pa, pb); + if (mtch != -1) { appendLink(i); - Link* pft[]={&m_links[i],&m_links[m_links.size()-1]}; - pft[0]->m_n[1]=pn[mtch]; - pft[1]->m_n[0]=pn[1-mtch]; - done=true; + Link* pft[] = {&m_links[i], &m_links[m_links.size() - 1]}; + pft[0]->m_n[1] = pn[mtch]; + pft[1]->m_n[0] = pn[1 - mtch]; + done = true; } } - for(i=0,ni=m_faces.size();im_n[l]=pn[mtch]; - pft[1]->m_n[k]=pn[1-mtch]; - appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); - appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + Face* pft[] = {&m_faces[i], &m_faces[m_faces.size() - 1]}; + pft[0]->m_n[l] = pn[mtch]; + pft[1]->m_n[k] = pn[1 - mtch]; + appendLink(pn[0], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true); + appendLink(pn[1], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true); } } } - if(!done) + if (!done) { m_ndbvt.remove(pn[0]->m_leaf); m_ndbvt.remove(pn[1]->m_leaf); m_nodes.pop_back(); m_nodes.pop_back(); } - return(done); + return (done); } // -bool btSoftBody::rayTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results) +bool btSoftBody::rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) { - if(m_faces.size()&&m_fdbvt.empty()) + if (m_faces.size() && m_fdbvt.empty()) initializeFaceTree(); - results.body = this; + results.body = this; results.fraction = 1.f; - results.feature = eFeature::None; - results.index = -1; + results.feature = eFeature::None; + results.index = -1; - return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0); + return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0); } // -void btSoftBody::setSolver(eSolverPresets::_ preset) +void btSoftBody::setSolver(eSolverPresets::_ preset) { m_cfg.m_vsequence.clear(); m_cfg.m_psequence.clear(); m_cfg.m_dsequence.clear(); - switch(preset) + switch (preset) { - case eSolverPresets::Positions: - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - m_cfg.m_psequence.push_back(ePSolver::Linear); - break; - case eSolverPresets::Velocities: - m_cfg.m_vsequence.push_back(eVSolver::Linear); + case eSolverPresets::Positions: + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + m_cfg.m_psequence.push_back(ePSolver::Linear); + break; + case eSolverPresets::Velocities: + m_cfg.m_vsequence.push_back(eVSolver::Linear); - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); - m_cfg.m_dsequence.push_back(ePSolver::Linear); - break; + m_cfg.m_dsequence.push_back(ePSolver::Linear); + break; } } // -void btSoftBody::predictMotion(btScalar dt) +void btSoftBody::predictMotion(btScalar dt) { + int i, ni; - int i,ni; - - /* Update */ - if(m_bUpdateRtCst) + /* Update */ + if (m_bUpdateRtCst) { - m_bUpdateRtCst=false; + m_bUpdateRtCst = false; updateConstants(); m_fdbvt.clear(); - if(m_cfg.collisions&fCollision::VF_SS) + if (m_cfg.collisions & fCollision::VF_SS) { - initializeFaceTree(); + initializeFaceTree(); } } - /* Prepare */ - m_sst.sdt = dt*m_cfg.timescale; - m_sst.isdt = 1/m_sst.sdt; - m_sst.velmrg = m_sst.sdt*3; - m_sst.radmrg = getCollisionShape()->getMargin(); - m_sst.updmrg = m_sst.radmrg*(btScalar)0.25; - /* Forces */ - addVelocity(m_worldInfo->m_gravity*m_sst.sdt); + /* Prepare */ + m_sst.sdt = dt * m_cfg.timescale; + m_sst.isdt = 1 / m_sst.sdt; + m_sst.velmrg = m_sst.sdt * 3; + m_sst.radmrg = getCollisionShape()->getMargin(); + m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; + /* Forces */ + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); applyForces(); - /* Integrate */ - for(i=0,ni=m_nodes.size();im_maxDisplacement; - btScalar clampDeltaV = maxDisplacement/m_sst.sdt; - for (int c=0;c<3;c++) + btScalar clampDeltaV = maxDisplacement / m_sst.sdt; + for (int c = 0; c < 3; c++) { - if (deltaV[c]>clampDeltaV) + if (deltaV[c] > clampDeltaV) { deltaV[c] = clampDeltaV; } - if (deltaV[c]<-clampDeltaV) + if (deltaV[c] < -clampDeltaV) { - deltaV[c]=-clampDeltaV; + deltaV[c] = -clampDeltaV; } } } - n.m_v += deltaV; - n.m_x += n.m_v*m_sst.sdt; - n.m_f = btVector3(0,0,0); + n.m_v += deltaV; + n.m_x += n.m_v * m_sst.sdt; + n.m_f = btVector3(0, 0, 0); } - /* Clusters */ + /* Clusters */ updateClusters(); - /* Bounds */ - updateBounds(); - /* Nodes */ - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - for(i=0,ni=m_nodes.size();im_v+ - f.m_n[1]->m_v+ - f.m_n[2]->m_v)/3; - vol = VolumeOf(f,m_sst.radmrg); - m_fdbvt.update( f.m_leaf, - vol, - v*m_sst.velmrg, - m_sst.updmrg); + Face& f = m_faces[i]; + const btVector3 v = (f.m_n[0]->m_v + + f.m_n[1]->m_v + + f.m_n[2]->m_v) / + 3; + vol = VolumeOf(f, m_sst.radmrg); + m_fdbvt.update(f.m_leaf, + vol, + v * m_sst.velmrg, + m_sst.updmrg); } } - /* Pose */ + /* Pose */ updatePose(); - /* Match */ - if(m_pose.m_bframe&&(m_cfg.kMT>0)) + /* Match */ + if (m_pose.m_bframe && (m_cfg.kMT > 0)) { - const btMatrix3x3 posetrs=m_pose.m_rot; - for(int i=0,ni=m_nodes.size();i0) + Node& n = m_nodes[i]; + if (n.m_im > 0) { - const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com; - n.m_x=Lerp(n.m_x,x,m_cfg.kMT); + const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; + n.m_x = Lerp(n.m_x, x, m_cfg.kMT); } } } - /* Clear contacts */ + /* Clear contacts */ m_rcontacts.resize(0); m_scontacts.resize(0); - /* Optimize dbvt's */ + /* Optimize dbvt's */ m_ndbvt.optimizeIncremental(1); m_fdbvt.optimizeIncremental(1); m_cdbvt.optimizeIncremental(1); } // -void btSoftBody::solveConstraints() +void btSoftBody::solveConstraints() { - - /* Apply clusters */ + /* Apply clusters */ applyClusters(false); - /* Prepare links */ + /* Prepare links */ - int i,ni; + int i, ni; - for(i=0,ni=m_links.size();im_q-l.m_n[0]->m_q; - l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); + Link& l = m_links[i]; + l.m_c3 = l.m_n[1]->m_q - l.m_n[0]->m_q; + l.m_c2 = 1 / (l.m_c3.length2() * l.m_c0); } - /* Prepare anchors */ - for(i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; - a.m_c0 = ImpulseMatrix( m_sst.sdt, - a.m_node->m_im, - a.m_body->getInvMass(), - a.m_body->getInvInertiaTensorWorld(), - ra); - a.m_c1 = ra; - a.m_c2 = m_sst.sdt*a.m_node->m_im; + Anchor& a = m_anchors[i]; + const btVector3 ra = a.m_body->getWorldTransform().getBasis() * a.m_local; + a.m_c0 = ImpulseMatrix(m_sst.sdt, + a.m_node->m_im, + a.m_body->getInvMass(), + a.m_body->getInvInertiaTensorWorld(), + ra); + a.m_c1 = ra; + a.m_c2 = m_sst.sdt * a.m_node->m_im; a.m_body->activate(); } - /* Solve velocities */ - if(m_cfg.viterations>0) + /* Solve velocities */ + if (m_cfg.viterations > 0) { - /* Solve */ - for(int isolve=0;isolve0) + /* Solve positions */ + if (m_cfg.piterations > 0) { - for(int isolve=0;isolve0) + /* Solve drift */ + if (m_cfg.diterations > 0) { - const btScalar vcf=m_cfg.kVCF*m_sst.isdt; - for(i=0,ni=m_nodes.size();i& bodies) +void btSoftBody::solveClusters(const btAlignedObjectArray& bodies) { - const int nb=bodies.size(); - int iterations=0; + const int nb = bodies.size(); + int iterations = 0; int i; - for(i=0;im_cfg.citerations); + iterations = btMax(iterations, bodies[i]->m_cfg.citerations); } - for(i=0;iprepareClusters(iterations); } - for(i=0;isolveClusters(sor); } } - for(i=0;icleanupClusters(); } } // -void btSoftBody::integrateMotion() +void btSoftBody::integrateMotion() { - /* Update */ + /* Update */ updateNormals(); } // -btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) +btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom, const btVector3& rayTo, btScalar mxt) { m_rayFrom = rayFrom; - m_rayNormalizedDirection = (rayTo-rayFrom); + m_rayNormalizedDirection = (rayTo - rayFrom); m_rayTo = rayTo; - m_mint = mxt; - m_face = 0; - m_tests = 0; + m_mint = mxt; + m_face = 0; + m_tests = 0; } // -void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) +void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) { - btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; - const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - m_mint); - if((t>0)&&(tdata; + const btScalar t = rayFromToTriangle(m_rayFrom, m_rayTo, m_rayNormalizedDirection, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + m_mint); + if ((t > 0) && (t < m_mint)) + { + m_mint = t; + m_face = &f; } ++m_tests; } // -btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayNormalizedDirection, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt) +btScalar btSoftBody::RayFromToCaster::rayFromToTriangle(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt) { - static const btScalar ceps=-SIMD_EPSILON*10; - static const btScalar teps=SIMD_EPSILON*10; + static const btScalar ceps = -SIMD_EPSILON * 10; + static const btScalar teps = SIMD_EPSILON * 10; - const btVector3 n=btCross(b-a,c-a); - const btScalar d=btDot(a,n); - const btScalar den=btDot(rayNormalizedDirection,n); - if(!btFuzzyZero(den)) + const btVector3 n = btCross(b - a, c - a); + const btScalar d = btDot(a, n); + const btScalar den = btDot(rayNormalizedDirection, n); + if (!btFuzzyZero(den)) { - const btScalar num=btDot(rayFrom,n)-d; - const btScalar t=-num/den; - if((t>teps)&&(t teps) && (t < maxt)) { - const btVector3 hit=rayFrom+rayNormalizedDirection*t; - if( (btDot(n,btCross(a-hit,b-hit))>ceps) && - (btDot(n,btCross(b-hit,c-hit))>ceps) && - (btDot(n,btCross(c-hit,a-hit))>ceps)) + const btVector3 hit = rayFrom + rayNormalizedDirection * t; + if ((btDot(n, btCross(a - hit, b - hit)) > ceps) && + (btDot(n, btCross(b - hit, c - hit)) > ceps) && + (btDot(n, btCross(c - hit, a - hit)) > ceps)) { - return(t); + return (t); } } } - return(-1); + return (-1); } // -void btSoftBody::pointersToIndices() +void btSoftBody::pointersToIndices() { -#define PTR2IDX(_p_,_b_) reinterpret_cast((_p_)-(_b_)) - btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0] : 0; - int i,ni; +#define PTR2IDX(_p_, _b_) reinterpret_cast((_p_) - (_b_)) + btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0; + int i, ni; - for(i=0,ni=m_nodes.size();idata=*(void**)&i; + m_nodes[i].m_leaf->data = *(void**)&i; } } - for(i=0,ni=m_links.size();idata=*(void**)&i; + m_faces[i].m_leaf->data = *(void**)&i; } } - for(i=0,ni=m_anchors.size();idata=&m_nodes[i]; + m_nodes[i].m_leaf->data = &m_nodes[i]; } } - for(i=0,ni=m_links.size();idata=&m_faces[i]; + m_faces[i].m_leaf->data = &m_faces[i]; } } - for(i=0,ni=m_anchors.size();im_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); - if(t>0) + const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + mint); + if (t > 0) { ++cnt; - if(!bcountonly) + if (!bcountonly) { - feature=btSoftBody::eFeature::Face; - index=i; - mint=t; + feature = btSoftBody::eFeature::Face; + index = i; + mint = t; } } } } else - {/* Use dbvt */ - RayFromToCaster collider(rayFrom,rayTo,mint); + { /* Use dbvt */ + RayFromToCaster collider(rayFrom, rayTo, mint); - btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider); - if(collider.m_face) + btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider); + if (collider.m_face) { - mint=collider.m_mint; - feature=btSoftBody::eFeature::Face; - index=(int)(collider.m_face-&m_faces[0]); - cnt=1; + mint = collider.m_mint; + feature = btSoftBody::eFeature::Face; + index = (int)(collider.m_face - &m_faces[0]); + cnt = 1; } } - for (int i=0;im_x; + btVector3 v1 = tet.m_n[index1]->m_x; + btVector3 v2 = tet.m_n[index2]->m_x; - int index0=tetfaces[f][0]; - int index1=tetfaces[f][1]; - int index2=tetfaces[f][2]; - btVector3 v0=tet.m_n[index0]->m_x; - btVector3 v1=tet.m_n[index1]->m_x; - btVector3 v2=tet.m_n[index2]->m_x; - - - const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir, - v0,v1,v2, - mint); - if(t>0) + const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir, + v0, v1, v2, + mint); + if (t > 0) { ++cnt; - if(!bcountonly) + if (!bcountonly) { - feature=btSoftBody::eFeature::Tetra; - index=i; - mint=t; + feature = btSoftBody::eFeature::Tetra; + index = i; + mint = t; } } } } - return(cnt); + return (cnt); } // -void btSoftBody::initializeFaceTree() +void btSoftBody::initializeFaceTree() { m_fdbvt.clear(); - for(int i=0;igetCollisionShape(); -// const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); - const btTransform &wtr = colObjWrap->getWorldTransform(); + const btTransform& wtr = colObjWrap->getWorldTransform(); //todo: check which transform is needed here - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), shp, nrm, margin); - if(dst<0) + if (dst < 0) { cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = wtr.getBasis()*nrm; - cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst ); - return(true); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + return (true); } - return(false); + return (false); } // -void btSoftBody::updateNormals() +void btSoftBody::updateNormals() { + const btVector3 zv(0, 0, 0); + int i, ni; - const btVector3 zv(0,0,0); - int i,ni; - - for(i=0,ni=m_nodes.size();im_x-f.m_n[0]->m_x, - f.m_n[2]->m_x-f.m_n[0]->m_x); - f.m_normal=n.normalized(); - f.m_n[0]->m_n+=n; - f.m_n[1]->m_n+=n; - f.m_n[2]->m_n+=n; + btSoftBody::Face& f = m_faces[i]; + const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, + f.m_n[2]->m_x - f.m_n[0]->m_x); + f.m_normal = n.normalized(); + f.m_n[0]->m_n += n; + f.m_n[1]->m_n += n; + f.m_n[2]->m_n += n; } - for(i=0,ni=m_nodes.size();iSIMD_EPSILON) + if (len > SIMD_EPSILON) m_nodes[i].m_n /= len; } } // -void btSoftBody::updateBounds() +void btSoftBody::updateBounds() { /*if( m_acceleratedSoftBody ) { @@ -2317,258 +2333,259 @@ void btSoftBody::updateBounds() m_bounds[1] = btVector3(1000, 1000, 1000); } else {*/ - if(m_ndbvt.m_root) + if (m_ndbvt.m_root) + { + const btVector3& mins = m_ndbvt.m_root->volume.Mins(); + const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); + const btScalar csm = getCollisionShape()->getMargin(); + const btVector3 mrg = btVector3(csm, + csm, + csm) * + 1; // ??? to investigate... + m_bounds[0] = mins - mrg; + m_bounds[1] = maxs + mrg; + if (0 != getBroadphaseHandle()) { - const btVector3& mins=m_ndbvt.m_root->volume.Mins(); - const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); - const btScalar csm=getCollisionShape()->getMargin(); - const btVector3 mrg=btVector3( csm, - csm, - csm)*1; // ??? to investigate... - m_bounds[0]=mins-mrg; - m_bounds[1]=maxs+mrg; - if(0!=getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); - } + m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); } - else - { - m_bounds[0]= - m_bounds[1]=btVector3(0,0,0); - } + } + else + { + m_bounds[0] = + m_bounds[1] = btVector3(0, 0, 0); + } //} } - // -void btSoftBody::updatePose() +void btSoftBody::updatePose() { - if(m_pose.m_bframe) + if (m_pose.m_bframe) { - btSoftBody::Pose& pose=m_pose; - const btVector3 com=evaluateCom(); - /* Com */ - pose.m_com = com; - /* Rotation */ - btMatrix3x3 Apq; - const btScalar eps=SIMD_EPSILON; - Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0); - Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3); - for(int i=0,ni=m_nodes.size();i1) + btMatrix3x3 r, s; + PolarDecompose(Apq, r, s); + pose.m_rot = r; + pose.m_scl = pose.m_aqq * r.transpose() * Apq; + if (m_cfg.maxvolume > 1) { - const btScalar idet=Clamp( 1/pose.m_scl.determinant(), - 1,m_cfg.maxvolume); - pose.m_scl=Mul(pose.m_scl,idet); + const btScalar idet = Clamp(1 / pose.m_scl.determinant(), + 1, m_cfg.maxvolume); + pose.m_scl = Mul(pose.m_scl, idet); } - } } // -void btSoftBody::updateArea(bool averageArea) +void btSoftBody::updateArea(bool averageArea) { - int i,ni; + int i, ni; - /* Face area */ - for(i=0,ni=m_faces.size();im_x,f.m_n[1]->m_x,f.m_n[2]->m_x); + Face& f = m_faces[i]; + f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x); } - - /* Node area */ + + /* Node area */ if (averageArea) { - btAlignedObjectArray counts; - counts.resize(m_nodes.size(),0); - for(i=0,ni=m_nodes.size();i counts; + counts.resize(m_nodes.size(), 0); + for (i = 0, ni = m_nodes.size(); i < ni; ++i) { - m_nodes[i].m_area = 0; + m_nodes[i].m_area = 0; } - for(i=0,ni=m_faces.size();im_area+=btFabs(f.m_ra); + f.m_n[j]->m_area += btFabs(f.m_ra); } } - for(i=0,ni=m_nodes.size();i0) - m_nodes[i].m_area/=(btScalar)counts[i]; + if (counts[i] > 0) + m_nodes[i].m_area /= (btScalar)counts[i]; else - m_nodes[i].m_area=0; + m_nodes[i].m_area = 0; } } else { // initialize node area as zero - for(i=0,ni=m_nodes.size();im_area += f.m_ra; } } - for(i=0,ni=m_nodes.size();im_im+l.m_n[1]->m_im)/m.m_kLST; + Link& l = m_links[i]; + Material& m = *l.m_material; + l.m_c0 = (l.m_n[0]->m_im + l.m_n[1]->m_im) / m.m_kLST; } } -void btSoftBody::updateConstants() +void btSoftBody::updateConstants() { resetLinkRestLengths(); updateLinkConstants(); updateArea(); } - - // -void btSoftBody::initializeClusters() +void btSoftBody::initializeClusters() { int i; - for( i=0;im_im==0) + if (c.m_nodes[j]->m_im == 0) { c.m_containsAnchor = true; - c.m_masses[j] = BT_LARGE_FLOAT; - } else - { - c.m_masses[j] = btScalar(1.)/c.m_nodes[j]->m_im; + c.m_masses[j] = BT_LARGE_FLOAT; } - c.m_imass += c.m_masses[j]; + else + { + c.m_masses[j] = btScalar(1.) / c.m_nodes[j]->m_im; + } + c.m_imass += c.m_masses[j]; } - c.m_imass = btScalar(1.)/c.m_imass; - c.m_com = btSoftBody::clusterCom(&c); - c.m_lv = btVector3(0,0,0); - c.m_av = btVector3(0,0,0); - c.m_leaf = 0; - /* Inertia */ - btMatrix3x3& ii=c.m_locii; - ii[0]=ii[1]=ii[2]=btVector3(0,0,0); + c.m_imass = btScalar(1.) / c.m_imass; + c.m_com = btSoftBody::clusterCom(&c); + c.m_lv = btVector3(0, 0, 0); + c.m_av = btVector3(0, 0, 0); + c.m_leaf = 0; + /* Inertia */ + btMatrix3x3& ii = c.m_locii; + ii[0] = ii[1] = ii[2] = btVector3(0, 0, 0); { - int i,ni; + int i, ni; - for(i=0,ni=c.m_nodes.size();im_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=c.m_masses[i]; - ii[0][0] += m*(q[1]+q[2]); - ii[1][1] += m*(q[0]+q[2]); - ii[2][2] += m*(q[0]+q[1]); - ii[0][1] -= m*k[0]*k[1]; - ii[0][2] -= m*k[0]*k[2]; - ii[1][2] -= m*k[1]*k[2]; + const btVector3 k = c.m_nodes[i]->m_x - c.m_com; + const btVector3 q = k * k; + const btScalar m = c.m_masses[i]; + ii[0][0] += m * (q[1] + q[2]); + ii[1][1] += m * (q[0] + q[2]); + ii[2][2] += m * (q[0] + q[1]); + ii[0][1] -= m * k[0] * k[1]; + ii[0][2] -= m * k[0] * k[2]; + ii[1][2] -= m * k[1] * k[2]; } } - ii[1][0]=ii[0][1]; - ii[2][0]=ii[0][2]; - ii[2][1]=ii[1][2]; - + ii[1][0] = ii[0][1]; + ii[2][0] = ii[0][2]; + ii[2][1] = ii[1][2]; + ii = ii.inverse(); - /* Frame */ + /* Frame */ c.m_framexform.setIdentity(); c.m_framexform.setOrigin(c.m_com); c.m_framerefs.resize(c.m_nodes.size()); { int i; - for(i=0;im_x-c.m_com; + c.m_framerefs[i] = c.m_nodes[i]->m_x - c.m_com; } } } } // -void btSoftBody::updateClusters() +void btSoftBody::updateClusters() { BT_PROFILE("UpdateClusters"); int i; - for(i=0;im_x-c.m_com; - const btVector3& b=c.m_framerefs[i]; - m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b; + const btVector3 a = c.m_nodes[i]->m_x - c.m_com; + const btVector3& b = c.m_framerefs[i]; + m[0] += a[0] * b; + m[1] += a[1] * b; + m[2] += a[2] * b; } - PolarDecompose(m,r,s); + PolarDecompose(m, r, s); c.m_framexform.setOrigin(c.m_com); - c.m_framexform.setBasis(r); - /* Inertia */ -#if 1/* Constant */ - c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); + c.m_framexform.setBasis(r); + /* Inertia */ +#if 1 /* Constant */ + c.m_invwi = c.m_framexform.getBasis() * c.m_locii * c.m_framexform.getBasis().transpose(); #else -#if 0/* Sphere */ +#if 0 /* Sphere */ const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass); const btVector3 inertia(rk,rk,rk); const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0, @@ -2576,186 +2593,181 @@ void btSoftBody::updateClusters() btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose(); -#else/* Actual */ - c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0); - for(int i=0;im_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=1/c.m_nodes[i]->m_im; - c.m_invwi[0][0] += m*(q[1]+q[2]); - c.m_invwi[1][1] += m*(q[0]+q[2]); - c.m_invwi[2][2] += m*(q[0]+q[1]); - c.m_invwi[0][1] -= m*k[0]*k[1]; - c.m_invwi[0][2] -= m*k[0]*k[2]; - c.m_invwi[1][2] -= m*k[1]*k[2]; + const btVector3 k = c.m_nodes[i]->m_x - c.m_com; + const btVector3 q = k * k; + const btScalar m = 1 / c.m_nodes[i]->m_im; + c.m_invwi[0][0] += m * (q[1] + q[2]); + c.m_invwi[1][1] += m * (q[0] + q[2]); + c.m_invwi[2][2] += m * (q[0] + q[1]); + c.m_invwi[0][1] -= m * k[0] * k[1]; + c.m_invwi[0][2] -= m * k[0] * k[2]; + c.m_invwi[1][2] -= m * k[1] * k[2]; } - c.m_invwi[1][0]=c.m_invwi[0][1]; - c.m_invwi[2][0]=c.m_invwi[0][2]; - c.m_invwi[2][1]=c.m_invwi[1][2]; - c.m_invwi=c.m_invwi.inverse(); + c.m_invwi[1][0] = c.m_invwi[0][1]; + c.m_invwi[2][0] = c.m_invwi[0][2]; + c.m_invwi[2][1] = c.m_invwi[1][2]; + c.m_invwi = c.m_invwi.inverse(); #endif #endif - /* Velocities */ - c.m_lv=btVector3(0,0,0); - c.m_av=btVector3(0,0,0); + /* Velocities */ + c.m_lv = btVector3(0, 0, 0); + c.m_av = btVector3(0, 0, 0); { int i; - for(i=0;im_v*c.m_masses[i]; - c.m_lv += v; - c.m_av += btCross(c.m_nodes[i]->m_x-c.m_com,v); + const btVector3 v = c.m_nodes[i]->m_v * c.m_masses[i]; + c.m_lv += v; + c.m_av += btCross(c.m_nodes[i]->m_x - c.m_com, v); } } - c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); - c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping); - c.m_vimpulses[0] = - c.m_vimpulses[1] = btVector3(0,0,0); - c.m_dimpulses[0] = - c.m_dimpulses[1] = btVector3(0,0,0); - c.m_nvimpulses = 0; - c.m_ndimpulses = 0; - /* Matching */ - if(c.m_matching>0) + c.m_lv = c.m_imass * c.m_lv * (1 - c.m_ldamping); + c.m_av = c.m_invwi * c.m_av * (1 - c.m_adamping); + c.m_vimpulses[0] = + c.m_vimpulses[1] = btVector3(0, 0, 0); + c.m_dimpulses[0] = + c.m_dimpulses[1] = btVector3(0, 0, 0); + c.m_nvimpulses = 0; + c.m_ndimpulses = 0; + /* Matching */ + if (c.m_matching > 0) { - for(int j=0;jm_x; - btVector3 mx=mi; - for(int j=1;jm_x; + btVector3 mx = mi; + for (int j = 1; j < n; ++j) { mi.setMin(c.m_nodes[j]->m_x); mx.setMax(c.m_nodes[j]->m_x); - } - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx); - if(c.m_leaf) - m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); + } + ATTRIBUTE_ALIGNED16(btDbvtVolume) + bounds = btDbvtVolume::FromMM(mi, mx); + if (c.m_leaf) + m_cdbvt.update(c.m_leaf, bounds, c.m_lv * m_sst.sdt * 3, m_sst.radmrg); else - c.m_leaf=m_cdbvt.insert(bounds,&c); + c.m_leaf = m_cdbvt.insert(bounds, &c); } } } - - } - - - // -void btSoftBody::cleanupClusters() +void btSoftBody::cleanupClusters() { - for(int i=0;iTerminate(m_sst.sdt); - if(m_joints[i]->m_delete) + if (m_joints[i]->m_delete) { btAlignedFree(m_joints[i]); m_joints.remove(m_joints[i--]); - } + } } } // -void btSoftBody::prepareClusters(int iterations) +void btSoftBody::prepareClusters(int iterations) { - for(int i=0;iPrepare(m_sst.sdt,iterations); + m_joints[i]->Prepare(m_sst.sdt, iterations); } } - // -void btSoftBody::solveClusters(btScalar sor) +void btSoftBody::solveClusters(btScalar sor) { - for(int i=0,ni=m_joints.size();iSolve(m_sst.sdt,sor); + m_joints[i]->Solve(m_sst.sdt, sor); } } // -void btSoftBody::applyClusters(bool drift) +void btSoftBody::applyClusters(bool drift) { BT_PROFILE("ApplyClusters"); -// const btScalar f0=m_sst.sdt; + // const btScalar f0=m_sst.sdt; //const btScalar f1=f0/2; btAlignedObjectArray deltas; btAlignedObjectArray weights; - deltas.resize(m_nodes.size(),btVector3(0,0,0)); - weights.resize(m_nodes.size(),0); + deltas.resize(m_nodes.size(), btVector3(0, 0, 0)); + weights.resize(m_nodes.size(), 0); int i; - if(drift) + if (drift) { - for(i=0;im_x; - const btScalar q=c.m_masses[j]; - deltas[idx] += (v+btCross(w,x-c.m_com))*q; - weights[idx] += q; + const int idx = int(c.m_nodes[j] - &m_nodes[0]); + const btVector3& x = c.m_nodes[j]->m_x; + const btScalar q = c.m_masses[j]; + deltas[idx] += (v + btCross(w, x - c.m_com)) * q; + weights[idx] += q; } } } - for(i=0;i0) + if (weights[i] > 0) { - m_nodes[i].m_x+=deltas[i]/weights[i]; + m_nodes[i].m_x += deltas[i] / weights[i]; } } } // -void btSoftBody::dampClusters() +void btSoftBody::dampClusters() { int i; - for(i=0;i0) + Cluster& c = *m_clusters[i]; + if (c.m_ndamping > 0) { - for(int j=0;j0) + Node& n = *c.m_nodes[j]; + if (n.m_im > 0) { - const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com); - if(vx.length2()<=n.m_v.length2()) - { - n.m_v += c.m_ndamping*(vx-n.m_v); - } + const btVector3 vx = c.m_lv + btCross(c.m_av, c.m_nodes[j]->m_q - c.m_com); + if (vx.length2() <= n.m_v.length2()) + { + n.m_v += c.m_ndamping * (vx - n.m_v); + } } } } @@ -2763,710 +2775,696 @@ void btSoftBody::dampClusters() } // -void btSoftBody::Joint::Prepare(btScalar dt,int) +void btSoftBody::Joint::Prepare(btScalar dt, int) { m_bodies[0].activate(); m_bodies[1].activate(); } // -void btSoftBody::LJoint::Prepare(btScalar dt,int iterations) +void btSoftBody::LJoint::Prepare(btScalar dt, int iterations) { - static const btScalar maxdrift=4; - Joint::Prepare(dt,iterations); - m_rpos[0] = m_bodies[0].xform()*m_refs[0]; - m_rpos[1] = m_bodies[1].xform()*m_refs[1]; - m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt; - m_rpos[0] -= m_bodies[0].xform().getOrigin(); - m_rpos[1] -= m_bodies[1].xform().getOrigin(); - m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0], - m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]); - if(m_split>0) + static const btScalar maxdrift = 4; + Joint::Prepare(dt, iterations); + m_rpos[0] = m_bodies[0].xform() * m_refs[0]; + m_rpos[1] = m_bodies[1].xform() * m_refs[1]; + m_drift = Clamp(m_rpos[0] - m_rpos[1], maxdrift) * m_erp / dt; + m_rpos[0] -= m_bodies[0].xform().getOrigin(); + m_rpos[1] -= m_bodies[1].xform().getOrigin(); + m_massmatrix = ImpulseMatrix(m_bodies[0].invMass(), m_bodies[0].invWorldInertia(), m_rpos[0], + m_bodies[1].invMass(), m_bodies[1].invWorldInertia(), m_rpos[1]); + if (m_split > 0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix * (m_drift * m_split); + m_drift *= 1 - m_split; } - m_drift /=(btScalar)iterations; + m_drift /= (btScalar)iterations; } // -void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor) +void btSoftBody::LJoint::Solve(btScalar dt, btScalar sor) { - const btVector3 va=m_bodies[0].velocity(m_rpos[0]); - const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); - const btVector3 vr=va-vb; - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); + const btVector3 va = m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb = m_bodies[1].velocity(m_rpos[1]); + const btVector3 vr = va - vb; + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix * (m_drift + vr * m_cfm) * sor; + m_bodies[0].applyImpulse(-impulse, m_rpos[0]); + m_bodies[1].applyImpulse(impulse, m_rpos[1]); } // -void btSoftBody::LJoint::Terminate(btScalar dt) +void btSoftBody::LJoint::Terminate(btScalar dt) { - if(m_split>0) + if (m_split > 0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]); + m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]); } } // -void btSoftBody::AJoint::Prepare(btScalar dt,int iterations) +void btSoftBody::AJoint::Prepare(btScalar dt, int iterations) { - static const btScalar maxdrift=SIMD_PI/16; + static const btScalar maxdrift = SIMD_PI / 16; m_icontrol->Prepare(this); - Joint::Prepare(dt,iterations); - m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; - m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; - m_drift = NormalizeAny(btCross(m_axis[1],m_axis[0])); - m_drift *= btMin(maxdrift,btAcos(Clamp(btDot(m_axis[0],m_axis[1]),-1,+1))); - m_drift *= m_erp/dt; - m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); - if(m_split>0) + Joint::Prepare(dt, iterations); + m_axis[0] = m_bodies[0].xform().getBasis() * m_refs[0]; + m_axis[1] = m_bodies[1].xform().getBasis() * m_refs[1]; + m_drift = NormalizeAny(btCross(m_axis[1], m_axis[0])); + m_drift *= btMin(maxdrift, btAcos(Clamp(btDot(m_axis[0], m_axis[1]), -1, +1))); + m_drift *= m_erp / dt; + m_massmatrix = AngularImpulseMatrix(m_bodies[0].invWorldInertia(), m_bodies[1].invWorldInertia()); + if (m_split > 0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix * (m_drift * m_split); + m_drift *= 1 - m_split; } - m_drift /=(btScalar)iterations; + m_drift /= (btScalar)iterations; } // -void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor) +void btSoftBody::AJoint::Solve(btScalar dt, btScalar sor) { - const btVector3 va=m_bodies[0].angularVelocity(); - const btVector3 vb=m_bodies[1].angularVelocity(); - const btVector3 vr=va-vb; - const btScalar sp=btDot(vr,m_axis[0]); - const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor; + const btVector3 va = m_bodies[0].angularVelocity(); + const btVector3 vb = m_bodies[1].angularVelocity(); + const btVector3 vr = va - vb; + const btScalar sp = btDot(vr, m_axis[0]); + const btVector3 vc = vr - m_axis[0] * m_icontrol->Speed(this, sp); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix * (m_drift + vc * m_cfm) * sor; m_bodies[0].applyAImpulse(-impulse); - m_bodies[1].applyAImpulse( impulse); + m_bodies[1].applyAImpulse(impulse); } // -void btSoftBody::AJoint::Terminate(btScalar dt) +void btSoftBody::AJoint::Terminate(btScalar dt) { - if(m_split>0) + if (m_split > 0) { m_bodies[0].applyDAImpulse(-m_sdrift); - m_bodies[1].applyDAImpulse( m_sdrift); + m_bodies[1].applyDAImpulse(m_sdrift); } } // -void btSoftBody::CJoint::Prepare(btScalar dt,int iterations) +void btSoftBody::CJoint::Prepare(btScalar dt, int iterations) { - Joint::Prepare(dt,iterations); - const bool dodrift=(m_life==0); - m_delete=(++m_life)>m_maxlife; - if(dodrift) + Joint::Prepare(dt, iterations); + const bool dodrift = (m_life == 0); + m_delete = (++m_life) > m_maxlife; + if (dodrift) { - m_drift=m_drift*m_erp/dt; - if(m_split>0) + m_drift = m_drift * m_erp / dt; + if (m_split > 0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix * (m_drift * m_split); + m_drift *= 1 - m_split; } - m_drift/=(btScalar)iterations; + m_drift /= (btScalar)iterations; } else { - m_drift=m_sdrift=btVector3(0,0,0); + m_drift = m_sdrift = btVector3(0, 0, 0); } } // -void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) +void btSoftBody::CJoint::Solve(btScalar dt, btScalar sor) { - const btVector3 va=m_bodies[0].velocity(m_rpos[0]); - const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); - const btVector3 vrel=va-vb; - const btScalar rvac=btDot(vrel,m_normal); - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_drift; - if(rvac<0) + const btVector3 va = m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb = m_bodies[1].velocity(m_rpos[1]); + const btVector3 vrel = va - vb; + const btScalar rvac = btDot(vrel, m_normal); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_drift; + if (rvac < 0) { - const btVector3 iv=m_normal*rvac; - const btVector3 fv=vrel-iv; - impulse.m_velocity += iv+fv*m_friction; + const btVector3 iv = m_normal * rvac; + const btVector3 fv = vrel - iv; + impulse.m_velocity += iv + fv * m_friction; } - impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; - - if (m_bodies[0].m_soft==m_bodies[1].m_soft) + impulse.m_velocity = m_massmatrix * impulse.m_velocity * sor; + + if (m_bodies[0].m_soft == m_bodies[1].m_soft) { - if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&& - (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ())) + if ((impulse.m_velocity.getX() == impulse.m_velocity.getX()) && (impulse.m_velocity.getY() == impulse.m_velocity.getY()) && + (impulse.m_velocity.getZ() == impulse.m_velocity.getZ())) { if (impulse.m_asVelocity) { - if (impulse.m_velocity.length() m_maxSelfCollisionImpulse) + if (impulse.m_velocity.length() < m_bodies[0].m_soft->m_maxSelfCollisionImpulse) { - - } else + } + else { - m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]); - m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]); + m_bodies[0].applyImpulse(-impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[0]); + m_bodies[1].applyImpulse(impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[1]); } } } - } else + } + else { - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); + m_bodies[0].applyImpulse(-impulse, m_rpos[0]); + m_bodies[1].applyImpulse(impulse, m_rpos[1]); } } // -void btSoftBody::CJoint::Terminate(btScalar dt) +void btSoftBody::CJoint::Terminate(btScalar dt) { - if(m_split>0) + if (m_split > 0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]); + m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]); } } // -void btSoftBody::applyForces() +void btSoftBody::applyForces() { - BT_PROFILE("SoftBody applyForces"); -// const btScalar dt = m_sst.sdt; - const btScalar kLF = m_cfg.kLF; - const btScalar kDG = m_cfg.kDG; - const btScalar kPR = m_cfg.kPR; - const btScalar kVC = m_cfg.kVC; - const bool as_lift = kLF>0; - const bool as_drag = kDG>0; - const bool as_pressure = kPR!=0; - const bool as_volume = kVC>0; - const bool as_aero = as_lift || - as_drag ; + // const btScalar dt = m_sst.sdt; + const btScalar kLF = m_cfg.kLF; + const btScalar kDG = m_cfg.kDG; + const btScalar kPR = m_cfg.kPR; + const btScalar kVC = m_cfg.kVC; + const bool as_lift = kLF > 0; + const bool as_drag = kDG > 0; + const bool as_pressure = kPR != 0; + const bool as_volume = kVC > 0; + const bool as_aero = as_lift || + as_drag; //const bool as_vaero = as_aero && // (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); //const bool as_faero = as_aero && // (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); - const bool use_medium = as_aero; - const bool use_volume = as_pressure || - as_volume ; - btScalar volume = 0; - btScalar ivolumetp = 0; - btScalar dvolumetv = 0; - btSoftBody::sMedium medium; - if(use_volume) + const bool use_medium = as_aero; + const bool use_volume = as_pressure || + as_volume; + btScalar volume = 0; + btScalar ivolumetp = 0; + btScalar dvolumetv = 0; + btSoftBody::sMedium medium; + if (use_volume) { - volume = getVolume(); - ivolumetp = 1/btFabs(volume)*kPR; - dvolumetv = (m_pose.m_volume-volume)*kVC; + volume = getVolume(); + ivolumetp = 1 / btFabs(volume) * kPR; + dvolumetv = (m_pose.m_volume - volume) * kVC; } - /* Per vertex forces */ - int i,ni; + /* Per vertex forces */ + int i, ni; - for(i=0,ni=m_nodes.size();i0) + btSoftBody::Node& n = m_nodes[i]; + if (n.m_im > 0) { - if(use_medium) + if (use_medium) { - /* Aerodynamics */ + /* Aerodynamics */ addAeroForceToNode(m_windVelocity, i); } - /* Pressure */ - if(as_pressure) + /* Pressure */ + if (as_pressure) { - n.m_f += n.m_n*(n.m_area*ivolumetp); + n.m_f += n.m_n * (n.m_area * ivolumetp); } - /* Volume */ - if(as_volume) + /* Volume */ + if (as_volume) { - n.m_f += n.m_n*(n.m_area*dvolumetv); + n.m_f += n.m_n * (n.m_area * dvolumetv); } } } - /* Per face forces */ - for(i=0,ni=m_faces.size();im_cfg.kAHR*kst; - const btScalar dt=psb->m_sst.sdt; - for(int i=0,ni=psb->m_anchors.size();im_cfg.kAHR * kst; + const btScalar dt = psb->m_sst.sdt; + for (int i = 0, ni = psb->m_anchors.size(); i < ni; ++i) { - const Anchor& a=psb->m_anchors[i]; - const btTransform& t=a.m_body->getWorldTransform(); - Node& n=*a.m_node; - const btVector3 wa=t*a.m_local; - const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt; - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; - const btVector3 impulse=a.m_c0*vr*a.m_influence; - n.m_x+=impulse*a.m_c2; - a.m_body->applyImpulse(-impulse,a.m_c1); + const Anchor& a = psb->m_anchors[i]; + const btTransform& t = a.m_body->getWorldTransform(); + Node& n = *a.m_node; + const btVector3 wa = t * a.m_local; + const btVector3 va = a.m_body->getVelocityInLocalPoint(a.m_c1) * dt; + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = (va - vb) + (wa - n.m_x) * kAHR; + const btVector3 impulse = a.m_c0 * vr * a.m_influence; + n.m_x += impulse * a.m_c2; + a.m_body->applyImpulse(-impulse, a.m_c1); } } - // void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) { BT_PROFILE("PSolve_RContacts"); - const btScalar dt = psb->m_sst.sdt; - const btScalar mrg = psb->getCollisionShape()->getMargin(); + const btScalar dt = psb->m_sst.sdt; + const btScalar mrg = psb->getCollisionShape()->getMargin(); btMultiBodyJacobianData jacobianData; - for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts.size(); i < ni; ++i) { - const RContact& c = psb->m_rcontacts[i]; - const sCti& cti = c.m_cti; - if (cti.m_colObj->hasContactResponse()) + const RContact& c = psb->m_rcontacts[i]; + const sCti& cti = c.m_cti; + if (cti.m_colObj->hasContactResponse()) { - 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) + btVector3 va(0, 0, 0); + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + btScalar* deltaV; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg ); - const btVector3 fv = vr - (cti.m_normal * dn); + 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) + { + const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); + const btVector3 fv = vr - (cti.m_normal * dn); // 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 ); + 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 (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); - } - } + + 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); + } + } } } } } // -void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,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.size(); i < ni; ++i) { - const SContact& c=psb->m_scontacts[i]; - const btVector3& nr=c.m_normal; - Node& n=*c.m_node; - Face& f=*c.m_face; - const btVector3 p=BaryEval( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - c.m_weights); - const btVector3 q=BaryEval( f.m_n[0]->m_q, - f.m_n[1]->m_q, - f.m_n[2]->m_q, - c.m_weights); - const btVector3 vr=(n.m_x-n.m_q)-(p-q); - btVector3 corr(0,0,0); - btScalar dot = btDot(vr,nr); - if(dot<0) + const SContact& c = psb->m_scontacts[i]; + const btVector3& nr = c.m_normal; + Node& n = *c.m_node; + Face& f = *c.m_face; + const btVector3 p = BaryEval(f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + c.m_weights); + const btVector3 q = BaryEval(f.m_n[0]->m_q, + f.m_n[1]->m_q, + f.m_n[2]->m_q, + c.m_weights); + const btVector3 vr = (n.m_x - n.m_q) - (p - q); + btVector3 corr(0, 0, 0); + btScalar dot = btDot(vr, nr); + if (dot < 0) { - const btScalar j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p)); - corr+=c.m_normal*j; + const btScalar j = c.m_margin - (btDot(nr, n.m_x) - btDot(nr, p)); + corr += c.m_normal * j; } - corr -= ProjectOnPlane(vr,nr)*c.m_friction; - n.m_x += corr*c.m_cfm[0]; - f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); - f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); - f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); + corr -= ProjectOnPlane(vr, nr) * c.m_friction; + n.m_x += corr * c.m_cfm[0]; + f.m_n[0]->m_x -= corr * (c.m_cfm[1] * c.m_weights.x()); + f.m_n[1]->m_x -= corr * (c.m_cfm[1] * c.m_weights.y()); + f.m_n[2]->m_x -= corr * (c.m_cfm[1] * c.m_weights.z()); } } // -void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,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]; - if(l.m_c0>0) + BT_PROFILE("PSolve_Links"); + for (int i = 0, ni = psb->m_links.size(); i < ni; ++i) + { + Link& l = psb->m_links[i]; + if (l.m_c0 > 0) { - Node& a=*l.m_n[0]; - Node& b=*l.m_n[1]; - const btVector3 del=b.m_x-a.m_x; - const btScalar len=del.length2(); - if (l.m_c1+len > SIMD_EPSILON) + Node& a = *l.m_n[0]; + Node& b = *l.m_n[1]; + const btVector3 del = b.m_x - a.m_x; + const btScalar len = del.length2(); + if (l.m_c1 + len > SIMD_EPSILON) { - const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst; - a.m_x-=del*(k*a.m_im); - b.m_x+=del*(k*b.m_im); + const btScalar k = ((l.m_c1 - len) / (l.m_c0 * (l.m_c1 + len))) * kst; + a.m_x -= del * (k * a.m_im); + b.m_x += del * (k * b.m_im); } } } } // -void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) +void btSoftBody::VSolve_Links(btSoftBody* psb, btScalar kst) { BT_PROFILE("VSolve_Links"); - for(int i=0,ni=psb->m_links.size();im_links[i]; - Node** n=l.m_n; - const btScalar j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; - n[0]->m_v+= l.m_c3*(j*n[0]->m_im); - n[1]->m_v-= l.m_c3*(j*n[1]->m_im); + for (int i = 0, ni = psb->m_links.size(); i < ni; ++i) + { + Link& l = psb->m_links[i]; + Node** n = l.m_n; + const btScalar j = -btDot(l.m_c3, n[0]->m_v - n[1]->m_v) * l.m_c2 * kst; + n[0]->m_v += l.m_c3 * (j * n[0]->m_im); + n[1]->m_v -= l.m_c3 * (j * n[1]->m_im); } } // -btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) +btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) { - switch(solver) + switch (solver) { - case ePSolver::Anchors: - return(&btSoftBody::PSolve_Anchors); - case ePSolver::Linear: - return(&btSoftBody::PSolve_Links); - case ePSolver::RContacts: - return(&btSoftBody::PSolve_RContacts); - case ePSolver::SContacts: - return(&btSoftBody::PSolve_SContacts); + case ePSolver::Anchors: + return (&btSoftBody::PSolve_Anchors); + case ePSolver::Linear: + return (&btSoftBody::PSolve_Links); + case ePSolver::RContacts: + return (&btSoftBody::PSolve_RContacts); + case ePSolver::SContacts: + return (&btSoftBody::PSolve_SContacts); default: { } } - return(0); + return (0); } // -btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) +btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) { - switch(solver) + switch (solver) { - case eVSolver::Linear: return(&btSoftBody::VSolve_Links); + case eVSolver::Linear: + return (&btSoftBody::VSolve_Links); default: { } } - return(0); + return (0); } // -void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap) +void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap) { - - switch(m_cfg.collisions&fCollision::RVSmask) + switch (m_cfg.collisions & fCollision::RVSmask) { - case fCollision::SDF_RS: + case fCollision::SDF_RS: { - btSoftColliders::CollideSDF_RS docollide; - btRigidBody* prb1=(btRigidBody*) btRigidBody::upcast(pcoWrap->getCollisionObject()); - btTransform wtr=pcoWrap->getWorldTransform(); + btSoftColliders::CollideSDF_RS docollide; + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + btTransform wtr = pcoWrap->getWorldTransform(); - const btTransform ctr=pcoWrap->getWorldTransform(); - const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); - const btScalar basemargin=getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pcoWrap->getCollisionShape()->getAabb( pcoWrap->getWorldTransform(), - mins, - maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(basemargin,basemargin,basemargin)); - docollide.psb = this; + const btTransform ctr = pcoWrap->getWorldTransform(); + const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length(); + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(), + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); + docollide.psb = this; docollide.m_colObj1Wrap = pcoWrap; docollide.m_rigidBody = prb1; - docollide.dynmargin = basemargin+timemargin; - docollide.stamargin = basemargin; - m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); + docollide.dynmargin = basemargin + timemargin; + docollide.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide); } break; - case fCollision::CL_RS: + case fCollision::CL_RS: { - btSoftColliders::CollideCL_RS collider; - collider.ProcessColObj(this,pcoWrap); + btSoftColliders::CollideCL_RS collider; + collider.ProcessColObj(this, pcoWrap); } break; } } // -void btSoftBody::defaultCollisionHandler(btSoftBody* psb) +void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { - const int cf=m_cfg.collisions&psb->m_cfg.collisions; - switch(cf&fCollision::SVSmask) + const int cf = m_cfg.collisions & psb->m_cfg.collisions; + switch (cf & fCollision::SVSmask) { - case fCollision::CL_SS: + case fCollision::CL_SS: { - //support self-collision if CL_SELF flag set - if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF) + if (this != psb || psb->m_cfg.collisions & fCollision::CL_SELF) { - btSoftColliders::CollideCL_SS docollide; - docollide.ProcessSoftSoft(this,psb); + btSoftColliders::CollideCL_SS docollide; + docollide.ProcessSoftSoft(this, psb); } - } break; - case fCollision::VF_SS: + case fCollision::VF_SS: { //only self-collision for Cluster, not Vertex-Face yet - if (this!=psb) + if (this != psb) { - btSoftColliders::CollideVF_SS docollide; - /* common */ - docollide.mrg= getCollisionShape()->getMargin()+ - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - docollide.psb[0]=this; - docollide.psb[1]=psb; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - /* psb1 nodes vs psb0 faces */ - docollide.psb[0]=psb; - docollide.psb[1]=this; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + btSoftColliders::CollideVF_SS docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); } } break; - default: + default: { - } } } - - -void btSoftBody::setWindVelocity( const btVector3 &velocity ) +void btSoftBody::setWindVelocity(const btVector3& velocity) { m_windVelocity = velocity; } - const btVector3& btSoftBody::getWindVelocity() { return m_windVelocity; } - - -int btSoftBody::calculateSerializeBufferSize() const +int btSoftBody::calculateSerializeBufferSize() const { int sz = sizeof(btSoftBodyData); return sz; } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const { - btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer; + btSoftBodyData* sbd = (btSoftBodyData*)dataBuffer; btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer); - btHashMap m_nodeIndexMap; + btHashMap m_nodeIndexMap; sbd->m_numMaterials = m_materials.size(); - sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0; + sbd->m_materials = sbd->m_numMaterials ? (SoftBodyMaterialData**)serializer->getUniquePointer((void*)&m_materials) : 0; if (sbd->m_materials) { int sz = sizeof(SoftBodyMaterialData*); int numElem = sbd->m_numMaterials; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); //SoftBodyMaterialData** memPtr = chunk->m_oldPtr; SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr; - for (int i=0;igetUniquePointer((void*)mat) : 0; if (!serializer->findPointer(mat)) { //serialize it here - btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1); + btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData), 1); SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr; memPtr->m_flags = mat->m_flags; memPtr->m_angularStiffness = mat->m_kAST; memPtr->m_linearStiffness = mat->m_kLST; memPtr->m_volumeStiffness = mat->m_kVST; - serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat); + serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_SBMATERIAL_CODE, mat); } } - serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials); + serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_ARRAY_CODE, (void*)&m_materials); } - - - sbd->m_numNodes = m_nodes.size(); - sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0; + sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes) : 0; if (sbd->m_nodes) { int sz = sizeof(SoftBodyNodeData); int numElem = sbd->m_numNodes; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr; - for (int i=0;im_accumulatedForce); + m_nodes[i].m_f.serializeFloat(memPtr->m_accumulatedForce); memPtr->m_area = m_nodes[i].m_area; memPtr->m_attach = m_nodes[i].m_battach; memPtr->m_inverseMass = m_nodes[i].m_im; - memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0; + memPtr->m_material = m_nodes[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_nodes[i].m_material) : 0; m_nodes[i].m_n.serializeFloat(memPtr->m_normal); m_nodes[i].m_x.serializeFloat(memPtr->m_position); m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition); m_nodes[i].m_v.serializeFloat(memPtr->m_velocity); - m_nodeIndexMap.insert(&m_nodes[i],i); + m_nodeIndexMap.insert(&m_nodes[i], i); } - serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes); + serializer->finalizeChunk(chunk, "SoftBodyNodeData", BT_SBNODE_CODE, (void*)&m_nodes); } sbd->m_numLinks = m_links.size(); - sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; + sbd->m_links = sbd->m_numLinks ? (SoftBodyLinkData*)serializer->getUniquePointer((void*)&m_links[0]) : 0; if (sbd->m_links) { int sz = sizeof(SoftBodyLinkData); int numElem = sbd->m_numLinks; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr; - for (int i=0;im_bbending = m_links[i].m_bbending; - memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0; - memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1; - memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1; - btAssert(memPtr->m_nodeIndices[0]m_nodeIndices[1]m_material = m_links[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_links[i].m_material) : 0; + memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0] : -1; + memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0] : -1; + btAssert(memPtr->m_nodeIndices[0] < m_nodes.size()); + btAssert(memPtr->m_nodeIndices[1] < m_nodes.size()); memPtr->m_restLength = m_links[i].m_rl; } - serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]); - + serializer->finalizeChunk(chunk, "SoftBodyLinkData", BT_ARRAY_CODE, (void*)&m_links[0]); } - sbd->m_numFaces = m_faces.size(); - sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0; + sbd->m_faces = sbd->m_numFaces ? (SoftBodyFaceData*)serializer->getUniquePointer((void*)&m_faces[0]) : 0; if (sbd->m_faces) { int sz = sizeof(SoftBodyFaceData); int numElem = sbd->m_numFaces; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr; - for (int i=0;im_material = m_faces[i].m_material ? (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0; - m_faces[i].m_normal.serializeFloat( memPtr->m_normal); - for (int j=0;j<3;j++) + memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_faces[i].m_material) : 0; + m_faces[i].m_normal.serializeFloat(memPtr->m_normal); + for (int j = 0; j < 3; j++) { - memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1; + memPtr->m_nodeIndices[j] = m_faces[i].m_n[j] ? m_faces[i].m_n[j] - &m_nodes[0] : -1; } memPtr->m_restArea = m_faces[i].m_ra; } - serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]); + serializer->finalizeChunk(chunk, "SoftBodyFaceData", BT_ARRAY_CODE, (void*)&m_faces[0]); } - sbd->m_numTetrahedra = m_tetras.size(); - sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0; + sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*)serializer->getUniquePointer((void*)&m_tetras[0]) : 0; if (sbd->m_tetrahedra) { int sz = sizeof(SoftBodyTetraData); int numElem = sbd->m_numTetrahedra; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr; - for (int i=0;im_c0[j] ); - memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1; + m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]); + memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j] ? m_tetras[j].m_n[j] - &m_nodes[0] : -1; } memPtr->m_c1 = m_tetras[i].m_c1; memPtr->m_c2 = m_tetras[i].m_c2; - memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0; + memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_tetras[i].m_material) : 0; memPtr->m_restVolume = m_tetras[i].m_rv; } - serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]); + serializer->finalizeChunk(chunk, "SoftBodyTetraData", BT_ARRAY_CODE, (void*)&m_tetras[0]); } sbd->m_numAnchors = m_anchors.size(); - sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0; + sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*)serializer->getUniquePointer((void*)&m_anchors[0]) : 0; if (sbd->m_anchors) { int sz = sizeof(SoftRigidAnchorData); int numElem = sbd->m_numAnchors; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr; - for (int i=0;im_c0); m_anchors[i].m_c1.serializeFloat(memPtr->m_c1); memPtr->m_c2 = m_anchors[i].m_c2; m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame); - memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1; - - memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*) serializer->getUniquePointer((void*)m_anchors[i].m_body): 0; + memPtr->m_nodeIndex = m_anchors[i].m_node ? m_anchors[i].m_node - &m_nodes[0] : -1; + + memPtr->m_rigidBody = m_anchors[i].m_body ? (btRigidBodyData*)serializer->getUniquePointer((void*)m_anchors[i].m_body) : 0; btAssert(memPtr->m_nodeIndex < m_nodes.size()); } - serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]); + serializer->finalizeChunk(chunk, "SoftRigidAnchorData", BT_ARRAY_CODE, (void*)&m_anchors[0]); } - sbd->m_config.m_dynamicFriction = m_cfg.kDF; sbd->m_config.m_baumgarte = m_cfg.kVCF; @@ -3501,64 +3499,63 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose); int sz = sizeof(SoftBodyPoseData); - btChunk* chunk = serializer->allocate(sz,1); + btChunk* chunk = serializer->allocate(sz, 1); SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr; - + m_pose.m_aqq.serializeFloat(memPtr->m_aqq); memPtr->m_bframe = m_pose.m_bframe; memPtr->m_bvolume = m_pose.m_bvolume; m_pose.m_com.serializeFloat(memPtr->m_com); - + memPtr->m_numPositions = m_pose.m_pos.size(); - memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0; + memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]) : 0; if (memPtr->m_numPositions) { int numElem = memPtr->m_numPositions; int sz = sizeof(btVector3Data); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr; - for (int i=0;ifinalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]); + serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_pose.m_pos[0]); } memPtr->m_restVolume = m_pose.m_volume; m_pose.m_rot.serializeFloat(memPtr->m_rot); m_pose.m_scl.serializeFloat(memPtr->m_scale); memPtr->m_numWeigts = m_pose.m_wgh.size(); - memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0; + memPtr->m_weights = memPtr->m_numWeigts ? (float*)serializer->getUniquePointer((void*)&m_pose.m_wgh[0]) : 0; if (memPtr->m_numWeigts) { - int numElem = memPtr->m_numWeigts; int sz = sizeof(float); - btChunk* chunk = serializer->allocate(sz,numElem); - float* memPtr = (float*) chunk->m_oldPtr; - for (int i=0;iallocate(sz, numElem); + float* memPtr = (float*)chunk->m_oldPtr; + for (int i = 0; i < numElem; i++, memPtr++) { *memPtr = m_pose.m_wgh[i]; } - serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]); + serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_pose.m_wgh[0]); } - serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose); + serializer->finalizeChunk(chunk, "SoftBodyPoseData", BT_ARRAY_CODE, (void*)&m_pose); } //clusters for convex-cluster collision detection sbd->m_numClusters = m_clusters.size(); - sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0; + sbd->m_clusters = sbd->m_numClusters ? (SoftBodyClusterData*)serializer->getUniquePointer((void*)m_clusters[0]) : 0; if (sbd->m_numClusters) { int numElem = sbd->m_numClusters; int sz = sizeof(SoftBodyClusterData); - btChunk* chunk = serializer->allocate(sz,numElem); - SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr; - for (int i=0;iallocate(sz, numElem); + SoftBodyClusterData* memPtr = (SoftBodyClusterData*)chunk->m_oldPtr; + for (int i = 0; i < numElem; i++, memPtr++) { - memPtr->m_adamping= m_clusters[i]->m_adamping; + memPtr->m_adamping = m_clusters[i]->m_adamping; m_clusters[i]->m_av.serializeFloat(memPtr->m_av); memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex; memPtr->m_collide = m_clusters[i]->m_collide; @@ -3589,69 +3586,64 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]); memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses; - - - memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0; + memPtr->m_framerefs = memPtr->m_numFrameRefs ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0; if (memPtr->m_framerefs) { int numElem = memPtr->m_numFrameRefs; int sz = sizeof(btVector3FloatData); - btChunk* chunk = serializer->allocate(sz,numElem); - btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr; - for (int j=0;jallocate(sz, numElem); + btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr; + for (int j = 0; j < numElem; j++, memPtr++) { m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr); } - serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]); + serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_framerefs[0]); } - - memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0; + + memPtr->m_masses = memPtr->m_numMasses ? (float*)serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]) : 0; if (memPtr->m_masses) { int numElem = memPtr->m_numMasses; int sz = sizeof(float); - btChunk* chunk = serializer->allocate(sz,numElem); - float* memPtr = (float*) chunk->m_oldPtr; - for (int j=0;jallocate(sz, numElem); + float* memPtr = (float*)chunk->m_oldPtr; + for (int j = 0; j < numElem; j++, memPtr++) { *memPtr = m_clusters[i]->m_masses[j]; } - serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]); + serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_masses[0]); } - memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0; - if (memPtr->m_nodeIndices ) + memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*)serializer->getUniquePointer((void*)&m_clusters[i]->m_nodes) : 0; + if (memPtr->m_nodeIndices) { int numElem = memPtr->m_numMasses; int sz = sizeof(int); - btChunk* chunk = serializer->allocate(sz,numElem); - int* memPtr = (int*) chunk->m_oldPtr; - for (int j=0;jallocate(sz, numElem); + int* memPtr = (int*)chunk->m_oldPtr; + for (int j = 0; j < numElem; j++, memPtr++) { int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]); btAssert(indexPtr); *memPtr = *indexPtr; } - serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes); + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_nodes); } } - serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]); - + serializer->finalizeChunk(chunk, "SoftBodyClusterData", BT_ARRAY_CODE, (void*)m_clusters[0]); } - - sbd->m_numJoints = m_joints.size(); - sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0; + sbd->m_joints = m_joints.size() ? (btSoftBodyJointData*)serializer->getUniquePointer((void*)&m_joints[0]) : 0; if (sbd->m_joints) { int sz = sizeof(btSoftBodyJointData); int numElem = m_joints.size(); - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr; - for (int i=0;im_jointType = (int)m_joints[i]->Type(); m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]); @@ -3660,8 +3652,8 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ memPtr->m_erp = float(m_joints[i]->m_erp); memPtr->m_split = float(m_joints[i]->m_split); memPtr->m_delete = m_joints[i]->m_delete; - - for (int j=0;j<4;j++) + + for (int j = 0; j < 4; j++) { memPtr->m_relPosition[0].m_floats[j] = 0.f; memPtr->m_relPosition[1].m_floats[j] = 0.f; @@ -3700,10 +3692,8 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid); } } - serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]); + serializer->finalizeChunk(chunk, "btSoftBodyJointData", BT_ARRAY_CODE, (void*)&m_joints[0]); } - return btSoftBodyDataName; } - diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBody.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftBody.h index ada0dfd1a..9b35b799d 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBody.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBody.h @@ -31,862 +31,890 @@ subject to the following restrictions: //#define btRigidBodyData btRigidBodyDoubleData //#define btRigidBodyDataName "btRigidBodyDoubleData" //#else -#define btSoftBodyData btSoftBodyFloatData -#define btSoftBodyDataName "btSoftBodyFloatData" +#define btSoftBodyData btSoftBodyFloatData +#define btSoftBodyDataName "btSoftBodyFloatData" //#endif //BT_USE_DOUBLE_PRECISION class btBroadphaseInterface; class btDispatcher; class btSoftBodySolver; -/* btSoftBodyWorldInfo */ -struct btSoftBodyWorldInfo +/* btSoftBodyWorldInfo */ +struct btSoftBodyWorldInfo { - btScalar air_density; - btScalar water_density; - btScalar water_offset; - btScalar m_maxDisplacement; - btVector3 water_normal; - btBroadphaseInterface* m_broadphase; - btDispatcher* m_dispatcher; - btVector3 m_gravity; - btSparseSdf<3> m_sparsesdf; + btScalar air_density; + btScalar water_density; + btScalar water_offset; + btScalar m_maxDisplacement; + btVector3 water_normal; + btBroadphaseInterface* m_broadphase; + btDispatcher* m_dispatcher; + btVector3 m_gravity; + btSparseSdf<3> m_sparsesdf; btSoftBodyWorldInfo() - :air_density((btScalar)1.2), - water_density(0), - water_offset(0), - m_maxDisplacement(1000.f),//avoid soft body from 'exploding' so use some upper threshold of maximum motion that a node can travel per frame - water_normal(0,0,0), - m_broadphase(0), - m_dispatcher(0), - m_gravity(0,-10,0) + : air_density((btScalar)1.2), + water_density(0), + water_offset(0), + m_maxDisplacement(1000.f), //avoid soft body from 'exploding' so use some upper threshold of maximum motion that a node can travel per frame + water_normal(0, 0, 0), + m_broadphase(0), + m_dispatcher(0), + m_gravity(0, -10, 0) { } -}; +}; - -///The btSoftBody is an class to simulate cloth and volumetric soft bodies. +///The btSoftBody is an class to simulate cloth and volumetric soft bodies. ///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject. -class btSoftBody : public btCollisionObject +class btSoftBody : public btCollisionObject { public: btAlignedObjectArray m_collisionDisabledObjects; // The solver object that handles this soft body - btSoftBodySolver *m_softBodySolver; + btSoftBodySolver* m_softBodySolver; // // Enumerations // - ///eAeroModel - struct eAeroModel { enum _ { - V_Point, ///Vertex normals are oriented toward velocity - V_TwoSided, ///Vertex normals are flipped to match velocity - V_TwoSidedLiftDrag, ///Vertex normals are flipped to match velocity and lift and drag forces are applied - V_OneSided, ///Vertex normals are taken as it is - F_TwoSided, ///Face normals are flipped to match velocity - F_TwoSidedLiftDrag, ///Face normals are flipped to match velocity and lift and drag forces are applied - F_OneSided, ///Face normals are taken as it is - END - };}; + ///eAeroModel + struct eAeroModel + { + enum _ + { + V_Point, ///Vertex normals are oriented toward velocity + V_TwoSided, ///Vertex normals are flipped to match velocity + V_TwoSidedLiftDrag, ///Vertex normals are flipped to match velocity and lift and drag forces are applied + V_OneSided, ///Vertex normals are taken as it is + F_TwoSided, ///Face normals are flipped to match velocity + F_TwoSidedLiftDrag, ///Face normals are flipped to match velocity and lift and drag forces are applied + F_OneSided, ///Face normals are taken as it is + END + }; + }; ///eVSolver : velocities solvers - struct eVSolver { enum _ { - Linear, ///Linear solver - END - };}; + struct eVSolver + { + enum _ + { + Linear, ///Linear solver + END + }; + }; ///ePSolver : positions solvers - struct ePSolver { enum _ { - Linear, ///Linear solver - Anchors, ///Anchor solver - RContacts, ///Rigid contacts solver - SContacts, ///Soft contacts solver - END - };}; + struct ePSolver + { + enum _ + { + Linear, ///Linear solver + Anchors, ///Anchor solver + RContacts, ///Rigid contacts solver + SContacts, ///Soft contacts solver + END + }; + }; ///eSolverPresets - struct eSolverPresets { enum _ { - Positions, - Velocities, - Default = Positions, - END - };}; + struct eSolverPresets + { + enum _ + { + Positions, + Velocities, + Default = Positions, + END + }; + }; ///eFeature - struct eFeature { enum _ { - None, - Node, - Link, - Face, - Tetra, - END - };}; + struct eFeature + { + enum _ + { + None, + Node, + Link, + Face, + Tetra, + END + }; + }; - typedef btAlignedObjectArray tVSolverArray; - typedef btAlignedObjectArray tPSolverArray; + typedef btAlignedObjectArray tVSolverArray; + typedef btAlignedObjectArray tPSolverArray; // // Flags // ///fCollision - struct fCollision { enum _ { - RVSmask = 0x000f, ///Rigid versus soft mask - SDF_RS = 0x0001, ///SDF based rigid vs soft - CL_RS = 0x0002, ///Cluster vs convex rigid vs soft + struct fCollision + { + enum _ + { + RVSmask = 0x000f, ///Rigid versus soft mask + SDF_RS = 0x0001, ///SDF based rigid vs soft + CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - SVSmask = 0x0030, ///Rigid versus soft mask - VF_SS = 0x0010, ///Vertex vs face soft vs soft handling - CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling - CL_SELF = 0x0040, ///Cluster soft body self collision - /* presets */ - Default = SDF_RS, - END - };}; + SVSmask = 0x0030, ///Rigid versus soft mask + VF_SS = 0x0010, ///Vertex vs face soft vs soft handling + CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling + CL_SELF = 0x0040, ///Cluster soft body self collision + /* presets */ + Default = SDF_RS, + END + }; + }; ///fMaterial - struct fMaterial { enum _ { - DebugDraw = 0x0001, /// Enable debug draw - /* presets */ - Default = DebugDraw, - END - };}; + struct fMaterial + { + enum _ + { + DebugDraw = 0x0001, /// Enable debug draw + /* presets */ + Default = DebugDraw, + END + }; + }; // // API Types // - /* sRayCast */ + /* sRayCast */ struct sRayCast { - btSoftBody* body; /// soft body - eFeature::_ feature; /// feature type - int index; /// feature index - btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction) + btSoftBody* body; /// soft body + eFeature::_ feature; /// feature type + int index; /// feature index + btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction) }; - /* ImplicitFn */ - struct ImplicitFn + /* ImplicitFn */ + struct ImplicitFn { virtual ~ImplicitFn() {} - virtual btScalar Eval(const btVector3& x)=0; + virtual btScalar Eval(const btVector3& x) = 0; }; // // Internal types // - typedef btAlignedObjectArray tScalarArray; - typedef btAlignedObjectArray tVector3Array; + typedef btAlignedObjectArray tScalarArray; + typedef btAlignedObjectArray tVector3Array; - /* sCti is Softbody contact info */ - struct sCti + /* sCti is Softbody contact info */ + struct sCti { - const btCollisionObject* m_colObj; /* Rigid body */ - btVector3 m_normal; /* Outward normal */ - btScalar m_offset; /* Offset from origin */ - }; - - /* sMedium */ - struct sMedium - { - btVector3 m_velocity; /* Velocity */ - btScalar m_pressure; /* Pressure */ - btScalar m_density; /* Density */ + const btCollisionObject* m_colObj; /* Rigid body */ + btVector3 m_normal; /* Outward normal */ + btScalar m_offset; /* Offset from origin */ }; - /* Base type */ - struct Element + /* sMedium */ + struct sMedium { - void* m_tag; // User data + btVector3 m_velocity; /* Velocity */ + btScalar m_pressure; /* Pressure */ + btScalar m_density; /* Density */ + }; + + /* Base type */ + struct Element + { + void* m_tag; // User data Element() : m_tag(0) {} }; - /* Material */ - struct Material : Element + /* Material */ + struct Material : Element { - btScalar m_kLST; // Linear stiffness coefficient [0,1] - btScalar m_kAST; // Area/Angular stiffness coefficient [0,1] - btScalar m_kVST; // Volume stiffness coefficient [0,1] - int m_flags; // Flags + btScalar m_kLST; // Linear stiffness coefficient [0,1] + btScalar m_kAST; // Area/Angular stiffness coefficient [0,1] + btScalar m_kVST; // Volume stiffness coefficient [0,1] + int m_flags; // Flags }; - /* Feature */ - struct Feature : Element + /* Feature */ + struct Feature : Element { - Material* m_material; // Material + Material* m_material; // Material }; - /* Node */ - struct Node : Feature + /* Node */ + struct Node : Feature { - btVector3 m_x; // Position - btVector3 m_q; // Previous step position - btVector3 m_v; // Velocity - btVector3 m_f; // Force accumulator - btVector3 m_n; // Normal - btScalar m_im; // 1/mass - btScalar m_area; // Area - btDbvtNode* m_leaf; // Leaf data - int m_battach:1; // Attached + btVector3 m_x; // Position + btVector3 m_q; // Previous step position + btVector3 m_v; // Velocity + btVector3 m_f; // Force accumulator + btVector3 m_n; // Normal + btScalar m_im; // 1/mass + btScalar m_area; // Area + btDbvtNode* m_leaf; // Leaf data + int m_battach : 1; // Attached }; - /* Link */ - ATTRIBUTE_ALIGNED16(struct) Link : Feature + /* Link */ + 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 + 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 + BT_DECLARE_ALIGNED_ALLOCATOR(); - }; - /* Face */ - struct Face : Feature + /* Face */ + struct Face : Feature { - Node* m_n[3]; // Node pointers - btVector3 m_normal; // Normal - btScalar m_ra; // Rest area - btDbvtNode* m_leaf; // Leaf data + Node* m_n[3]; // Node pointers + btVector3 m_normal; // Normal + btScalar m_ra; // Rest area + btDbvtNode* m_leaf; // Leaf data }; - /* Tetra */ - struct Tetra : Feature + /* Tetra */ + struct Tetra : Feature { - Node* m_n[4]; // Node pointers - btScalar m_rv; // Rest volume - btDbvtNode* m_leaf; // Leaf data - btVector3 m_c0[4]; // gradients - btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) - btScalar m_c2; // m_c1/sum(|g0..3|^2) + Node* m_n[4]; // Node pointers + btScalar m_rv; // Rest volume + btDbvtNode* m_leaf; // Leaf data + btVector3 m_c0[4]; // gradients + btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) + btScalar m_c2; // m_c1/sum(|g0..3|^2) }; - /* RContact */ - struct RContact + /* RContact */ + struct RContact { - sCti m_cti; // Contact infos - Node* m_node; // Owner node - btMatrix3x3 m_c0; // Impulse matrix - btVector3 m_c1; // Relative anchor - btScalar m_c2; // ima*dt - btScalar m_c3; // Friction - btScalar m_c4; // Hardness + sCti m_cti; // Contact infos + Node* m_node; // Owner node + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // ima*dt + btScalar m_c3; // Friction + btScalar m_c4; // Hardness }; - /* SContact */ - struct SContact + /* SContact */ + struct SContact { - Node* m_node; // Node - Face* m_face; // Face - btVector3 m_weights; // Weigths - btVector3 m_normal; // Normal - btScalar m_margin; // Margin - btScalar m_friction; // Friction - btScalar m_cfm[2]; // Constraint force mixing + Node* m_node; // Node + Face* m_face; // Face + btVector3 m_weights; // Weigths + btVector3 m_normal; // Normal + btScalar m_margin; // Margin + btScalar m_friction; // Friction + btScalar m_cfm[2]; // Constraint force mixing }; - /* Anchor */ - struct Anchor + /* Anchor */ + struct Anchor { - Node* m_node; // Node pointer - btVector3 m_local; // Anchor position in body space - btRigidBody* m_body; // Body - btScalar m_influence; - btMatrix3x3 m_c0; // Impulse matrix - btVector3 m_c1; // Relative anchor - btScalar m_c2; // ima*dt + Node* m_node; // Node pointer + btVector3 m_local; // Anchor position in body space + btRigidBody* m_body; // Body + btScalar m_influence; + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // ima*dt }; - /* Note */ - struct Note : Element + /* Note */ + struct Note : Element { - const char* m_text; // Text - btVector3 m_offset; // Offset - int m_rank; // Rank - Node* m_nodes[4]; // Nodes - btScalar m_coords[4]; // Coordinates - }; - /* Pose */ - struct Pose - { - bool m_bvolume; // Is valid - bool m_bframe; // Is frame - btScalar m_volume; // Rest volume - tVector3Array m_pos; // Reference positions - tScalarArray m_wgh; // Weights - btVector3 m_com; // COM - btMatrix3x3 m_rot; // Rotation - btMatrix3x3 m_scl; // Scale - btMatrix3x3 m_aqq; // Base scaling + const char* m_text; // Text + btVector3 m_offset; // Offset + int m_rank; // Rank + Node* m_nodes[4]; // Nodes + btScalar m_coords[4]; // Coordinates }; - /* Cluster */ - struct Cluster + /* Pose */ + struct Pose { - tScalarArray m_masses; - btAlignedObjectArray m_nodes; - tVector3Array m_framerefs; - btTransform m_framexform; - btScalar m_idmass; - btScalar m_imass; - btMatrix3x3 m_locii; - btMatrix3x3 m_invwi; - btVector3 m_com; - btVector3 m_vimpulses[2]; - btVector3 m_dimpulses[2]; - int m_nvimpulses; - int m_ndimpulses; - btVector3 m_lv; - btVector3 m_av; - btDbvtNode* m_leaf; - btScalar m_ndamping; /* Node damping */ - btScalar m_ldamping; /* Linear damping */ - btScalar m_adamping; /* Angular damping */ - btScalar m_matching; - btScalar m_maxSelfCollisionImpulse; - btScalar m_selfCollisionImpulseFactor; - bool m_containsAnchor; - bool m_collide; - int m_clusterIndex; - Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) - ,m_maxSelfCollisionImpulse(100.f), - m_selfCollisionImpulseFactor(0.01f), - m_containsAnchor(false) - {} + bool m_bvolume; // Is valid + bool m_bframe; // Is frame + btScalar m_volume; // Rest volume + tVector3Array m_pos; // Reference positions + tScalarArray m_wgh; // Weights + btVector3 m_com; // COM + btMatrix3x3 m_rot; // Rotation + btMatrix3x3 m_scl; // Scale + btMatrix3x3 m_aqq; // Base scaling }; - /* Impulse */ - struct Impulse + /* Cluster */ + struct Cluster { - btVector3 m_velocity; - btVector3 m_drift; - int m_asVelocity:1; - int m_asDrift:1; - Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {} - Impulse operator -() const + tScalarArray m_masses; + btAlignedObjectArray m_nodes; + tVector3Array m_framerefs; + btTransform m_framexform; + btScalar m_idmass; + btScalar m_imass; + btMatrix3x3 m_locii; + btMatrix3x3 m_invwi; + btVector3 m_com; + btVector3 m_vimpulses[2]; + btVector3 m_dimpulses[2]; + int m_nvimpulses; + int m_ndimpulses; + btVector3 m_lv; + btVector3 m_av; + btDbvtNode* m_leaf; + btScalar m_ndamping; /* Node damping */ + btScalar m_ldamping; /* Linear damping */ + btScalar m_adamping; /* Angular damping */ + btScalar m_matching; + btScalar m_maxSelfCollisionImpulse; + btScalar m_selfCollisionImpulseFactor; + bool m_containsAnchor; + bool m_collide; + int m_clusterIndex; + Cluster() : m_leaf(0), m_ndamping(0), m_ldamping(0), m_adamping(0), m_matching(0), m_maxSelfCollisionImpulse(100.f), m_selfCollisionImpulseFactor(0.01f), m_containsAnchor(false) { - Impulse i=*this; - i.m_velocity=-i.m_velocity; - i.m_drift=-i.m_drift; - return(i); - } - Impulse operator*(btScalar x) const - { - Impulse i=*this; - i.m_velocity*=x; - i.m_drift*=x; - return(i); } }; - /* Body */ - struct Body + /* Impulse */ + struct Impulse { - Cluster* m_soft; - btRigidBody* m_rigid; - const btCollisionObject* m_collisionObject; + btVector3 m_velocity; + btVector3 m_drift; + int m_asVelocity : 1; + int m_asDrift : 1; + Impulse() : m_velocity(0, 0, 0), m_drift(0, 0, 0), m_asVelocity(0), m_asDrift(0) {} + Impulse operator-() const + { + Impulse i = *this; + i.m_velocity = -i.m_velocity; + i.m_drift = -i.m_drift; + return (i); + } + Impulse operator*(btScalar x) const + { + Impulse i = *this; + i.m_velocity *= x; + i.m_drift *= x; + return (i); + } + }; + /* Body */ + struct Body + { + Cluster* m_soft; + btRigidBody* m_rigid; + const btCollisionObject* m_collisionObject; - Body() : m_soft(0),m_rigid(0),m_collisionObject(0) {} - Body(Cluster* p) : m_soft(p),m_rigid(0),m_collisionObject(0) {} - Body(const btCollisionObject* colObj) : m_soft(0),m_collisionObject(colObj) + Body() : m_soft(0), m_rigid(0), m_collisionObject(0) {} + Body(Cluster* p) : m_soft(p), m_rigid(0), m_collisionObject(0) {} + Body(const btCollisionObject* colObj) : m_soft(0), m_collisionObject(colObj) { m_rigid = (btRigidBody*)btRigidBody::upcast(m_collisionObject); } - void activate() const + void activate() const { - if(m_rigid) + if (m_rigid) m_rigid->activate(); if (m_collisionObject) m_collisionObject->activate(); - } - const btMatrix3x3& invWorldInertia() const + const btMatrix3x3& invWorldInertia() const { - static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0); - if(m_rigid) return(m_rigid->getInvInertiaTensorWorld()); - if(m_soft) return(m_soft->m_invwi); - return(iwi); + static const btMatrix3x3 iwi(0, 0, 0, 0, 0, 0, 0, 0, 0); + if (m_rigid) return (m_rigid->getInvInertiaTensorWorld()); + if (m_soft) return (m_soft->m_invwi); + return (iwi); } - btScalar invMass() const + btScalar invMass() const { - if(m_rigid) return(m_rigid->getInvMass()); - if(m_soft) return(m_soft->m_imass); - return(0); + if (m_rigid) return (m_rigid->getInvMass()); + if (m_soft) return (m_soft->m_imass); + return (0); } - const btTransform& xform() const + const btTransform& xform() const { - static const btTransform identity=btTransform::getIdentity(); - if(m_collisionObject) return(m_collisionObject->getWorldTransform()); - if(m_soft) return(m_soft->m_framexform); - return(identity); + static const btTransform identity = btTransform::getIdentity(); + if (m_collisionObject) return (m_collisionObject->getWorldTransform()); + if (m_soft) return (m_soft->m_framexform); + return (identity); } - btVector3 linearVelocity() const + btVector3 linearVelocity() const { - if(m_rigid) return(m_rigid->getLinearVelocity()); - if(m_soft) return(m_soft->m_lv); - return(btVector3(0,0,0)); + if (m_rigid) return (m_rigid->getLinearVelocity()); + if (m_soft) return (m_soft->m_lv); + return (btVector3(0, 0, 0)); } - btVector3 angularVelocity(const btVector3& rpos) const - { - if(m_rigid) return(btCross(m_rigid->getAngularVelocity(),rpos)); - if(m_soft) return(btCross(m_soft->m_av,rpos)); - return(btVector3(0,0,0)); - } - btVector3 angularVelocity() const - { - if(m_rigid) return(m_rigid->getAngularVelocity()); - if(m_soft) return(m_soft->m_av); - return(btVector3(0,0,0)); - } - btVector3 velocity(const btVector3& rpos) const + btVector3 angularVelocity(const btVector3& rpos) const { - return(linearVelocity()+angularVelocity(rpos)); + if (m_rigid) return (btCross(m_rigid->getAngularVelocity(), rpos)); + if (m_soft) return (btCross(m_soft->m_av, rpos)); + return (btVector3(0, 0, 0)); } - void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const + btVector3 angularVelocity() const { - if(m_rigid) m_rigid->applyImpulse(impulse,rpos); - if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse); + if (m_rigid) return (m_rigid->getAngularVelocity()); + if (m_soft) return (m_soft->m_av); + return (btVector3(0, 0, 0)); } - void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const + btVector3 velocity(const btVector3& rpos) const { - if(m_rigid) m_rigid->applyImpulse(impulse,rpos); - if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse); - } - void applyImpulse(const Impulse& impulse,const btVector3& rpos) const + return (linearVelocity() + angularVelocity(rpos)); + } + void applyVImpulse(const btVector3& impulse, const btVector3& rpos) const { - if(impulse.m_asVelocity) + if (m_rigid) m_rigid->applyImpulse(impulse, rpos); + if (m_soft) btSoftBody::clusterVImpulse(m_soft, rpos, impulse); + } + void applyDImpulse(const btVector3& impulse, const btVector3& rpos) const + { + if (m_rigid) m_rigid->applyImpulse(impulse, rpos); + if (m_soft) btSoftBody::clusterDImpulse(m_soft, rpos, impulse); + } + void applyImpulse(const Impulse& impulse, const btVector3& rpos) const + { + if (impulse.m_asVelocity) { -// printf("impulse.m_velocity = %f,%f,%f\n",impulse.m_velocity.getX(),impulse.m_velocity.getY(),impulse.m_velocity.getZ()); - applyVImpulse(impulse.m_velocity,rpos); + // printf("impulse.m_velocity = %f,%f,%f\n",impulse.m_velocity.getX(),impulse.m_velocity.getY(),impulse.m_velocity.getZ()); + applyVImpulse(impulse.m_velocity, rpos); } - if(impulse.m_asDrift) + if (impulse.m_asDrift) { -// printf("impulse.m_drift = %f,%f,%f\n",impulse.m_drift.getX(),impulse.m_drift.getY(),impulse.m_drift.getZ()); - applyDImpulse(impulse.m_drift,rpos); + // printf("impulse.m_drift = %f,%f,%f\n",impulse.m_drift.getX(),impulse.m_drift.getY(),impulse.m_drift.getZ()); + applyDImpulse(impulse.m_drift, rpos); } } - void applyVAImpulse(const btVector3& impulse) const + void applyVAImpulse(const btVector3& impulse) const { - if(m_rigid) m_rigid->applyTorqueImpulse(impulse); - if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); + if (m_rigid) m_rigid->applyTorqueImpulse(impulse); + if (m_soft) btSoftBody::clusterVAImpulse(m_soft, impulse); } - void applyDAImpulse(const btVector3& impulse) const + void applyDAImpulse(const btVector3& impulse) const { - if(m_rigid) m_rigid->applyTorqueImpulse(impulse); - if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); + if (m_rigid) m_rigid->applyTorqueImpulse(impulse); + if (m_soft) btSoftBody::clusterDAImpulse(m_soft, impulse); } - void applyAImpulse(const Impulse& impulse) const + void applyAImpulse(const Impulse& impulse) const { - if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); - if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); + if (impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); + if (impulse.m_asDrift) applyDAImpulse(impulse.m_drift); } - void applyDCImpulse(const btVector3& impulse) const + void applyDCImpulse(const btVector3& impulse) const { - if(m_rigid) m_rigid->applyCentralImpulse(impulse); - if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); + if (m_rigid) m_rigid->applyCentralImpulse(impulse); + if (m_soft) btSoftBody::clusterDCImpulse(m_soft, impulse); } }; - /* Joint */ - struct Joint + /* Joint */ + struct Joint { - struct eType { enum _ { - Linear=0, - Angular, - Contact - };}; + struct eType + { + enum _ + { + Linear = 0, + Angular, + Contact + }; + }; struct Specs { - Specs() : erp(1),cfm(1),split(1) {} - btScalar erp; - btScalar cfm; - btScalar split; + Specs() : erp(1), cfm(1), split(1) {} + btScalar erp; + btScalar cfm; + btScalar split; }; - Body m_bodies[2]; - btVector3 m_refs[2]; - btScalar m_cfm; - btScalar m_erp; - btScalar m_split; - btVector3 m_drift; - btVector3 m_sdrift; - btMatrix3x3 m_massmatrix; - bool m_delete; - virtual ~Joint() {} + Body m_bodies[2]; + btVector3 m_refs[2]; + btScalar m_cfm; + btScalar m_erp; + btScalar m_split; + btVector3 m_drift; + btVector3 m_sdrift; + btMatrix3x3 m_massmatrix; + bool m_delete; + virtual ~Joint() {} Joint() : m_delete(false) {} - virtual void Prepare(btScalar dt,int iterations); - virtual void Solve(btScalar dt,btScalar sor)=0; - virtual void Terminate(btScalar dt)=0; - virtual eType::_ Type() const=0; + virtual void Prepare(btScalar dt, int iterations); + virtual void Solve(btScalar dt, btScalar sor) = 0; + virtual void Terminate(btScalar dt) = 0; + virtual eType::_ Type() const = 0; }; - /* LJoint */ - struct LJoint : Joint + /* LJoint */ + struct LJoint : Joint { struct Specs : Joint::Specs { - btVector3 position; - }; - btVector3 m_rpos[2]; - void Prepare(btScalar dt,int iterations); - void Solve(btScalar dt,btScalar sor); - void Terminate(btScalar dt); - eType::_ Type() const { return(eType::Linear); } + btVector3 position; + }; + btVector3 m_rpos[2]; + void Prepare(btScalar dt, int iterations); + void Solve(btScalar dt, btScalar sor); + void Terminate(btScalar dt); + eType::_ Type() const { return (eType::Linear); } }; - /* AJoint */ - struct AJoint : Joint + /* AJoint */ + struct AJoint : Joint { struct IControl { virtual ~IControl() {} - virtual void Prepare(AJoint*) {} - virtual btScalar Speed(AJoint*,btScalar current) { return(current); } - static IControl* Default() { static IControl def;return(&def); } + virtual void Prepare(AJoint*) {} + virtual btScalar Speed(AJoint*, btScalar current) { return (current); } + static IControl* Default() + { + static IControl def; + return (&def); + } }; struct Specs : Joint::Specs { Specs() : icontrol(IControl::Default()) {} - btVector3 axis; - IControl* icontrol; - }; - btVector3 m_axis[2]; - IControl* m_icontrol; - void Prepare(btScalar dt,int iterations); - void Solve(btScalar dt,btScalar sor); - void Terminate(btScalar dt); - eType::_ Type() const { return(eType::Angular); } + btVector3 axis; + IControl* icontrol; + }; + btVector3 m_axis[2]; + IControl* m_icontrol; + void Prepare(btScalar dt, int iterations); + void Solve(btScalar dt, btScalar sor); + void Terminate(btScalar dt); + eType::_ Type() const { return (eType::Angular); } }; - /* CJoint */ - struct CJoint : Joint - { - int m_life; - int m_maxlife; - btVector3 m_rpos[2]; - btVector3 m_normal; - btScalar m_friction; - void Prepare(btScalar dt,int iterations); - void Solve(btScalar dt,btScalar sor); - void Terminate(btScalar dt); - eType::_ Type() const { return(eType::Contact); } - }; - /* Config */ - struct Config + /* CJoint */ + struct CJoint : Joint { - eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) - btScalar kVCF; // Velocities correction factor (Baumgarte) - btScalar kDP; // Damping coefficient [0,1] - btScalar kDG; // Drag coefficient [0,+inf] - btScalar kLF; // Lift coefficient [0,+inf] - btScalar kPR; // Pressure coefficient [-inf,+inf] - btScalar kVC; // Volume conversation coefficient [0,+inf] - btScalar kDF; // Dynamic friction coefficient [0,1] - btScalar kMT; // Pose matching coefficient [0,1] - btScalar kCHR; // Rigid contacts hardness [0,1] - btScalar kKHR; // Kinetic contacts hardness [0,1] - btScalar kSHR; // Soft contacts hardness [0,1] - btScalar kAHR; // Anchors hardness [0,1] - btScalar kSRHR_CL; // Soft vs rigid hardness [0,1] (cluster only) - btScalar kSKHR_CL; // Soft vs kinetic hardness [0,1] (cluster only) - btScalar kSSHR_CL; // Soft vs soft hardness [0,1] (cluster only) - btScalar kSR_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) - btScalar kSK_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) - btScalar kSS_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) - btScalar maxvolume; // Maximum volume ratio for pose - btScalar timescale; // Time scale - int viterations; // Velocities solver iterations - int piterations; // Positions solver iterations - int diterations; // Drift solver iterations - int citerations; // Cluster solver iterations - int collisions; // Collisions flags - tVSolverArray m_vsequence; // Velocity solvers sequence - tPSolverArray m_psequence; // Position solvers sequence - tPSolverArray m_dsequence; // Drift solvers sequence + int m_life; + int m_maxlife; + btVector3 m_rpos[2]; + btVector3 m_normal; + btScalar m_friction; + void Prepare(btScalar dt, int iterations); + void Solve(btScalar dt, btScalar sor); + void Terminate(btScalar dt); + eType::_ Type() const { return (eType::Contact); } }; - /* SolverState */ - struct SolverState + /* Config */ + struct Config { - btScalar sdt; // dt*timescale - btScalar isdt; // 1/sdt - btScalar velmrg; // velocity margin - btScalar radmrg; // radial margin - btScalar updmrg; // Update margin - }; + eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) + btScalar kVCF; // Velocities correction factor (Baumgarte) + btScalar kDP; // Damping coefficient [0,1] + btScalar kDG; // Drag coefficient [0,+inf] + btScalar kLF; // Lift coefficient [0,+inf] + btScalar kPR; // Pressure coefficient [-inf,+inf] + btScalar kVC; // Volume conversation coefficient [0,+inf] + btScalar kDF; // Dynamic friction coefficient [0,1] + btScalar kMT; // Pose matching coefficient [0,1] + btScalar kCHR; // Rigid contacts hardness [0,1] + btScalar kKHR; // Kinetic contacts hardness [0,1] + btScalar kSHR; // Soft contacts hardness [0,1] + btScalar kAHR; // Anchors hardness [0,1] + btScalar kSRHR_CL; // Soft vs rigid hardness [0,1] (cluster only) + btScalar kSKHR_CL; // Soft vs kinetic hardness [0,1] (cluster only) + btScalar kSSHR_CL; // Soft vs soft hardness [0,1] (cluster only) + btScalar kSR_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) + btScalar kSK_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) + btScalar kSS_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) + btScalar maxvolume; // Maximum volume ratio for pose + btScalar timescale; // Time scale + int viterations; // Velocities solver iterations + int piterations; // Positions solver iterations + int diterations; // Drift solver iterations + int citerations; // Cluster solver iterations + int collisions; // Collisions flags + tVSolverArray m_vsequence; // Velocity solvers sequence + tPSolverArray m_psequence; // Position solvers sequence + tPSolverArray m_dsequence; // Drift solvers sequence + }; + /* SolverState */ + struct SolverState + { + btScalar sdt; // dt*timescale + btScalar isdt; // 1/sdt + btScalar velmrg; // velocity margin + btScalar radmrg; // radial margin + btScalar updmrg; // Update margin + }; /// RayFromToCaster takes a ray from, ray to (instead of direction!) - struct RayFromToCaster : btDbvt::ICollide + struct RayFromToCaster : btDbvt::ICollide { - btVector3 m_rayFrom; - btVector3 m_rayTo; - btVector3 m_rayNormalizedDirection; - btScalar m_mint; - Face* m_face; - int m_tests; - RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); - void Process(const btDbvtNode* leaf); + btVector3 m_rayFrom; + btVector3 m_rayTo; + btVector3 m_rayNormalizedDirection; + btScalar m_mint; + Face* m_face; + int m_tests; + RayFromToCaster(const btVector3& rayFrom, const btVector3& rayTo, btScalar mxt); + void Process(const btDbvtNode* leaf); - static /*inline*/ btScalar rayFromToTriangle(const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayNormalizedDirection, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt=SIMD_INFINITY); + static /*inline*/ btScalar rayFromToTriangle(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt = SIMD_INFINITY); }; // // Typedefs // - typedef void (*psolver_t)(btSoftBody*,btScalar,btScalar); - typedef void (*vsolver_t)(btSoftBody*,btScalar); - typedef btAlignedObjectArray tClusterArray; - typedef btAlignedObjectArray tNoteArray; - typedef btAlignedObjectArray tNodeArray; - typedef btAlignedObjectArray tLeafArray; - typedef btAlignedObjectArray tLinkArray; - typedef btAlignedObjectArray tFaceArray; - typedef btAlignedObjectArray tTetraArray; - typedef btAlignedObjectArray tAnchorArray; - typedef btAlignedObjectArray tRContactArray; - typedef btAlignedObjectArray tSContactArray; - typedef btAlignedObjectArray tMaterialArray; - typedef btAlignedObjectArray tJointArray; - typedef btAlignedObjectArray tSoftBodyArray; + typedef void (*psolver_t)(btSoftBody*, btScalar, btScalar); + typedef void (*vsolver_t)(btSoftBody*, btScalar); + typedef btAlignedObjectArray tClusterArray; + typedef btAlignedObjectArray tNoteArray; + typedef btAlignedObjectArray tNodeArray; + typedef btAlignedObjectArray tLeafArray; + typedef btAlignedObjectArray tLinkArray; + typedef btAlignedObjectArray tFaceArray; + typedef btAlignedObjectArray tTetraArray; + typedef btAlignedObjectArray tAnchorArray; + typedef btAlignedObjectArray tRContactArray; + typedef btAlignedObjectArray tSContactArray; + typedef btAlignedObjectArray tMaterialArray; + typedef btAlignedObjectArray tJointArray; + typedef btAlignedObjectArray tSoftBodyArray; // // Fields // - Config m_cfg; // Configuration - SolverState m_sst; // Solver state - Pose m_pose; // Pose - void* m_tag; // User data - btSoftBodyWorldInfo* m_worldInfo; // World info - tNoteArray m_notes; // Notes - tNodeArray m_nodes; // Nodes - tLinkArray m_links; // Links - tFaceArray m_faces; // Faces - tTetraArray m_tetras; // Tetras - tAnchorArray m_anchors; // Anchors - tRContactArray m_rcontacts; // Rigid contacts - tSContactArray m_scontacts; // Soft contacts - tJointArray m_joints; // Joints - tMaterialArray m_materials; // Materials - btScalar m_timeacc; // Time accumulator - btVector3 m_bounds[2]; // Spatial bounds - bool m_bUpdateRtCst; // Update runtime constants - btDbvt m_ndbvt; // Nodes tree - btDbvt m_fdbvt; // Faces tree - btDbvt m_cdbvt; // Clusters tree - tClusterArray m_clusters; // Clusters + Config m_cfg; // Configuration + SolverState m_sst; // Solver state + Pose m_pose; // Pose + void* m_tag; // User data + btSoftBodyWorldInfo* m_worldInfo; // World info + tNoteArray m_notes; // Notes + tNodeArray m_nodes; // Nodes + tLinkArray m_links; // Links + tFaceArray m_faces; // Faces + tTetraArray m_tetras; // Tetras + tAnchorArray m_anchors; // Anchors + tRContactArray m_rcontacts; // Rigid contacts + tSContactArray m_scontacts; // Soft contacts + tJointArray m_joints; // Joints + tMaterialArray m_materials; // Materials + btScalar m_timeacc; // Time accumulator + btVector3 m_bounds[2]; // Spatial bounds + bool m_bUpdateRtCst; // Update runtime constants + btDbvt m_ndbvt; // Nodes tree + btDbvt m_fdbvt; // Faces tree + btDbvt m_cdbvt; // Clusters tree + tClusterArray m_clusters; // Clusters - btAlignedObjectArraym_clusterConnectivity;//cluster connectivity, for self-collision + btAlignedObjectArray m_clusterConnectivity; //cluster connectivity, for self-collision - btTransform m_initialWorldTransform; + btTransform m_initialWorldTransform; + + btVector3 m_windVelocity; + + btScalar m_restLengthScale; - btVector3 m_windVelocity; - - btScalar m_restLengthScale; - // // Api // - /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m); + /* ctor */ + btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m); - /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo); + /* ctor */ + btSoftBody(btSoftBodyWorldInfo* worldInfo); - void initDefaults(); + void initDefaults(); - /* dtor */ + /* dtor */ virtual ~btSoftBody(); - /* Check for existing link */ + /* Check for existing link */ - btAlignedObjectArray m_userIndexMapping; + btAlignedObjectArray m_userIndexMapping; - btSoftBodyWorldInfo* getWorldInfo() + btSoftBodyWorldInfo* getWorldInfo() { return m_worldInfo; } ///@todo: avoid internal softbody shape hack and move collision code to collision library - virtual void setCollisionShape(btCollisionShape* collisionShape) + virtual void setCollisionShape(btCollisionShape* collisionShape) { - } - bool checkLink( int node0, - int node1) const; - bool checkLink( const Node* node0, - const Node* node1) const; - /* Check for existring face */ - bool checkFace( int node0, - int node1, - int node2) const; - /* Append material */ - Material* appendMaterial(); - /* Append note */ - void appendNote( const char* text, - const btVector3& o, - const btVector4& c=btVector4(1,0,0,0), - Node* n0=0, - Node* n1=0, - Node* n2=0, - Node* n3=0); - void appendNote( const char* text, - const btVector3& o, - Node* feature); - void appendNote( const char* text, - const btVector3& o, - Link* feature); - void appendNote( const char* text, - const btVector3& o, - Face* feature); - /* Append node */ - void appendNode( const btVector3& x,btScalar m); - /* Append link */ - void appendLink(int model=-1,Material* mat=0); - void appendLink( int node0, - int node1, - Material* mat=0, - bool bcheckexist=false); - void appendLink( Node* node0, - Node* node1, - Material* mat=0, - bool bcheckexist=false); - /* Append face */ - void appendFace(int model=-1,Material* mat=0); - void appendFace( int node0, - int node1, - int node2, - Material* mat=0); - void appendTetra(int model,Material* mat); + bool checkLink(int node0, + int node1) const; + bool checkLink(const Node* node0, + const Node* node1) const; + /* Check for existring face */ + bool checkFace(int node0, + int node1, + int node2) const; + /* Append material */ + Material* appendMaterial(); + /* Append note */ + void appendNote(const char* text, + const btVector3& o, + const btVector4& c = btVector4(1, 0, 0, 0), + Node* n0 = 0, + Node* n1 = 0, + Node* n2 = 0, + Node* n3 = 0); + void appendNote(const char* text, + const btVector3& o, + Node* feature); + void appendNote(const char* text, + const btVector3& o, + Link* feature); + void appendNote(const char* text, + const btVector3& o, + Face* feature); + /* Append node */ + void appendNode(const btVector3& x, btScalar m); + /* Append link */ + void appendLink(int model = -1, Material* mat = 0); + void appendLink(int node0, + int node1, + Material* mat = 0, + bool bcheckexist = false); + void appendLink(Node* node0, + Node* node1, + Material* mat = 0, + bool bcheckexist = false); + /* Append face */ + void appendFace(int model = -1, Material* mat = 0); + void appendFace(int node0, + int node1, + int node2, + Material* mat = 0); + void appendTetra(int model, Material* mat); // - void appendTetra(int node0, - int node1, - int node2, - int node3, - Material* mat=0); + void appendTetra(int node0, + int node1, + int node2, + int node3, + Material* mat = 0); - - /* Append anchor */ - void appendAnchor( int node, - btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false,btScalar influence = 1); - void appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies=false,btScalar influence = 1); - /* Append linear joint */ - void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); - void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); - void appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body); - /* Append linear joint */ - void appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1); - void appendAngularJoint(const AJoint::Specs& specs,Body body=Body()); - void appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body); - /* Add force (or gravity) to the entire body */ - void addForce( const btVector3& force); - /* Add force (or gravity) to a node of the body */ - void addForce( const btVector3& force, - int node); + /* Append anchor */ + void appendAnchor(int node, + btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); + void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); + /* Append linear joint */ + void appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1); + void appendLinearJoint(const LJoint::Specs& specs, Body body = Body()); + void appendLinearJoint(const LJoint::Specs& specs, btSoftBody* body); + /* Append linear joint */ + void appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1); + void appendAngularJoint(const AJoint::Specs& specs, Body body = Body()); + void appendAngularJoint(const AJoint::Specs& specs, btSoftBody* body); + /* Add force (or gravity) to the entire body */ + void addForce(const btVector3& force); + /* Add force (or gravity) to a node of the body */ + void addForce(const btVector3& force, + int node); /* Add aero force to a node of the body */ - void addAeroForceToNode(const btVector3& windVelocity,int nodeIndex); + void addAeroForceToNode(const btVector3& windVelocity, int nodeIndex); /* Add aero force to a face of the body */ - void addAeroForceToFace(const btVector3& windVelocity,int faceIndex); + void addAeroForceToFace(const btVector3& windVelocity, int faceIndex); - /* Add velocity to the entire body */ - void addVelocity( const btVector3& velocity); + /* Add velocity to the entire body */ + void addVelocity(const btVector3& velocity); - /* Set velocity for the entire body */ - void setVelocity( const btVector3& velocity); + /* Set velocity for the entire body */ + void setVelocity(const btVector3& velocity); - /* Add velocity to a node of the body */ - void addVelocity( const btVector3& velocity, - int node); - /* Set mass */ - void setMass( int node, - btScalar mass); - /* Get mass */ - btScalar getMass( int node) const; - /* Get total mass */ - btScalar getTotalMass() const; - /* Set total mass (weighted by previous masses) */ - void setTotalMass( btScalar mass, - bool fromfaces=false); - /* Set total density */ - void setTotalDensity(btScalar density); + /* Add velocity to a node of the body */ + void addVelocity(const btVector3& velocity, + int node); + /* Set mass */ + void setMass(int node, + btScalar mass); + /* Get mass */ + btScalar getMass(int node) const; + /* Get total mass */ + btScalar getTotalMass() const; + /* Set total mass (weighted by previous masses) */ + void setTotalMass(btScalar mass, + bool fromfaces = false); + /* Set total density */ + void setTotalDensity(btScalar density); /* Set volume mass (using tetrahedrons) */ - void setVolumeMass( btScalar mass); + void setVolumeMass(btScalar mass); /* Set volume density (using tetrahedrons) */ - void setVolumeDensity( btScalar density); - /* Transform */ - void transform( const btTransform& trs); - /* Translate */ - void translate( const btVector3& trs); - /* Rotate */ - void rotate( const btQuaternion& rot); - /* Scale */ - void scale( const btVector3& scl); + void setVolumeDensity(btScalar density); + /* Transform */ + void transform(const btTransform& trs); + /* Translate */ + void translate(const btVector3& trs); + /* Rotate */ + void rotate(const btQuaternion& rot); + /* Scale */ + void scale(const btVector3& scl); /* Get link resting lengths scale */ - btScalar getRestLengthScale(); + btScalar getRestLengthScale(); /* Scale resting length of all springs */ - void setRestLengthScale(btScalar restLength); - /* Set current state as pose */ - void setPose( bool bvolume, - bool bframe); - /* Set current link lengths as resting lengths */ - void resetLinkRestLengths(); - /* Return the volume */ - btScalar getVolume() const; - /* Cluster count */ - int clusterCount() const; - /* Cluster center of mass */ - static btVector3 clusterCom(const Cluster* cluster); - btVector3 clusterCom(int cluster) const; - /* Cluster velocity at rpos */ - static btVector3 clusterVelocity(const Cluster* cluster,const btVector3& rpos); - /* Cluster impulse */ - static void clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse); - static void clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse); - static void clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse); - static void clusterVAImpulse(Cluster* cluster,const btVector3& impulse); - static void clusterDAImpulse(Cluster* cluster,const btVector3& impulse); - static void clusterAImpulse(Cluster* cluster,const Impulse& impulse); - static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse); - /* Generate bending constraints based on distance in the adjency graph */ - int generateBendingConstraints( int distance, - Material* mat=0); - /* Randomize constraints to reduce solver bias */ - void randomizeConstraints(); - /* Release clusters */ - void releaseCluster(int index); - void releaseClusters(); - /* Generate clusters (K-mean) */ + void setRestLengthScale(btScalar restLength); + /* Set current state as pose */ + void setPose(bool bvolume, + bool bframe); + /* Set current link lengths as resting lengths */ + void resetLinkRestLengths(); + /* Return the volume */ + btScalar getVolume() const; + /* Cluster count */ + int clusterCount() const; + /* Cluster center of mass */ + static btVector3 clusterCom(const Cluster* cluster); + btVector3 clusterCom(int cluster) const; + /* Cluster velocity at rpos */ + static btVector3 clusterVelocity(const Cluster* cluster, const btVector3& rpos); + /* Cluster impulse */ + static void clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse); + static void clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse); + static void clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse); + static void clusterVAImpulse(Cluster* cluster, const btVector3& impulse); + static void clusterDAImpulse(Cluster* cluster, const btVector3& impulse); + static void clusterAImpulse(Cluster* cluster, const Impulse& impulse); + static void clusterDCImpulse(Cluster* cluster, const btVector3& impulse); + /* Generate bending constraints based on distance in the adjency graph */ + int generateBendingConstraints(int distance, + Material* mat = 0); + /* Randomize constraints to reduce solver bias */ + void randomizeConstraints(); + /* Release clusters */ + void releaseCluster(int index); + void releaseClusters(); + /* Generate clusters (K-mean) */ ///generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle ///otherwise an approximation will be used (better performance) - int generateClusters(int k,int maxiterations=8192); - /* Refine */ - void refine(ImplicitFn* ifn,btScalar accurary,bool cut); - /* CutLink */ - bool cutLink(int node0,int node1,btScalar position); - bool cutLink(const Node* node0,const Node* node1,btScalar position); + int generateClusters(int k, int maxiterations = 8192); + /* Refine */ + void refine(ImplicitFn* ifn, btScalar accurary, bool cut); + /* CutLink */ + bool cutLink(int node0, int node1, btScalar position); + bool cutLink(const Node* node0, const Node* node1, btScalar position); ///Ray casting using rayFrom and rayTo in worldspace, (not direction!) - bool rayTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results); - /* Solver presets */ - void setSolver(eSolverPresets::_ preset); - /* predictMotion */ - void predictMotion(btScalar dt); - /* solveConstraints */ - void solveConstraints(); - /* staticSolve */ - void staticSolve(int iterations); - /* solveCommonConstraints */ - static void solveCommonConstraints(btSoftBody** bodies,int count,int iterations); - /* solveClusters */ - static void solveClusters(const btAlignedObjectArray& bodies); - /* integrateMotion */ - void integrateMotion(); - /* defaultCollisionHandlers */ - void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); - void defaultCollisionHandler(btSoftBody* psb); - - + bool rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results); + /* Solver presets */ + void setSolver(eSolverPresets::_ preset); + /* predictMotion */ + void predictMotion(btScalar dt); + /* solveConstraints */ + void solveConstraints(); + /* staticSolve */ + void staticSolve(int iterations); + /* solveCommonConstraints */ + static void solveCommonConstraints(btSoftBody** bodies, int count, int iterations); + /* solveClusters */ + static void solveClusters(const btAlignedObjectArray& bodies); + /* integrateMotion */ + void integrateMotion(); + /* defaultCollisionHandlers */ + void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); + void defaultCollisionHandler(btSoftBody* psb); // // Functionality to deal with new accelerated solvers. @@ -895,8 +923,7 @@ public: /** * Set a wind velocity for interaction with the air. */ - void setWindVelocity( const btVector3 &velocity ); - + void setWindVelocity(const btVector3& velocity); /** * Return the wind velocity for interaction with the air. @@ -907,41 +934,40 @@ public: // Set the solver that handles this soft body // Should not be allowed to get out of sync with reality // Currently called internally on addition to the world - void setSoftBodySolver( btSoftBodySolver *softBodySolver ) + void setSoftBodySolver(btSoftBodySolver* softBodySolver) { m_softBodySolver = softBodySolver; } // // Return the solver that handles this soft body - // - btSoftBodySolver *getSoftBodySolver() + // + btSoftBodySolver* getSoftBodySolver() { return m_softBodySolver; } // // Return the solver that handles this soft body - // - btSoftBodySolver *getSoftBodySolver() const + // + btSoftBodySolver* getSoftBodySolver() const { return m_softBodySolver; } - // // Cast // - static const btSoftBody* upcast(const btCollisionObject* colObj) + static const btSoftBody* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()==CO_SOFT_BODY) + if (colObj->getInternalType() == CO_SOFT_BODY) return (const btSoftBody*)colObj; return 0; } - static btSoftBody* upcast(btCollisionObject* colObj) + static btSoftBody* upcast(btCollisionObject* colObj) { - if (colObj->getInternalType()==CO_SOFT_BODY) + if (colObj->getInternalType() == CO_SOFT_BODY) return (btSoftBody*)colObj; return 0; } @@ -950,7 +976,7 @@ public: // ::btCollisionObject // - virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const + virtual void getAabb(btVector3& aabbMin, btVector3& aabbMax) const { aabbMin = m_bounds[0]; aabbMax = m_bounds[1]; @@ -958,48 +984,42 @@ public: // // Private // - void pointersToIndices(); - void indicesToPointers(const int* map=0); + void pointersToIndices(); + void indicesToPointers(const int* map = 0); - int rayTest(const btVector3& rayFrom,const btVector3& rayTo, - btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; - void initializeFaceTree(); - btVector3 evaluateCom() const; - bool checkContact(const btCollisionObjectWrapper* colObjWrap,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; - void updateNormals(); - void updateBounds(); - void updatePose(); - void updateConstants(); - void updateLinkConstants(); - void updateArea(bool averageArea = true); - void initializeClusters(); - void updateClusters(); - void cleanupClusters(); - void prepareClusters(int iterations); - void solveClusters(btScalar sor); - void applyClusters(bool drift); - void dampClusters(); - void applyForces(); - static void PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti); - static void PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti); - static void PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti); - static void PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti); - static void VSolve_Links(btSoftBody* psb,btScalar kst); - static psolver_t getSolver(ePSolver::_ solver); - static vsolver_t getSolver(eVSolver::_ solver); + int rayTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const; + void initializeFaceTree(); + btVector3 evaluateCom() const; + bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; + void updateNormals(); + void updateBounds(); + void updatePose(); + void updateConstants(); + void updateLinkConstants(); + void updateArea(bool averageArea = true); + void initializeClusters(); + void updateClusters(); + void cleanupClusters(); + void prepareClusters(int iterations); + void solveClusters(btScalar sor); + void applyClusters(bool drift); + void dampClusters(); + void applyForces(); + static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); + static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); + static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti); + static void PSolve_Links(btSoftBody* psb, btScalar kst, btScalar ti); + static void VSolve_Links(btSoftBody* psb, btScalar kst); + static psolver_t getSolver(ePSolver::_ solver); + static vsolver_t getSolver(eVSolver::_ solver); - - virtual int calculateSerializeBufferSize() const; + 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; + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; //virtual void serializeSingleObject(class btSerializer* serializer) const; - - }; - - - -#endif //_BT_SOFT_BODY_H +#endif //_BT_SOFT_BODY_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index ab84bddf2..750718f57 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSoftBodyConcaveCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" @@ -27,34 +26,28 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexHullShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletSoftBody/btSoftBody.h" -#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable +#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06) //make this configurable -btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -: btCollisionAlgorithm(ci), -m_isSwapped(isSwapped), -m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped) +btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btCollisionAlgorithm(ci), + m_isSwapped(isSwapped), + m_btSoftBodyTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped) { } - - btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm() { } - - -btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped): -m_dispatcher(dispatcher), -m_dispatchInfoPtr(0) +btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher), + m_dispatchInfoPtr(0) { - m_softBody = (isSwapped? (btSoftBody*)body1Wrap->getCollisionObject():(btSoftBody*)body0Wrap->getCollisionObject()); - m_triBody = isSwapped? body0Wrap->getCollisionObject():body1Wrap->getCollisionObject(); + m_softBody = (isSwapped ? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject()); + m_triBody = isSwapped ? body0Wrap->getCollisionObject() : body1Wrap->getCollisionObject(); // // create the manifold from the dispatcher 'manifold pool' @@ -68,46 +61,42 @@ btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() { clearCache(); // m_dispatcher->releaseManifold( m_manifoldPtr ); - } - -void btSoftBodyTriangleCallback::clearCache() +void btSoftBodyTriangleCallback::clearCache() { - for (int i=0;im_childShape); - m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary? + m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape); //necessary? delete tmp->m_childShape; } m_shapeCache.clear(); } - -void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) +void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { //just for debugging purposes //printf("triangle %d",m_triangleCount++); - + btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; ///debug drawing of the overlapping triangles - if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe)) + if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)) { - btVector3 color(1,1,0); + btVector3 color(1, 1, 0); const btTransform& tr = m_triBody->getWorldTransform(); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color); } - btTriIndex triIndex(partId,triangleIndex,0); + btTriIndex triIndex(partId, triangleIndex, 0); btHashKey triKey(triIndex.getUid()); - btTriIndex* shapeIndex = m_shapeCache[triKey]; if (shapeIndex) { @@ -117,82 +106,73 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, //copy over user pointers to temporary shape tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer()); - btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); + 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); + btCollisionObjectWrapper triBody(0, tm, m_triBody, m_triBody->getWorldTransform(), partId, triangleIndex); 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); + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody, &triBody, 0, algoType); //m_manifoldPtr); - colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); + colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - + return; } - //aabb filter is already applied! + //aabb filter is already applied! //btCollisionObject* colObj = static_cast(m_convexProxy->m_clientObject); // if (m_softBody->getCollisionShape()->getShapeType()== { // btVector3 other; - btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); + btVector3 normal = (triangle[1] - triangle[0]).cross(triangle[2] - triangle[0]); normal.normalize(); - normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; + normal *= BT_SOFTBODY_TRIANGLE_EXTRUSION; // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; // other+=normal*22.f; - btVector3 pts[6] = {triangle[0]+normal, - triangle[1]+normal, - triangle[2]+normal, - triangle[0]-normal, - triangle[1]-normal, - triangle[2]-normal}; - - btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); + btVector3 pts[6] = {triangle[0] + normal, + triangle[1] + normal, + triangle[2] + normal, + triangle[0] - normal, + triangle[1] - normal, + triangle[2] - normal}; + btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(), 6); // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); - //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); + //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); // tm.setMargin(m_collisionMarginTriangle); //copy over user pointers to temporary shape tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer()); - - 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());//?? + 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());//?? 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); + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody, &triBody, 0, algoType); //m_manifoldPtr); - colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); + colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); triIndex.m_childShape = tm; - m_shapeCache.insert(triKey,triIndex); - + m_shapeCache.insert(triKey, triIndex); } - - - } - - -void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { m_dispatchInfoPtr = &dispatchInfo; - m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); + m_collisionMarginTriangle = collisionMarginTriangle + btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); m_resultOut = resultOut; - - btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax; - m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax); - btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5); - btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5); + btVector3 aabbWorldSpaceMin, aabbWorldSpaceMax; + m_softBody->getAabb(aabbWorldSpaceMin, aabbWorldSpaceMax); + btVector3 halfExtents = (aabbWorldSpaceMax - aabbWorldSpaceMin) * btScalar(0.5); + btVector3 softBodyCenter = (aabbWorldSpaceMax + aabbWorldSpaceMin) * btScalar(0.5); btTransform softTransform; softTransform.setIdentity(); @@ -200,56 +180,45 @@ void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMargin btTransform convexInTriangleSpace; convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform; - btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax); + btTransformAabb(halfExtents, m_collisionMarginTriangle, convexInTriangleSpace, m_aabbMin, m_aabbMax); } void btSoftBodyConcaveCollisionAlgorithm::clearCache() { m_btSoftBodyTriangleCallback.clearCache(); - } -void btSoftBodyConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSoftBodyConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - - //btCollisionObject* convexBody = m_isSwapped ? body1 : body0; const btCollisionObjectWrapper* triBody = m_isSwapped ? body0Wrap : body1Wrap; if (triBody->getCollisionShape()->isConcave()) { - - - const btCollisionObject* triOb = triBody->getCollisionObject(); - const btConcaveShape* concaveShape = static_cast( triOb->getCollisionShape()); + const btCollisionObject* triOb = triBody->getCollisionObject(); + const btConcaveShape* concaveShape = static_cast(triOb->getCollisionShape()); // if (convexBody->getCollisionShape()->isConvex()) { btScalar collisionMarginTriangle = concaveShape->getMargin(); // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); - m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,triBody,dispatchInfo,resultOut); + m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, triBody, dispatchInfo, resultOut); - - concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); + concaveShape->processAllTriangles(&m_btSoftBodyTriangleCallback, m_btSoftBodyTriangleCallback.getAabbMin(), m_btSoftBodyTriangleCallback.getAabbMax()); // resultOut->refreshContactPoints(); - } - } - } - -btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; btCollisionObject* convexbody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; - //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) //only perform CCD above a certain threshold, this prevents blocking on the long run @@ -268,25 +237,23 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - struct LocalTriangleSphereCastCallback : public btTriangleCallback + struct LocalTriangleSphereCastCallback : public btTriangleCallback { btTransform m_ccdSphereFromTrans; btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; + btTransform m_meshTransform; - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; + btScalar m_ccdSphereRadius; + btScalar m_hitFraction; - - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) - :m_ccdSphereFromTrans(from), - m_ccdSphereToTrans(to), - m_ccdSphereRadius(ccdSphereRadius), - m_hitFraction(hitFraction) - { + LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction) + : m_ccdSphereFromTrans(from), + m_ccdSphereToTrans(to), + m_ccdSphereRadius(ccdSphereRadius), + m_hitFraction(hitFraction) + { } - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { (void)partId; @@ -296,29 +263,23 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO ident.setIdentity(); btConvexCast::CastResult castResult; castResult.m_fraction = m_hitFraction; - btSphereShape pointShape(m_ccdSphereRadius); - btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); + btSphereShape pointShape(m_ccdSphereRadius); + btTriangleShape triShape(triangle[0], triangle[1], triangle[2]); + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); //local space? - if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, - ident,ident,castResult)) + if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans, + ident, ident, castResult)) { if (m_hitFraction > castResult.m_fraction) m_hitFraction = castResult.m_fraction; } - } - }; - - - - if (triBody->getCollisionShape()->isConcave()) { btVector3 rayAabbMin = convexFromLocal.getOrigin(); @@ -326,33 +287,30 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO btVector3 rayAabbMax = convexFromLocal.getOrigin(); rayAabbMax.setMax(convexToLocal.getOrigin()); btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0); + rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0); - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->getCcdSweptSphereRadius(),curHitFraction); + btScalar curHitFraction = btScalar(1.); //is this available? + LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal, + convexbody->getCcdSweptSphereRadius(), curHitFraction); raycastCallback.m_hitFraction = convexbody->getHitFraction(); btCollisionObject* concavebody = triBody; - btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); + btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape(); if (triangleMesh) { - triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); + triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax); } - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) { - convexbody->setHitFraction( raycastCallback.m_hitFraction); + convexbody->setHitFraction(raycastCallback.m_hitFraction); return raycastCallback.m_hitFraction; } } return btScalar(1.); - } diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h index 11c7b88f9..3adedbd80 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h @@ -29,63 +29,62 @@ class btCollisionShape; #include "LinearMath/btHashMap.h" -#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS +#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS struct btTriIndex { int m_PartIdTriangleIndex; - class btCollisionShape* m_childShape; + class btCollisionShape* m_childShape; - btTriIndex(int partId,int triangleIndex,btCollisionShape* shape) + btTriIndex(int partId, int triangleIndex, btCollisionShape* shape) { - m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; + m_PartIdTriangleIndex = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex; m_childShape = shape; } - int getTriangleIndex() const + int getTriangleIndex() const { // Get only the lower bits where the triangle index is stored unsigned int x = 0; - unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); - return (m_PartIdTriangleIndex&~(y)); + unsigned int y = (~(x & 0)) << (31 - MAX_NUM_PARTS_IN_BITS); + return (m_PartIdTriangleIndex & ~(y)); } - int getPartId() const + int getPartId() const { // Get only the highest bits where the part index is stored - return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); + return (m_PartIdTriangleIndex >> (31 - MAX_NUM_PARTS_IN_BITS)); } - int getUid() const + int getUid() const { return m_PartIdTriangleIndex; } }; - ///For each triangle in the concave mesh that overlaps with the AABB of a soft body (m_softBody), processTriangle is called. class btSoftBodyTriangleCallback : public btTriangleCallback { btSoftBody* m_softBody; const btCollisionObject* m_triBody; - btVector3 m_aabbMin; - btVector3 m_aabbMax ; + btVector3 m_aabbMin; + btVector3 m_aabbMax; btManifoldResult* m_resultOut; - btDispatcher* m_dispatcher; + btDispatcher* m_dispatcher; const btDispatcherInfo* m_dispatchInfoPtr; btScalar m_collisionMarginTriangle; - btHashMap,btTriIndex> m_shapeCache; + btHashMap, btTriIndex> m_shapeCache; public: - int m_triangleCount; + int m_triangleCount; // btPersistentManifold* m_manifoldPtr; - btSoftBodyTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + btSoftBodyTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); - void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triObjWrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btCollisionObjectWrapper* triObjWrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); virtual ~btSoftBodyTriangleCallback(); @@ -101,55 +100,48 @@ public: { return m_aabbMax; } - }; - - - /// btSoftBodyConcaveCollisionAlgorithm supports collision between soft body shapes and (concave) trianges meshes. -class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm +class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm { - - bool m_isSwapped; + bool m_isSwapped; btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback; public: - - btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + btSoftBodyConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btSoftBodyConcaveCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { //we don't add any manifolds } - void clearCache(); + void clearCache(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm)); - return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + return new (mem) btSoftBodyConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm)); - return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + return new (mem) btSoftBodyConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyData.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyData.h index 87d8841cf..cec6f401e 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyData.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyData.h @@ -19,199 +19,194 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletDynamics/Dynamics/btRigidBody.h" - -struct SoftBodyMaterialData +struct SoftBodyMaterialData { - float m_linearStiffness; - float m_angularStiffness; - float m_volumeStiffness; - int m_flags; + float m_linearStiffness; + float m_angularStiffness; + float m_volumeStiffness; + int m_flags; }; -struct SoftBodyNodeData +struct SoftBodyNodeData { - SoftBodyMaterialData *m_material; - btVector3FloatData m_position; - btVector3FloatData m_previousPosition; - btVector3FloatData m_velocity; - btVector3FloatData m_accumulatedForce; - btVector3FloatData m_normal; - float m_inverseMass; - float m_area; - int m_attach; - int m_pad; + SoftBodyMaterialData *m_material; + btVector3FloatData m_position; + btVector3FloatData m_previousPosition; + btVector3FloatData m_velocity; + btVector3FloatData m_accumulatedForce; + btVector3FloatData m_normal; + float m_inverseMass; + float m_area; + int m_attach; + int m_pad; }; -struct SoftBodyLinkData +struct SoftBodyLinkData { - SoftBodyMaterialData *m_material; - int m_nodeIndices[2]; // Node pointers - float m_restLength; // Rest length - int m_bbending; // Bending link + SoftBodyMaterialData *m_material; + int m_nodeIndices[2]; // Node pointers + float m_restLength; // Rest length + int m_bbending; // Bending link }; -struct SoftBodyFaceData +struct SoftBodyFaceData { - btVector3FloatData m_normal; // Normal - SoftBodyMaterialData *m_material; - int m_nodeIndices[3]; // Node pointers - float m_restArea; // Rest area -}; - -struct SoftBodyTetraData -{ - btVector3FloatData m_c0[4]; // gradients - SoftBodyMaterialData *m_material; - int m_nodeIndices[4]; // Node pointers - float m_restVolume; // Rest volume - float m_c1; // (4*kVST)/(im0+im1+im2+im3) - float m_c2; // m_c1/sum(|g0..3|^2) - int m_pad; + btVector3FloatData m_normal; // Normal + SoftBodyMaterialData *m_material; + int m_nodeIndices[3]; // Node pointers + float m_restArea; // Rest area }; -struct SoftRigidAnchorData +struct SoftBodyTetraData { - btMatrix3x3FloatData m_c0; // Impulse matrix - btVector3FloatData m_c1; // Relative anchor - btVector3FloatData m_localFrame; // Anchor position in body space - btRigidBodyData *m_rigidBody; - int m_nodeIndex; // Node pointer - float m_c2; // ima*dt + btVector3FloatData m_c0[4]; // gradients + SoftBodyMaterialData *m_material; + int m_nodeIndices[4]; // Node pointers + float m_restVolume; // Rest volume + float m_c1; // (4*kVST)/(im0+im1+im2+im3) + float m_c2; // m_c1/sum(|g0..3|^2) + int m_pad; }; - - -struct SoftBodyConfigData +struct SoftRigidAnchorData { - int m_aeroModel; // Aerodynamic model (default: V_Point) - float m_baumgarte; // Velocities correction factor (Baumgarte) - float m_damping; // Damping coefficient [0,1] - float m_drag; // Drag coefficient [0,+inf] - float m_lift; // Lift coefficient [0,+inf] - float m_pressure; // Pressure coefficient [-inf,+inf] - float m_volume; // Volume conversation coefficient [0,+inf] - float m_dynamicFriction; // Dynamic friction coefficient [0,1] - float m_poseMatch; // Pose matching coefficient [0,1] - float m_rigidContactHardness; // Rigid contacts hardness [0,1] - float m_kineticContactHardness; // Kinetic contacts hardness [0,1] - float m_softContactHardness; // Soft contacts hardness [0,1] - float m_anchorHardness; // Anchors hardness [0,1] - float m_softRigidClusterHardness; // Soft vs rigid hardness [0,1] (cluster only) - float m_softKineticClusterHardness; // Soft vs kinetic hardness [0,1] (cluster only) - float m_softSoftClusterHardness; // Soft vs soft hardness [0,1] (cluster only) - float m_softRigidClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) - float m_softKineticClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) - float m_softSoftClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) - float m_maxVolume; // Maximum volume ratio for pose - float m_timeScale; // Time scale - int m_velocityIterations; // Velocities solver iterations - int m_positionIterations; // Positions solver iterations - int m_driftIterations; // Drift solver iterations - int m_clusterIterations; // Cluster solver iterations - int m_collisionFlags; // Collisions flags + btMatrix3x3FloatData m_c0; // Impulse matrix + btVector3FloatData m_c1; // Relative anchor + btVector3FloatData m_localFrame; // Anchor position in body space + btRigidBodyData *m_rigidBody; + int m_nodeIndex; // Node pointer + float m_c2; // ima*dt }; -struct SoftBodyPoseData +struct SoftBodyConfigData { - btMatrix3x3FloatData m_rot; // Rotation - btMatrix3x3FloatData m_scale; // Scale - btMatrix3x3FloatData m_aqq; // Base scaling - btVector3FloatData m_com; // COM - - btVector3FloatData *m_positions; // Reference positions - float *m_weights; // Weights - int m_numPositions; - int m_numWeigts; - - int m_bvolume; // Is valid - int m_bframe; // Is frame - float m_restVolume; // Rest volume - int m_pad; + int m_aeroModel; // Aerodynamic model (default: V_Point) + float m_baumgarte; // Velocities correction factor (Baumgarte) + float m_damping; // Damping coefficient [0,1] + float m_drag; // Drag coefficient [0,+inf] + float m_lift; // Lift coefficient [0,+inf] + float m_pressure; // Pressure coefficient [-inf,+inf] + float m_volume; // Volume conversation coefficient [0,+inf] + float m_dynamicFriction; // Dynamic friction coefficient [0,1] + float m_poseMatch; // Pose matching coefficient [0,1] + float m_rigidContactHardness; // Rigid contacts hardness [0,1] + float m_kineticContactHardness; // Kinetic contacts hardness [0,1] + float m_softContactHardness; // Soft contacts hardness [0,1] + float m_anchorHardness; // Anchors hardness [0,1] + float m_softRigidClusterHardness; // Soft vs rigid hardness [0,1] (cluster only) + float m_softKineticClusterHardness; // Soft vs kinetic hardness [0,1] (cluster only) + float m_softSoftClusterHardness; // Soft vs soft hardness [0,1] (cluster only) + float m_softRigidClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) + float m_softKineticClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) + float m_softSoftClusterImpulseSplit; // Soft vs rigid impulse split [0,1] (cluster only) + float m_maxVolume; // Maximum volume ratio for pose + float m_timeScale; // Time scale + int m_velocityIterations; // Velocities solver iterations + int m_positionIterations; // Positions solver iterations + int m_driftIterations; // Drift solver iterations + int m_clusterIterations; // Cluster solver iterations + int m_collisionFlags; // Collisions flags }; -struct SoftBodyClusterData +struct SoftBodyPoseData { - btTransformFloatData m_framexform; - btMatrix3x3FloatData m_locii; - btMatrix3x3FloatData m_invwi; - btVector3FloatData m_com; - btVector3FloatData m_vimpulses[2]; - btVector3FloatData m_dimpulses[2]; - btVector3FloatData m_lv; - btVector3FloatData m_av; - - btVector3FloatData *m_framerefs; - int *m_nodeIndices; - float *m_masses; + btMatrix3x3FloatData m_rot; // Rotation + btMatrix3x3FloatData m_scale; // Scale + btMatrix3x3FloatData m_aqq; // Base scaling + btVector3FloatData m_com; // COM - int m_numFrameRefs; - int m_numNodes; - int m_numMasses; + btVector3FloatData *m_positions; // Reference positions + float *m_weights; // Weights + int m_numPositions; + int m_numWeigts; - 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; + int m_bvolume; // Is valid + int m_bframe; // Is frame + float m_restVolume; // Rest volume + int m_pad; }; - -enum btSoftJointBodyType +struct SoftBodyClusterData { - BT_JOINT_SOFT_BODY_CLUSTER=1, + btTransformFloatData m_framexform; + btMatrix3x3FloatData m_locii; + btMatrix3x3FloatData m_invwi; + btVector3FloatData m_com; + btVector3FloatData m_vimpulses[2]; + btVector3FloatData m_dimpulses[2]; + btVector3FloatData m_lv; + btVector3FloatData m_av; + + btVector3FloatData *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; +}; + +enum btSoftJointBodyType +{ + BT_JOINT_SOFT_BODY_CLUSTER = 1, BT_JOINT_RIGID_BODY, BT_JOINT_COLLISION_OBJECT }; -struct btSoftBodyJointData +struct btSoftBodyJointData { - void *m_bodyA; - void *m_bodyB; - btVector3FloatData m_refs[2]; - float m_cfm; - float m_erp; - float m_split; - int m_delete; - btVector3FloatData m_relPosition[2];//linear - int m_bodyAtype; - int m_bodyBtype; - int m_jointType; - int m_pad; + void *m_bodyA; + void *m_bodyB; + btVector3FloatData m_refs[2]; + float m_cfm; + float m_erp; + float m_split; + int m_delete; + btVector3FloatData m_relPosition[2]; //linear + int m_bodyAtype; + int m_bodyBtype; + int m_jointType; + int m_pad; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btSoftBodyFloatData +struct btSoftBodyFloatData { - btCollisionObjectFloatData m_collisionObjectData; + btCollisionObjectFloatData 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; - btSoftBodyJointData *m_joints; + SoftBodyPoseData *m_pose; + SoftBodyMaterialData **m_materials; + SoftBodyNodeData *m_nodes; + SoftBodyLinkData *m_links; + SoftBodyFaceData *m_faces; + SoftBodyTetraData *m_tetrahedra; + SoftRigidAnchorData *m_anchors; + SoftBodyClusterData *m_clusters; + btSoftBodyJointData *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; + 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 //BT_SOFTBODY_FLOAT_DATA - +#endif //BT_SOFTBODY_FLOAT_DATA diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp index 51fcd16da..d0a9921d8 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -21,106 +21,111 @@ subject to the following restrictions: #include "LinearMath/btConvexHull.h" #include "LinearMath/btConvexHullComputer.h" - // -static void drawVertex( btIDebugDraw* idraw, - const btVector3& x,btScalar s,const btVector3& c) +static void drawVertex(btIDebugDraw* idraw, + const btVector3& x, btScalar s, const btVector3& c) { - idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); - idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); - idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); + idraw->drawLine(x - btVector3(s, 0, 0), x + btVector3(s, 0, 0), c); + idraw->drawLine(x - btVector3(0, s, 0), x + btVector3(0, s, 0), c); + idraw->drawLine(x - btVector3(0, 0, s), x + btVector3(0, 0, s), c); } // -static void drawBox( btIDebugDraw* idraw, - const btVector3& mins, - const btVector3& maxs, - const btVector3& color) +static void drawBox(btIDebugDraw* idraw, + const btVector3& mins, + const btVector3& maxs, + const btVector3& color) { - const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), - btVector3(maxs.x(),mins.y(),mins.z()), - btVector3(maxs.x(),maxs.y(),mins.z()), - btVector3(mins.x(),maxs.y(),mins.z()), - btVector3(mins.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),maxs.y(),maxs.z()), - btVector3(mins.x(),maxs.y(),maxs.z())}; - idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); - idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); - idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); - idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); - idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); - idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); + const btVector3 c[] = {btVector3(mins.x(), mins.y(), mins.z()), + btVector3(maxs.x(), mins.y(), mins.z()), + btVector3(maxs.x(), maxs.y(), mins.z()), + btVector3(mins.x(), maxs.y(), mins.z()), + btVector3(mins.x(), mins.y(), maxs.z()), + btVector3(maxs.x(), mins.y(), maxs.z()), + btVector3(maxs.x(), maxs.y(), maxs.z()), + btVector3(mins.x(), maxs.y(), maxs.z())}; + idraw->drawLine(c[0], c[1], color); + idraw->drawLine(c[1], c[2], color); + idraw->drawLine(c[2], c[3], color); + idraw->drawLine(c[3], c[0], color); + idraw->drawLine(c[4], c[5], color); + idraw->drawLine(c[5], c[6], color); + idraw->drawLine(c[6], c[7], color); + idraw->drawLine(c[7], c[4], color); + idraw->drawLine(c[0], c[4], color); + idraw->drawLine(c[1], c[5], color); + idraw->drawLine(c[2], c[6], color); + idraw->drawLine(c[3], c[7], color); } // -static void drawTree( btIDebugDraw* idraw, - const btDbvtNode* node, - int depth, - const btVector3& ncolor, - const btVector3& lcolor, - int mindepth, - int maxdepth) +static void drawTree(btIDebugDraw* idraw, + const btDbvtNode* node, + int depth, + const btVector3& ncolor, + const btVector3& lcolor, + int mindepth, + int maxdepth) { - if(node) + if (node) { - if(node->isinternal()&&((depthisinternal() && ((depth < maxdepth) || (maxdepth < 0))) { - drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); - drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); + drawTree(idraw, node->childs[0], depth + 1, ncolor, lcolor, mindepth, maxdepth); + drawTree(idraw, node->childs[1], depth + 1, ncolor, lcolor, mindepth, maxdepth); } - if(depth>=mindepth) + if (depth >= mindepth) { - const btScalar scl=(btScalar)(node->isinternal()?1:1); - const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; - const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; - drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); + const btScalar scl = (btScalar)(node->isinternal() ? 1 : 1); + const btVector3 mi = node->volume.Center() - node->volume.Extents() * scl; + const btVector3 mx = node->volume.Center() + node->volume.Extents() * scl; + drawBox(idraw, mi, mx, node->isleaf() ? lcolor : ncolor); } } } // template -static inline T sum(const btAlignedObjectArray& items) +static inline T sum(const btAlignedObjectArray& items) { - T v; - if(items.size()) + T v; + if (items.size()) { - v=items[0]; - for(int i=1,ni=items.size();i -static inline void add(btAlignedObjectArray& items,const Q& value) +template +static inline void add(btAlignedObjectArray& items, const Q& value) { - for(int i=0,ni=items.size();i -static inline void mul(btAlignedObjectArray& items,const Q& value) +template +static inline void mul(btAlignedObjectArray& items, const Q& value) { - for(int i=0,ni=items.size();i -static inline T average(const btAlignedObjectArray& items) +static inline T average(const btAlignedObjectArray& items) { - const btScalar n=(btScalar)(items.size()>0?items.size():1); - return(sum(items)/n); + const btScalar n = (btScalar)(items.size() > 0 ? items.size() : 1); + return (sum(items) / n); } #if 0 @@ -158,86 +163,84 @@ static btVector3 stresscolor(btScalar stress) #endif // -void btSoftBodyHelpers::Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) +void btSoftBodyHelpers::Draw(btSoftBody* psb, + btIDebugDraw* idraw, + int drawflags) { - const btScalar scl=(btScalar)0.1; - const btScalar nscl=scl*5; - const btVector3 lcolor=btVector3(0,0,0); - const btVector3 ncolor=btVector3(1,1,1); - const btVector3 ccolor=btVector3(1,0,0); - int i,j,nj; + const btScalar scl = (btScalar)0.1; + const btScalar nscl = scl * 5; + const btVector3 lcolor = btVector3(0, 0, 0); + const btVector3 ncolor = btVector3(1, 1, 1); + const btVector3 ccolor = btVector3(1, 0, 0); + int i, j, nj; - /* Clusters */ - if(0!=(drawflags&fDrawFlags::Clusters)) + /* Clusters */ + if (0 != (drawflags & fDrawFlags::Clusters)) { srand(1806); - for(i=0;im_clusters.size();++i) + for (i = 0; i < psb->m_clusters.size(); ++i) { - if(psb->m_clusters[i]->m_collide) + if (psb->m_clusters[i]->m_collide) { - btVector3 color( rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX); - color=color.normalized()*0.75; - btAlignedObjectArray vertices; + btVector3 color(rand() / (btScalar)RAND_MAX, + rand() / (btScalar)RAND_MAX, + rand() / (btScalar)RAND_MAX); + color = color.normalized() * 0.75; + btAlignedObjectArray vertices; vertices.resize(psb->m_clusters[i]->m_nodes.size()); - for(j=0,nj=vertices.size();jm_clusters[i]->m_nodes[j]->m_x; + for (j = 0, nj = vertices.size(); j < nj; ++j) + { + vertices[j] = psb->m_clusters[i]->m_nodes[j]->m_x; } #define USE_NEW_CONVEX_HULL_COMPUTER #ifdef USE_NEW_CONVEX_HULL_COMPUTER - btConvexHullComputer computer; + btConvexHullComputer computer; int stride = sizeof(btVector3); int count = vertices.size(); - btScalar shrink=0.f; - btScalar shrinkClamp=0.f; - computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp); - for (int i=0;igetNextEdgeOfFace(); + const btConvexHullComputer::Edge* firstEdge = &computer.edges[face]; + const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace(); int v0 = firstEdge->getSourceVertex(); int v1 = firstEdge->getTargetVertex(); - while (edge!=firstEdge) + while (edge != firstEdge) { int v2 = edge->getTargetVertex(); - idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1); + idraw->drawTriangle(computer.vertices[v0], computer.vertices[v1], computer.vertices[v2], color, 1); edge = edge->getNextEdgeOfFace(); - v0=v1; - v1=v2; + v0 = v1; + v1 = v2; }; } #else - HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); - HullResult hres; - HullLibrary hlib; - hdsc.mMaxVertices=vertices.size(); - hlib.CreateConvexHull(hdsc,hres); - const btVector3 center=average(hres.m_OutputVertices); - add(hres.m_OutputVertices,-center); - mul(hres.m_OutputVertices,(btScalar)1); - add(hres.m_OutputVertices,center); - for(j=0;j<(int)hres.mNumFaces;++j) + HullDesc hdsc(QF_TRIANGLES, vertices.size(), &vertices[0]); + HullResult hres; + HullLibrary hlib; + hdsc.mMaxVertices = vertices.size(); + hlib.CreateConvexHull(hdsc, hres); + const btVector3 center = average(hres.m_OutputVertices); + add(hres.m_OutputVertices, -center); + mul(hres.m_OutputVertices, (btScalar)1); + add(hres.m_OutputVertices, center); + for (j = 0; j < (int)hres.mNumFaces; ++j) { - const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; + const int idx[] = {hres.m_Indices[j * 3 + 0], hres.m_Indices[j * 3 + 1], hres.m_Indices[j * 3 + 2]}; idraw->drawTriangle(hres.m_OutputVertices[idx[0]], - hres.m_OutputVertices[idx[1]], - hres.m_OutputVertices[idx[2]], - color,1); + hres.m_OutputVertices[idx[1]], + hres.m_OutputVertices[idx[2]], + color, 1); } hlib.ReleaseResult(hres); #endif - } - /* Velocities */ + /* Velocities */ #if 0 for(int j=0;jm_clusters[i].m_nodes.size();++j) { @@ -247,273 +250,269 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); } #endif - /* Frame */ - // btSoftBody::Cluster& c=*psb->m_clusters[i]; - // idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); - // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); - // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); + /* Frame */ + // btSoftBody::Cluster& c=*psb->m_clusters[i]; + // idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); + // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); + // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); } } else { - /* Nodes */ - if(0!=(drawflags&fDrawFlags::Nodes)) + /* Nodes */ + if (0 != (drawflags & fDrawFlags::Nodes)) { - for(i=0;im_nodes.size();++i) + for (i = 0; i < psb->m_nodes.size(); ++i) { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0)); - idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0)); - idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1)); + const btSoftBody::Node& n = psb->m_nodes[i]; + if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue; + idraw->drawLine(n.m_x - btVector3(scl, 0, 0), n.m_x + btVector3(scl, 0, 0), btVector3(1, 0, 0)); + idraw->drawLine(n.m_x - btVector3(0, scl, 0), n.m_x + btVector3(0, scl, 0), btVector3(0, 1, 0)); + idraw->drawLine(n.m_x - btVector3(0, 0, scl), n.m_x + btVector3(0, 0, scl), btVector3(0, 0, 1)); } } - /* Links */ - if(0!=(drawflags&fDrawFlags::Links)) + /* Links */ + if (0 != (drawflags & fDrawFlags::Links)) { - for(i=0;im_links.size();++i) + for (i = 0; i < psb->m_links.size(); ++i) { - const btSoftBody::Link& l=psb->m_links[i]; - if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor); + const btSoftBody::Link& l = psb->m_links[i]; + if (0 == (l.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue; + idraw->drawLine(l.m_n[0]->m_x, l.m_n[1]->m_x, lcolor); } } - /* Normals */ - if(0!=(drawflags&fDrawFlags::Normals)) + /* Normals */ + if (0 != (drawflags & fDrawFlags::Normals)) { - for(i=0;im_nodes.size();++i) + for (i = 0; i < psb->m_nodes.size(); ++i) { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - const btVector3 d=n.m_n*nscl; - idraw->drawLine(n.m_x,n.m_x+d,ncolor); - idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5); + const btSoftBody::Node& n = psb->m_nodes[i]; + if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue; + const btVector3 d = n.m_n * nscl; + idraw->drawLine(n.m_x, n.m_x + d, ncolor); + idraw->drawLine(n.m_x, n.m_x - d, ncolor * 0.5); } } - /* Contacts */ - if(0!=(drawflags&fDrawFlags::Contacts)) + /* Contacts */ + if (0 != (drawflags & fDrawFlags::Contacts)) { - static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; - for(i=0;im_rcontacts.size();++i) - { - const btSoftBody::RContact& c=psb->m_rcontacts[i]; - const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* - (btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); - const btVector3 x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); - const btVector3 y=btCross(x,c.m_cti.m_normal).normalized(); - idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); - idraw->drawLine(o-y*nscl,o+y*nscl,ccolor); - idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0)); + static const btVector3 axis[] = {btVector3(1, 0, 0), + btVector3(0, 1, 0), + btVector3(0, 0, 1)}; + for (i = 0; i < psb->m_rcontacts.size(); ++i) + { + const btSoftBody::RContact& c = psb->m_rcontacts[i]; + const btVector3 o = c.m_node->m_x - c.m_cti.m_normal * + (btDot(c.m_node->m_x, c.m_cti.m_normal) + c.m_cti.m_offset); + const btVector3 x = btCross(c.m_cti.m_normal, axis[c.m_cti.m_normal.minAxis()]).normalized(); + const btVector3 y = btCross(x, c.m_cti.m_normal).normalized(); + idraw->drawLine(o - x * nscl, o + x * nscl, ccolor); + idraw->drawLine(o - y * nscl, o + y * nscl, ccolor); + idraw->drawLine(o, o + c.m_cti.m_normal * nscl * 3, btVector3(1, 1, 0)); } } - /* Faces */ - if(0!=(drawflags&fDrawFlags::Faces)) + /* Faces */ + if (0 != (drawflags & fDrawFlags::Faces)) + { + const btScalar scl = (btScalar)0.8; + const btScalar alp = (btScalar)1; + const btVector3 col(0, (btScalar)0.7, 0); + for (i = 0; i < psb->m_faces.size(); ++i) + { + const btSoftBody::Face& f = psb->m_faces[i]; + if (0 == (f.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue; + const btVector3 x[] = {f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x}; + const btVector3 c = (x[0] + x[1] + x[2]) / 3; + idraw->drawTriangle((x[0] - c) * scl + c, + (x[1] - c) * scl + c, + (x[2] - c) * scl + c, + col, alp); + } + } + /* Tetras */ + if (0 != (drawflags & fDrawFlags::Tetras)) + { + const btScalar scl = (btScalar)0.8; + const btScalar alp = (btScalar)1; + const btVector3 col((btScalar)0.3, (btScalar)0.3, (btScalar)0.7); + for (int i = 0; i < psb->m_tetras.size(); ++i) + { + const btSoftBody::Tetra& t = psb->m_tetras[i]; + if (0 == (t.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue; + const btVector3 x[] = {t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x}; + const btVector3 c = (x[0] + x[1] + x[2] + x[3]) / 4; + idraw->drawTriangle((x[0] - c) * scl + c, (x[1] - c) * scl + c, (x[2] - c) * scl + c, col, alp); + idraw->drawTriangle((x[0] - c) * scl + c, (x[1] - c) * scl + c, (x[3] - c) * scl + c, col, alp); + idraw->drawTriangle((x[1] - c) * scl + c, (x[2] - c) * scl + c, (x[3] - c) * scl + c, col, alp); + idraw->drawTriangle((x[2] - c) * scl + c, (x[0] - c) * scl + c, (x[3] - c) * scl + c, col, alp); + } + } + } + /* Anchors */ + if (0 != (drawflags & fDrawFlags::Anchors)) { - const btScalar scl=(btScalar)0.8; - const btScalar alp=(btScalar)1; - const btVector3 col(0,(btScalar)0.7,0); - for(i=0;im_faces.size();++i) + for (i = 0; i < psb->m_anchors.size(); ++i) { - const btSoftBody::Face& f=psb->m_faces[i]; - if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; - const btVector3 c=(x[0]+x[1]+x[2])/3; - idraw->drawTriangle((x[0]-c)*scl+c, - (x[1]-c)*scl+c, - (x[2]-c)*scl+c, - col,alp); - } - } - /* Tetras */ - if(0!=(drawflags&fDrawFlags::Tetras)) - { - const btScalar scl=(btScalar)0.8; - const btScalar alp=(btScalar)1; - const btVector3 col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7); - for(int i=0;im_tetras.size();++i) - { - const btSoftBody::Tetra& t=psb->m_tetras[i]; - if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - const btVector3 x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x}; - const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4; - idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp); - idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp); - idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp); - idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp); - } - } - } - /* Anchors */ - if(0!=(drawflags&fDrawFlags::Anchors)) - { - for(i=0;im_anchors.size();++i) - { - const btSoftBody::Anchor& a=psb->m_anchors[i]; - const btVector3 q=a.m_body->getWorldTransform()*a.m_local; - drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0)); - drawVertex(idraw,q,0.25,btVector3(0,1,0)); - idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); + const btSoftBody::Anchor& a = psb->m_anchors[i]; + const btVector3 q = a.m_body->getWorldTransform() * a.m_local; + drawVertex(idraw, a.m_node->m_x, 0.25, btVector3(1, 0, 0)); + drawVertex(idraw, q, 0.25, btVector3(0, 1, 0)); + idraw->drawLine(a.m_node->m_x, q, btVector3(1, 1, 1)); } - for(i=0;im_nodes.size();++i) + for (i = 0; i < psb->m_nodes.size(); ++i) { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - if(n.m_im<=0) + const btSoftBody::Node& n = psb->m_nodes[i]; + if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue; + if (n.m_im <= 0) { - drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); + drawVertex(idraw, n.m_x, 0.25, btVector3(1, 0, 0)); } } } - - /* Notes */ - if(0!=(drawflags&fDrawFlags::Notes)) + /* Notes */ + if (0 != (drawflags & fDrawFlags::Notes)) { - for(i=0;im_notes.size();++i) + for (i = 0; i < psb->m_notes.size(); ++i) { - const btSoftBody::Note& n=psb->m_notes[i]; - btVector3 p=n.m_offset; - for(int j=0;jm_notes[i]; + btVector3 p = n.m_offset; + for (int j = 0; j < n.m_rank; ++j) { - p+=n.m_nodes[j]->m_x*n.m_coords[j]; + p += n.m_nodes[j]->m_x * n.m_coords[j]; } - idraw->draw3dText(p,n.m_text); + idraw->draw3dText(p, n.m_text); } } - /* Node tree */ - if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw); - /* Face tree */ - if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw); - /* Cluster tree */ - if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw); - /* Joints */ - if(0!=(drawflags&fDrawFlags::Joints)) + /* Node tree */ + if (0 != (drawflags & fDrawFlags::NodeTree)) DrawNodeTree(psb, idraw); + /* Face tree */ + if (0 != (drawflags & fDrawFlags::FaceTree)) DrawFaceTree(psb, idraw); + /* Cluster tree */ + if (0 != (drawflags & fDrawFlags::ClusterTree)) DrawClusterTree(psb, idraw); + /* Joints */ + if (0 != (drawflags & fDrawFlags::Joints)) { - for(i=0;im_joints.size();++i) + for (i = 0; i < psb->m_joints.size(); ++i) { - const btSoftBody::Joint* pj=psb->m_joints[i]; - switch(pj->Type()) + const btSoftBody::Joint* pj = psb->m_joints[i]; + switch (pj->Type()) { - case btSoftBody::Joint::eType::Linear: + case btSoftBody::Joint::eType::Linear: { - const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; - const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; - idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); - idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); - drawVertex(idraw,a0,0.25,btVector3(1,1,0)); - drawVertex(idraw,a1,0.25,btVector3(0,1,1)); + const btSoftBody::LJoint* pjl = (const btSoftBody::LJoint*)pj; + const btVector3 a0 = pj->m_bodies[0].xform() * pjl->m_refs[0]; + const btVector3 a1 = pj->m_bodies[1].xform() * pjl->m_refs[1]; + idraw->drawLine(pj->m_bodies[0].xform().getOrigin(), a0, btVector3(1, 1, 0)); + idraw->drawLine(pj->m_bodies[1].xform().getOrigin(), a1, btVector3(0, 1, 1)); + drawVertex(idraw, a0, 0.25, btVector3(1, 1, 0)); + drawVertex(idraw, a1, 0.25, btVector3(0, 1, 1)); } break; - case btSoftBody::Joint::eType::Angular: + case btSoftBody::Joint::eType::Angular: { //const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; - const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); - const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); - const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; - idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); - idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); - idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); - idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); + const btVector3 o0 = pj->m_bodies[0].xform().getOrigin(); + const btVector3 o1 = pj->m_bodies[1].xform().getOrigin(); + const btVector3 a0 = pj->m_bodies[0].xform().getBasis() * pj->m_refs[0]; + const btVector3 a1 = pj->m_bodies[1].xform().getBasis() * pj->m_refs[1]; + idraw->drawLine(o0, o0 + a0 * 10, btVector3(1, 1, 0)); + idraw->drawLine(o0, o0 + a1 * 10, btVector3(1, 1, 0)); + idraw->drawLine(o1, o1 + a0 * 10, btVector3(0, 1, 1)); + idraw->drawLine(o1, o1 + a1 * 10, btVector3(0, 1, 1)); break; } default: { } - - } + } } } } // -void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool /*stress*/) +void btSoftBodyHelpers::DrawInfos(btSoftBody* psb, + btIDebugDraw* idraw, + bool masses, + bool areas, + bool /*stress*/) { - for(int i=0;im_nodes.size();++i) + for (int i = 0; i < psb->m_nodes.size(); ++i) { - const btSoftBody::Node& n=psb->m_nodes[i]; - char text[2048]={0}; - char buff[1024]; - if(masses) + const btSoftBody::Node& n = psb->m_nodes[i]; + char text[2048] = {0}; + char buff[1024]; + if (masses) { - sprintf(buff," M(%.2f)",1/n.m_im); - strcat(text,buff); + sprintf(buff, " M(%.2f)", 1 / n.m_im); + strcat(text, buff); } - if(areas) + if (areas) { - sprintf(buff," A(%.2f)",n.m_area); - strcat(text,buff); + sprintf(buff, " A(%.2f)", n.m_area); + strcat(text, buff); } - if(text[0]) idraw->draw3dText(n.m_x,text); + if (text[0]) idraw->draw3dText(n.m_x, text); } } // -void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) +void btSoftBodyHelpers::DrawNodeTree(btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { - drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); + drawTree(idraw, psb->m_ndbvt.m_root, 0, btVector3(1, 0, 1), btVector3(1, 1, 1), mindepth, maxdepth); } // -void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) +void btSoftBodyHelpers::DrawFaceTree(btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { - drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); + drawTree(idraw, psb->m_fdbvt.m_root, 0, btVector3(0, 1, 0), btVector3(1, 0, 0), mindepth, maxdepth); } // -void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) +void btSoftBodyHelpers::DrawClusterTree(btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { - drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); + drawTree(idraw, psb->m_cdbvt.m_root, 0, btVector3(0, 1, 1), btVector3(1, 0, 0), mindepth, maxdepth); } - //The btSoftBody object from the BulletSDK includes an array of Nodes and Links. These links appear -// to be first set up to connect a node to between 5 and 6 of its neighbors [480 links], -//and then to the rest of the nodes after the execution of the Floyd-Warshall graph algorithm -//[another 930 links]. +// to be first set up to connect a node to between 5 and 6 of its neighbors [480 links], +//and then to the rest of the nodes after the execution of the Floyd-Warshall graph algorithm +//[another 930 links]. //The way the links are stored by default, we have a number of cases where adjacent links share a node in common -// - this leads to the creation of a data dependency through memory. -//The PSolve_Links() function reads and writes nodes as it iterates over each link. -//So, we now have the possibility of a data dependency between iteration X -//that processes link L with iteration X+1 that processes link L+1 -//because L and L+1 have one node in common, and iteration X updates the positions of that node, +// - this leads to the creation of a data dependency through memory. +//The PSolve_Links() function reads and writes nodes as it iterates over each link. +//So, we now have the possibility of a data dependency between iteration X +//that processes link L with iteration X+1 that processes link L+1 +//because L and L+1 have one node in common, and iteration X updates the positions of that node, //and iteration X+1 reads in the position of that shared node. // -//Such a memory dependency limits the ability of a modern CPU to speculate beyond -//a certain point because it has to respect a possible dependency -//- this prevents the CPU from making full use of its out-of-order resources. -//If we re-order the links such that we minimize the cases where a link L and L+1 share a common node, -//we create a temporal gap between when the node position is written, -//and when it is subsequently read. This in turn allows the CPU to continue execution without -//risking a dependency violation. Such a reordering would result in significant speedups on -//modern CPUs with lots of execution resources. -//In our testing, we see it have a tremendous impact not only on the A7, -//but also on all x86 cores that ship with modern Macs. -//The attached source file includes a single function (ReoptimizeLinkOrder) which can be called on a -//btSoftBody object in the solveConstraints() function before the actual solver is invoked, +//Such a memory dependency limits the ability of a modern CPU to speculate beyond +//a certain point because it has to respect a possible dependency +//- this prevents the CPU from making full use of its out-of-order resources. +//If we re-order the links such that we minimize the cases where a link L and L+1 share a common node, +//we create a temporal gap between when the node position is written, +//and when it is subsequently read. This in turn allows the CPU to continue execution without +//risking a dependency violation. Such a reordering would result in significant speedups on +//modern CPUs with lots of execution resources. +//In our testing, we see it have a tremendous impact not only on the A7, +//but also on all x86 cores that ship with modern Macs. +//The attached source file includes a single function (ReoptimizeLinkOrder) which can be called on a +//btSoftBody object in the solveConstraints() function before the actual solver is invoked, //or right after generateBendingConstraints() once we have all 1410 links. - //=================================================================== // // -// This function takes in a list of interdependent Links and tries +// This function takes in a list of interdependent Links and tries // to maximize the distance between calculation // of dependent links. This increases the amount of parallelism that can // be exploited by out-of-order instruction processors with large but @@ -522,93 +521,103 @@ void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, //=================================================================== // A small structure to track lists of dependent link calculations -class LinkDeps_t { - public: - int value; // A link calculation that is dependent on this one - // Positive values = "input A" while negative values = "input B" - LinkDeps_t *next; // Next dependence in the list +class LinkDeps_t +{ +public: + int value; // A link calculation that is dependent on this one + // Positive values = "input A" while negative values = "input B" + LinkDeps_t* next; // Next dependence in the list }; -typedef LinkDeps_t *LinkDepsPtr_t; +typedef LinkDeps_t* LinkDepsPtr_t; // Dependency list constants -#define REOP_NOT_DEPENDENT -1 -#define REOP_NODE_COMPLETE -2 // Must be less than REOP_NOT_DEPENDENT +#define REOP_NOT_DEPENDENT -1 +#define REOP_NODE_COMPLETE -2 // Must be less than REOP_NOT_DEPENDENT - -void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody *psb /* This can be replaced by a btSoftBody pointer */) +void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody* psb /* This can be replaced by a btSoftBody pointer */) { - int i, nLinks=psb->m_links.size(), nNodes=psb->m_nodes.size(); - btSoftBody::Link *lr; + int i, nLinks = psb->m_links.size(), nNodes = psb->m_nodes.size(); + btSoftBody::Link* lr; int ar, br; - btSoftBody::Node *node0 = &(psb->m_nodes[0]); - btSoftBody::Node *node1 = &(psb->m_nodes[1]); + btSoftBody::Node* node0 = &(psb->m_nodes[0]); + btSoftBody::Node* node1 = &(psb->m_nodes[1]); LinkDepsPtr_t linkDep; int readyListHead, readyListTail, linkNum, linkDepFrees, depLink; - + // Allocate temporary buffers - int *nodeWrittenAt = new int[nNodes+1]; // What link calculation produced this node's current values? - int *linkDepA = new int[nLinks]; // Link calculation input is dependent upon prior calculation #N - int *linkDepB = new int[nLinks]; - int *readyList = new int[nLinks]; // List of ready-to-process link calculations (# of links, maximum) - LinkDeps_t *linkDepFreeList = new LinkDeps_t[2*nLinks]; // Dependent-on-me list elements (2x# of links, maximum) - LinkDepsPtr_t *linkDepListStarts = new LinkDepsPtr_t[nLinks]; // Start nodes of dependent-on-me lists, one for each link - + int* nodeWrittenAt = new int[nNodes + 1]; // What link calculation produced this node's current values? + int* linkDepA = new int[nLinks]; // Link calculation input is dependent upon prior calculation #N + int* linkDepB = new int[nLinks]; + int* readyList = new int[nLinks]; // List of ready-to-process link calculations (# of links, maximum) + LinkDeps_t* linkDepFreeList = new LinkDeps_t[2 * nLinks]; // Dependent-on-me list elements (2x# of links, maximum) + LinkDepsPtr_t* linkDepListStarts = new LinkDepsPtr_t[nLinks]; // Start nodes of dependent-on-me lists, one for each link + // Copy the original, unsorted links to a side buffer - btSoftBody::Link *linkBuffer = new btSoftBody::Link[nLinks]; - memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link)*nLinks); + btSoftBody::Link* linkBuffer = new btSoftBody::Link[nLinks]; + memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link) * nLinks); // Clear out the node setup and ready list - for (i=0; i < nNodes+1; i++) { + for (i = 0; i < nNodes + 1; i++) + { nodeWrittenAt[i] = REOP_NOT_DEPENDENT; } - for (i=0; i < nLinks; i++) { + for (i = 0; i < nLinks; i++) + { linkDepListStarts[i] = NULL; } readyListHead = readyListTail = linkDepFrees = 0; // Initial link analysis to set up data structures - for (i=0; i < nLinks; i++) { - + for (i = 0; i < nLinks; i++) + { // Note which prior link calculations we are dependent upon & build up dependence lists lr = &(psb->m_links[i]); - ar = (lr->m_n[0] - node0)/(node1 - node0); - br = (lr->m_n[1] - node0)/(node1 - node0); - if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT) { + ar = (lr->m_n[0] - node0) / (node1 - node0); + br = (lr->m_n[1] - node0) / (node1 - node0); + if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT) + { linkDepA[i] = nodeWrittenAt[ar]; linkDep = &linkDepFreeList[linkDepFrees++]; linkDep->value = i; linkDep->next = linkDepListStarts[nodeWrittenAt[ar]]; linkDepListStarts[nodeWrittenAt[ar]] = linkDep; - } else { + } + else + { linkDepA[i] = REOP_NOT_DEPENDENT; } - if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) { + if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) + { linkDepB[i] = nodeWrittenAt[br]; linkDep = &linkDepFreeList[linkDepFrees++]; - linkDep->value = -(i+1); + linkDep->value = -(i + 1); linkDep->next = linkDepListStarts[nodeWrittenAt[br]]; linkDepListStarts[nodeWrittenAt[br]] = linkDep; - } else { + } + else + { linkDepB[i] = REOP_NOT_DEPENDENT; } - + // Add this link to the initial ready list, if it is not dependent on any other links - if ((linkDepA[i] == REOP_NOT_DEPENDENT) && (linkDepB[i] == REOP_NOT_DEPENDENT)) { + if ((linkDepA[i] == REOP_NOT_DEPENDENT) && (linkDepB[i] == REOP_NOT_DEPENDENT)) + { readyList[readyListTail++] = i; - linkDepA[i] = linkDepB[i] = REOP_NODE_COMPLETE; // Probably not needed now + linkDepA[i] = linkDepB[i] = REOP_NODE_COMPLETE; // Probably not needed now } - + // Update the nodes to mark which ones are calculated by this link nodeWrittenAt[ar] = nodeWrittenAt[br] = i; } - + // Process the ready list and create the sorted list of links // -- By treating the ready list as a queue, we maximize the distance between any // inter-dependent node calculations // -- All other (non-related) nodes in the ready list will automatically be inserted // in between each set of inter-dependent link calculations by this loop i = 0; - while (readyListHead != readyListTail) { + while (readyListHead != readyListTail) + { // Use ready list to select the next link to process linkNum = readyList[readyListHead++]; // Copy the next-to-calculate link back into the original link array @@ -616,180 +625,183 @@ void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody *psb /* This can be repla // Free up any link inputs that are dependent on this one linkDep = linkDepListStarts[linkNum]; - while (linkDep) { + while (linkDep) + { depLink = linkDep->value; - if (depLink >= 0) { + if (depLink >= 0) + { linkDepA[depLink] = REOP_NOT_DEPENDENT; - } else { + } + else + { depLink = -depLink - 1; linkDepB[depLink] = REOP_NOT_DEPENDENT; } // Add this dependent link calculation to the ready list if *both* inputs are clear - if ((linkDepA[depLink] == REOP_NOT_DEPENDENT) && (linkDepB[depLink] == REOP_NOT_DEPENDENT)) { + if ((linkDepA[depLink] == REOP_NOT_DEPENDENT) && (linkDepB[depLink] == REOP_NOT_DEPENDENT)) + { readyList[readyListTail++] = depLink; - linkDepA[depLink] = linkDepB[depLink] = REOP_NODE_COMPLETE; // Probably not needed now + linkDepA[depLink] = linkDepB[depLink] = REOP_NODE_COMPLETE; // Probably not needed now } linkDep = linkDep->next; } } // Delete the temporary buffers - delete [] nodeWrittenAt; - delete [] linkDepA; - delete [] linkDepB; - delete [] readyList; - delete [] linkDepFreeList; - delete [] linkDepListStarts; - delete [] linkBuffer; + delete[] nodeWrittenAt; + delete[] linkDepA; + delete[] linkDepB; + delete[] readyList; + delete[] linkDepFreeList; + delete[] linkDepListStarts; + delete[] linkBuffer; } - // -void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) +void btSoftBodyHelpers::DrawFrame(btSoftBody* psb, + btIDebugDraw* idraw) { - if(psb->m_pose.m_bframe) + if (psb->m_pose.m_bframe) { - static const btScalar ascl=10; - static const btScalar nscl=(btScalar)0.1; - const btVector3 com=psb->m_pose.m_com; - const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl; - const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); - const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); - const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); - idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); - idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); - idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); - for(int i=0;im_pose.m_pos.size();++i) + static const btScalar ascl = 10; + static const btScalar nscl = (btScalar)0.1; + const btVector3 com = psb->m_pose.m_com; + const btMatrix3x3 trs = psb->m_pose.m_rot * psb->m_pose.m_scl; + const btVector3 Xaxis = (trs * btVector3(1, 0, 0)).normalized(); + const btVector3 Yaxis = (trs * btVector3(0, 1, 0)).normalized(); + const btVector3 Zaxis = (trs * btVector3(0, 0, 1)).normalized(); + idraw->drawLine(com, com + Xaxis * ascl, btVector3(1, 0, 0)); + idraw->drawLine(com, com + Yaxis * ascl, btVector3(0, 1, 0)); + idraw->drawLine(com, com + Zaxis * ascl, btVector3(0, 0, 1)); + for (int i = 0; i < psb->m_pose.m_pos.size(); ++i) { - const btVector3 x=com+trs*psb->m_pose.m_pos[i]; - drawVertex(idraw,x,nscl,btVector3(1,0,1)); + const btVector3 x = com + trs * psb->m_pose.m_pos[i]; + drawVertex(idraw, x, nscl, btVector3(1, 0, 1)); } } } // -btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, - const btVector3& to, - int res, - int fixeds) +btSoftBody* btSoftBodyHelpers::CreateRope(btSoftBodyWorldInfo& worldInfo, const btVector3& from, + const btVector3& to, + int res, + int fixeds) { - /* Create nodes */ - const int r=res+2; - btVector3* x=new btVector3[r]; - btScalar* m=new btScalar[r]; + /* Create nodes */ + const int r = res + 2; + btVector3* x = new btVector3[r]; + btScalar* m = new btScalar[r]; int i; - for(i=0;isetMass(0,0); - if(fixeds&2) psb->setMass(r-1,0); + btSoftBody* psb = new btSoftBody(&worldInfo, r, x, m); + if (fixeds & 1) psb->setMass(0, 0); + if (fixeds & 2) psb->setMass(r - 1, 0); delete[] x; delete[] m; - /* Create links */ - for(i=1;iappendLink(i-1,i); + psb->appendLink(i - 1, i); } - /* Finished */ - return(psb); + /* Finished */ + return (psb); } // -btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags) +btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags) { -#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) - /* Create nodes */ - if((resx<2)||(resy<2)) return(0); - const int rx=resx; - const int ry=resy; - const int tot=rx*ry; - btVector3* x=new btVector3[tot]; - btScalar* m=new btScalar[tot]; +#define IDX(_x_, _y_) ((_y_)*rx + (_x_)) + /* Create nodes */ + if ((resx < 2) || (resy < 2)) return (0); + const int rx = resx; + const int ry = resy; + const int tot = rx * ry; + btVector3* x = new btVector3[tot]; + btScalar* m = new btScalar[tot]; int iy; - for(iy=0;iysetMass(IDX(0,0),0); - if(fixeds&2) psb->setMass(IDX(rx-1,0),0); - if(fixeds&4) psb->setMass(IDX(0,ry-1),0); - if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); + btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m); + if (fixeds & 1) psb->setMass(IDX(0, 0), 0); + if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0); + if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0); + if (fixeds & 8) psb->setMass(IDX(rx - 1, ry - 1), 0); delete[] x; delete[] m; - /* Create links and faces */ - for(iy=0;iyappendLink(idx,IDX(ix+1,iy)); - if(mdy) psb->appendLink(idx,IDX(ix,iy+1)); - if(mdx&&mdy) + const int idx = IDX(ix, iy); + const bool mdx = (ix + 1) < rx; + const bool mdy = (iy + 1) < ry; + if (mdx) psb->appendLink(idx, IDX(ix + 1, iy)); + if (mdy) psb->appendLink(idx, IDX(ix, iy + 1)); + if (mdx && mdy) { - if((ix+iy)&1) + if ((ix + iy) & 1) { - psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1)); - psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); - if(gendiags) + psb->appendFace(IDX(ix, iy), IDX(ix + 1, iy), IDX(ix + 1, iy + 1)); + psb->appendFace(IDX(ix, iy), IDX(ix + 1, iy + 1), IDX(ix, iy + 1)); + if (gendiags) { - psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1)); + psb->appendLink(IDX(ix, iy), IDX(ix + 1, iy + 1)); } } else { - psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); - psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1)); - if(gendiags) + psb->appendFace(IDX(ix, iy + 1), IDX(ix, iy), IDX(ix + 1, iy)); + psb->appendFace(IDX(ix, iy + 1), IDX(ix + 1, iy), IDX(ix + 1, iy + 1)); + if (gendiags) { - psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1)); + psb->appendLink(IDX(ix + 1, iy), IDX(ix, iy + 1)); } } } } } - /* Finished */ + /* Finished */ #undef IDX - return(psb); + return (psb); } // -btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags, - float* tex_coords) +btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags, + float* tex_coords) { - /* * * corners: @@ -857,92 +869,92 @@ btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, * */ -#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) - /* Create nodes */ - if((resx<2)||(resy<2)) return(0); - const int rx=resx; - const int ry=resy; - const int tot=rx*ry; - btVector3* x=new btVector3[tot]; - btScalar* m=new btScalar[tot]; +#define IDX(_x_, _y_) ((_y_)*rx + (_x_)) + /* Create nodes */ + if ((resx < 2) || (resy < 2)) return (0); + const int rx = resx; + const int ry = resy; + const int tot = rx * ry; + btVector3* x = new btVector3[tot]; + btScalar* m = new btScalar[tot]; int iy; - for(iy=0;iysetMass(IDX(0,0),0); - if(fixeds&2) psb->setMass(IDX(rx-1,0),0); - if(fixeds&4) psb->setMass(IDX(0,ry-1),0); - if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); - if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0); - if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0); - if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0); - if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0); - if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0); + btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m); + if (fixeds & 1) psb->setMass(IDX(0, 0), 0); + if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0); + if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0); + if (fixeds & 8) psb->setMass(IDX(rx - 1, ry - 1), 0); + if (fixeds & 16) psb->setMass(IDX((rx - 1) / 2, 0), 0); + if (fixeds & 32) psb->setMass(IDX(0, (ry - 1) / 2), 0); + if (fixeds & 64) psb->setMass(IDX(rx - 1, (ry - 1) / 2), 0); + if (fixeds & 128) psb->setMass(IDX((rx - 1) / 2, ry - 1), 0); + if (fixeds & 256) psb->setMass(IDX((rx - 1) / 2, (ry - 1) / 2), 0); delete[] x; delete[] m; - int z = 0; - /* Create links and faces */ - for(iy=0;iyappendLink(node00,node01); - if(mdy) psb->appendLink(node00,node10); - if(mdx&&mdy) + if (mdx) psb->appendLink(node00, node01); + if (mdy) psb->appendLink(node00, node10); + if (mdx && mdy) { - psb->appendFace(node00,node10,node11); - if (tex_coords) { - tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); - tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); - tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); + psb->appendFace(node00, node10, node11); + if (tex_coords) + { + tex_coords[z + 0] = CalculateUV(resx, resy, ix, iy, 0); + tex_coords[z + 1] = CalculateUV(resx, resy, ix, iy, 1); + tex_coords[z + 2] = CalculateUV(resx, resy, ix, iy, 0); + tex_coords[z + 3] = CalculateUV(resx, resy, ix, iy, 2); + tex_coords[z + 4] = CalculateUV(resx, resy, ix, iy, 3); + tex_coords[z + 5] = CalculateUV(resx, resy, ix, iy, 2); } - psb->appendFace(node11,node01,node00); - if (tex_coords) { - tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); - tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); - tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); + psb->appendFace(node11, node01, node00); + if (tex_coords) + { + tex_coords[z + 6] = CalculateUV(resx, resy, ix, iy, 3); + tex_coords[z + 7] = CalculateUV(resx, resy, ix, iy, 2); + tex_coords[z + 8] = CalculateUV(resx, resy, ix, iy, 3); + tex_coords[z + 9] = CalculateUV(resx, resy, ix, iy, 1); + tex_coords[z + 10] = CalculateUV(resx, resy, ix, iy, 0); + tex_coords[z + 11] = CalculateUV(resx, resy, ix, iy, 1); } - if (gendiags) psb->appendLink(node00,node11); + if (gendiags) psb->appendLink(node00, node11); z += 12; } } } - /* Finished */ + /* Finished */ #undef IDX - return(psb); + return (psb); } -float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) +float btSoftBodyHelpers::CalculateUV(int resx, int resy, int ix, int iy, int id) { - /* * * @@ -968,90 +980,93 @@ float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) * */ - float tc=0.0f; - if (id == 0) { - tc = (1.0f/((resx-1))*ix); + float tc = 0.0f; + if (id == 0) + { + tc = (1.0f / ((resx - 1)) * ix); } - else if (id==1) { - tc = (1.0f/((resy-1))*(resy-1-iy)); + else if (id == 1) + { + tc = (1.0f / ((resy - 1)) * (resy - 1 - iy)); } - else if (id==2) { - tc = (1.0f/((resy-1))*(resy-1-iy-1)); + else if (id == 2) + { + tc = (1.0f / ((resy - 1)) * (resy - 1 - iy - 1)); } - else if (id==3) { - tc = (1.0f/((resx-1))*(ix+1)); + else if (id == 3) + { + tc = (1.0f / ((resx - 1)) * (ix + 1)); } return tc; } // -btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, - const btVector3& radius, - int res) +btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, const btVector3& center, + const btVector3& radius, + int res) { - struct Hammersley + struct Hammersley { - static void Generate(btVector3* x,int n) + static void Generate(btVector3* x, int n) { - for(int i=0;i>=1) if(j&1) t+=p; - btScalar w=2*t-1; - btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; - btScalar s=btSqrt(1-w*w); - *x++=btVector3(s*btCos(a),s*btSin(a),w); + btScalar p = 0.5, t = 0; + for (int j = i; j; p *= 0.5, j >>= 1) + if (j & 1) t += p; + btScalar w = 2 * t - 1; + btScalar a = (SIMD_PI + 2 * i * SIMD_PI) / n; + btScalar s = btSqrt(1 - w * w); + *x++ = btVector3(s * btCos(a), s * btSin(a), w); } } }; - btAlignedObjectArray vtx; - vtx.resize(3+res); - Hammersley::Generate(&vtx[0],vtx.size()); - for(int i=0;i vtx; + vtx.resize(3 + res); + Hammersley::Generate(&vtx[0], vtx.size()); + for (int i = 0; i < vtx.size(); ++i) { - vtx[i]=vtx[i]*radius+center; + vtx[i] = vtx[i] * radius + center; } - return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size())); + return (CreateFromConvexHull(worldInfo, &vtx[0], vtx.size())); } - - // -btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, - const int* triangles, - int ntriangles, bool randomizeConstraints) +btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices, + const int* triangles, + int ntriangles, bool randomizeConstraints) { - int maxidx=0; - int i,j,ni; + int maxidx = 0; + int i, j, ni; - for(i=0,ni=ntriangles*3;i chks; - btAlignedObjectArray vtx; - chks.resize(maxidx*maxidx,false); + btAlignedObjectArray chks; + btAlignedObjectArray vtx; + chks.resize(maxidx * maxidx, false); vtx.resize(maxidx); - for(i=0,j=0,ni=maxidx*3;iappendLink(idx[j],idx[k]); + chks[IDX(idx[j], idx[k])] = true; + chks[IDX(idx[k], idx[j])] = true; + psb->appendLink(idx[j], idx[k]); } } #undef IDX - psb->appendFace(idx[0],idx[1],idx[2]); + psb->appendFace(idx[0], idx[1], idx[2]); } if (randomizeConstraints) @@ -1059,44 +1074,41 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo psb->randomizeConstraints(); } - return(psb); + return (psb); } // -btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, - int nvertices, bool randomizeConstraints) +btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, + int nvertices, bool randomizeConstraints) { - HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); - HullResult hres; - HullLibrary hlib;/*??*/ - hdsc.mMaxVertices=nvertices; - hlib.CreateConvexHull(hdsc,hres); - btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, - &hres.m_OutputVertices[0],0); - for(int i=0;i<(int)hres.mNumFaces;++i) + HullDesc hdsc(QF_TRIANGLES, nvertices, vertices); + HullResult hres; + HullLibrary hlib; /*??*/ + hdsc.mMaxVertices = nvertices; + hlib.CreateConvexHull(hdsc, hres); + btSoftBody* psb = new btSoftBody(&worldInfo, (int)hres.mNumOutputVertices, + &hres.m_OutputVertices[0], 0); + for (int i = 0; i < (int)hres.mNumFaces; ++i) { - const int idx[]={ static_cast(hres.m_Indices[i*3+0]), - static_cast(hres.m_Indices[i*3+1]), - static_cast(hres.m_Indices[i*3+2])}; - if(idx[0]appendLink( idx[0],idx[1]); - if(idx[1]appendLink( idx[1],idx[2]); - if(idx[2]appendLink( idx[2],idx[0]); - psb->appendFace(idx[0],idx[1],idx[2]); + const int idx[] = {static_cast(hres.m_Indices[i * 3 + 0]), + static_cast(hres.m_Indices[i * 3 + 1]), + static_cast(hres.m_Indices[i * 3 + 2])}; + if (idx[0] < idx[1]) psb->appendLink(idx[0], idx[1]); + if (idx[1] < idx[2]) psb->appendLink(idx[1], idx[2]); + if (idx[2] < idx[0]) psb->appendLink(idx[2], idx[0]); + psb->appendFace(idx[0], idx[1], idx[2]); } hlib.ReleaseResult(hres); if (randomizeConstraints) { psb->randomizeConstraints(); } - return(psb); + return (psb); } - - - static int nextLine(const char* buffer) { - int numBytesRead=0; + int numBytesRead = 0; while (*buffer != '\n') { @@ -1104,8 +1116,7 @@ static int nextLine(const char* buffer) numBytesRead++; } - - if (buffer[0]==0x0a) + if (buffer[0] == 0x0a) { buffer++; numBytesRead++; @@ -1113,8 +1124,8 @@ static int nextLine(const char* buffer) return numBytesRead; } -/* Create from TetGen .ele, .face, .node data */ -btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, +/* Create from TetGen .ele, .face, .node data */ +btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, const char* ele, const char* face, const char* node, @@ -1122,38 +1133,38 @@ btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldIn bool btetralinks, bool bfacesfromtetras) { -btAlignedObjectArray pos; -int nnode=0; -int ndims=0; -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); -node += nextLine(node); - -pos.resize(nnode); -for(int i=0;i>index; -// sn>>x;sn>>y;sn>>z; + btAlignedObjectArray pos; + int nnode = 0; + int ndims = 0; + 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); node += nextLine(node); - //for(int j=0;j>a; + pos.resize(nnode); + for (int i = 0; i < pos.size(); ++i) + { + int index = 0; + //int bound=0; + float x, y, z; + sscanf(node, "%d %f %f %f", &index, &x, &y, &z); - //if(hasbounds) - // sn>>bound; + // sn>>index; + // sn>>x;sn>>y;sn>>z; + node += nextLine(node); - pos[index].setX(btScalar(x)); - pos[index].setY(btScalar(y)); - pos[index].setZ(btScalar(z)); + //for(int j=0;j>a; + + //if(hasbounds) + // sn>>bound; + + pos[index].setX(btScalar(x)); + pos[index].setY(btScalar(y)); + pos[index].setZ(btScalar(z)); } -btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0); + btSoftBody* psb = new btSoftBody(&worldInfo, nnode, &pos[0], 0); #if 0 if(face&&face[0]) { @@ -1178,42 +1189,41 @@ if(face&&face[0]) } #endif -if(ele&&ele[0]) + if (ele && ele[0]) { - int ntetra=0; - int ncorner=0; - int neattrb=0; - sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb); - ele += nextLine(ele); - - //se>>ntetra;se>>ncorner;se>>neattrb; - for(int i=0;i>index; - //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3]; - sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]); - ele+=nextLine(ele); - //for(int j=0;j>a; - psb->appendTetra(ni[0],ni[1],ni[2],ni[3]); - if(btetralinks) + //se>>ntetra;se>>ncorner;se>>neattrb; + for (int i = 0; i < ntetra; ++i) + { + int index = 0; + int ni[4]; + + //se>>index; + //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3]; + sscanf(ele, "%d %d %d %d %d", &index, &ni[0], &ni[1], &ni[2], &ni[3]); + ele += nextLine(ele); + //for(int j=0;j>a; + psb->appendTetra(ni[0], ni[1], ni[2], ni[3]); + if (btetralinks) { - psb->appendLink(ni[0],ni[1],0,true); - psb->appendLink(ni[1],ni[2],0,true); - psb->appendLink(ni[2],ni[0],0,true); - psb->appendLink(ni[0],ni[3],0,true); - psb->appendLink(ni[1],ni[3],0,true); - psb->appendLink(ni[2],ni[3],0,true); + psb->appendLink(ni[0], ni[1], 0, true); + psb->appendLink(ni[1], ni[2], 0, true); + psb->appendLink(ni[2], ni[0], 0, true); + psb->appendLink(ni[0], ni[3], 0, true); + psb->appendLink(ni[1], ni[3], 0, true); + psb->appendLink(ni[2], ni[3], 0, true); } } } -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); + 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/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyHelpers.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyHelpers.h index 727153010..e433558c1 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyHelpers.h @@ -22,127 +22,130 @@ subject to the following restrictions: // Helpers // -/* fDrawFlags */ -struct fDrawFlags { enum _ { - Nodes = 0x0001, - Links = 0x0002, - Faces = 0x0004, - Tetras = 0x0008, - Normals = 0x0010, - Contacts = 0x0020, - Anchors = 0x0040, - Notes = 0x0080, - Clusters = 0x0100, - NodeTree = 0x0200, - FaceTree = 0x0400, - ClusterTree = 0x0800, - Joints = 0x1000, - /* presets */ - Std = Links+Faces+Tetras+Anchors+Notes+Joints, - StdTetra = Std-Faces+Tetras -};}; - -struct btSoftBodyHelpers +/* fDrawFlags */ +struct fDrawFlags { - /* Draw body */ - static void Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags=fDrawFlags::Std); - /* Draw body infos */ - static void DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool stress); - /* Draw node tree */ - static void DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); - /* Draw face tree */ - static void DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); - /* Draw cluster tree */ - static void DrawClusterTree(btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); - /* Draw rigid frame */ - static void DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw); - /* Create a rope */ - static btSoftBody* CreateRope( btSoftBodyWorldInfo& worldInfo, - const btVector3& from, - const btVector3& to, - int res, - int fixeds); - /* Create a patch */ - static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags); - /* Create a patch with UV Texture Coordinates */ - static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags, - float* tex_coords=0); - static float CalculateUV(int resx,int resy,int ix,int iy,int id); - /* Create an ellipsoid */ - static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, - const btVector3& center, - const btVector3& radius, - int res); - /* Create from trimesh */ - static btSoftBody* CreateFromTriMesh( btSoftBodyWorldInfo& worldInfo, - const btScalar* vertices, - const int* triangles, - int ntriangles, - bool randomizeConstraints = true); - /* Create from convex-hull */ - static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo, - const btVector3* vertices, - int nvertices, - bool randomizeConstraints = true); + enum _ + { + Nodes = 0x0001, + Links = 0x0002, + Faces = 0x0004, + Tetras = 0x0008, + Normals = 0x0010, + Contacts = 0x0020, + Anchors = 0x0040, + Notes = 0x0080, + Clusters = 0x0100, + NodeTree = 0x0200, + FaceTree = 0x0400, + ClusterTree = 0x0800, + Joints = 0x1000, + /* presets */ + Std = Links + Faces + Tetras + Anchors + Notes + Joints, + StdTetra = Std - Faces + Tetras + }; +}; +struct btSoftBodyHelpers +{ + /* Draw body */ + static void Draw(btSoftBody* psb, + btIDebugDraw* idraw, + int drawflags = fDrawFlags::Std); + /* Draw body infos */ + static void DrawInfos(btSoftBody* psb, + btIDebugDraw* idraw, + bool masses, + bool areas, + bool stress); + /* Draw node tree */ + static void DrawNodeTree(btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth = 0, + int maxdepth = -1); + /* Draw face tree */ + static void DrawFaceTree(btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth = 0, + int maxdepth = -1); + /* Draw cluster tree */ + static void DrawClusterTree(btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth = 0, + int maxdepth = -1); + /* Draw rigid frame */ + static void DrawFrame(btSoftBody* psb, + btIDebugDraw* idraw); + /* Create a rope */ + static btSoftBody* CreateRope(btSoftBodyWorldInfo& worldInfo, + const btVector3& from, + const btVector3& to, + int res, + int fixeds); + /* Create a patch */ + static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags); + /* Create a patch with UV Texture Coordinates */ + static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags, + float* tex_coords = 0); + static float CalculateUV(int resx, int resy, int ix, int iy, int id); + /* Create an ellipsoid */ + static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, + const btVector3& center, + const btVector3& radius, + int res); + /* Create from trimesh */ + static btSoftBody* CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo, + const btScalar* vertices, + const int* triangles, + int ntriangles, + bool randomizeConstraints = true); + /* Create from convex-hull */ + static btSoftBody* CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, + const btVector3* vertices, + int nvertices, + bool randomizeConstraints = true); - /* Export TetGen compatible .smesh file */ -// static void ExportAsSMeshFile( btSoftBody* psb, -// const char* filename); - /* Create from TetGen .ele, .face, .node files */ -// static btSoftBody* CreateFromTetGenFile( btSoftBodyWorldInfo& worldInfo, -// const char* ele, -// const char* face, -// const char* node, -// bool bfacelinks, -// bool btetralinks, -// bool bfacesfromtetras); - /* Create from TetGen .ele, .face, .node data */ - static btSoftBody* CreateFromTetGenData( btSoftBodyWorldInfo& worldInfo, - const char* ele, - const char* face, - const char* node, - bool bfacelinks, - bool btetralinks, - bool bfacesfromtetras); + /* Export TetGen compatible .smesh file */ + // static void ExportAsSMeshFile( btSoftBody* psb, + // const char* filename); + /* Create from TetGen .ele, .face, .node files */ + // static btSoftBody* CreateFromTetGenFile( btSoftBodyWorldInfo& worldInfo, + // const char* ele, + // const char* face, + // const char* node, + // bool bfacelinks, + // bool btetralinks, + // bool bfacesfromtetras); + /* Create from TetGen .ele, .face, .node data */ + static btSoftBody* CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, + const char* ele, + const char* face, + const char* node, + bool bfacelinks, + bool btetralinks, + bool bfacesfromtetras); /// Sort the list of links to move link calculations that are dependent upon earlier /// ones as far as possible away from the calculation of those values /// This tends to make adjacent loop iterations not dependent upon one another, /// so out-of-order processors can execute instructions from multiple iterations at once - static void ReoptimizeLinkOrder(btSoftBody *psb ); + static void ReoptimizeLinkOrder(btSoftBody* psb); }; -#endif //BT_SOFT_BODY_HELPERS_H +#endif //BT_SOFT_BODY_HELPERS_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyInternals.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyInternals.h index 1ad82616e..7efe514f3 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyInternals.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyInternals.h @@ -19,29 +19,37 @@ subject to the following restrictions: #include "btSoftBody.h" - #include "LinearMath/btQuickprof.h" #include "LinearMath/btPolarDecomposition.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionShapes/btConvexInternalShape.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -#include //for memset +#include //for memset // // btSymMatrix // template struct btSymMatrix { - btSymMatrix() : dim(0) {} - btSymMatrix(int n,const T& init=T()) { resize(n,init); } - void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } - int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r store; - int dim; -}; + btSymMatrix() : dim(0) {} + btSymMatrix(int n, const T& init = T()) { resize(n, init); } + void resize(int n, const T& init = T()) + { + dim = n; + store.resize((n * (n + 1)) / 2, init); + } + int index(int c, int r) const + { + if (c > r) btSwap(c, r); + btAssert(r < dim); + return ((r * (r + 1)) / 2 + c); + } + T& operator()(int c, int r) { return (store[index(c, r)]); } + const T& operator()(int c, int r) const { return (store[index(c, r)]); } + btAlignedObjectArray store; + int dim; +}; // // btSoftBodyCollisionShape @@ -49,67 +57,64 @@ struct btSymMatrix class btSoftBodyCollisionShape : public btConcaveShape { public: - btSoftBody* m_body; + btSoftBody* m_body; btSoftBodyCollisionShape(btSoftBody* backptr) { m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; - m_body=backptr; + m_body = backptr; } virtual ~btSoftBodyCollisionShape() { - } - void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const + void processAllTriangles(btTriangleCallback* /*callback*/, const btVector3& /*aabbMin*/, const btVector3& /*aabbMax*/) const { //not yet btAssert(0); } ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { /* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */ - const btVector3 mins=m_body->m_bounds[0]; - const btVector3 maxs=m_body->m_bounds[1]; - const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),maxs.y(),maxs.z()), - t*btVector3(mins.x(),maxs.y(),maxs.z())}; - aabbMin=aabbMax=crns[0]; - for(int i=1;i<8;++i) + const btVector3 mins = m_body->m_bounds[0]; + const btVector3 maxs = m_body->m_bounds[1]; + const btVector3 crns[] = {t * btVector3(mins.x(), mins.y(), mins.z()), + t * btVector3(maxs.x(), mins.y(), mins.z()), + t * btVector3(maxs.x(), maxs.y(), mins.z()), + t * btVector3(mins.x(), maxs.y(), mins.z()), + t * btVector3(mins.x(), mins.y(), maxs.z()), + t * btVector3(maxs.x(), mins.y(), maxs.z()), + t * btVector3(maxs.x(), maxs.y(), maxs.z()), + t * btVector3(mins.x(), maxs.y(), maxs.z())}; + aabbMin = aabbMax = crns[0]; + for (int i = 1; i < 8; ++i) { aabbMin.setMin(crns[i]); aabbMax.setMax(crns[i]); } } - - virtual void setLocalScaling(const btVector3& /*scaling*/) - { + virtual void setLocalScaling(const btVector3& /*scaling*/) + { ///na } virtual const btVector3& getLocalScaling() const { - static const btVector3 dummy(1,1,1); + static const btVector3 dummy(1, 1, 1); return dummy; } - virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const + virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const { ///not yet btAssert(0); } - virtual const char* getName()const + virtual const char* getName() const { return "SoftBody"; } - }; // @@ -118,48 +123,53 @@ public: class btSoftClusterCollisionShape : public btConvexInternalShape { public: - const btSoftBody::Cluster* m_cluster; + const btSoftBody::Cluster* m_cluster; - btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } + btSoftClusterCollisionShape(const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { - btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; - btScalar d=btDot(vec,n[0]->m_x); - int j=0; - for(int i=1,ni=m_cluster->m_nodes.size();im_nodes[0]; + btScalar d = btDot(vec, n[0]->m_x); + int j = 0; + for (int i = 1, ni = m_cluster->m_nodes.size(); i < ni; ++i) { - const btScalar k=btDot(vec,n[i]->m_x); - if(k>d) { d=k;j=i; } + const btScalar k = btDot(vec, n[i]->m_x); + if (k > d) + { + d = k; + j = i; + } } - return(n[j]->m_x); + return (n[j]->m_x); } - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return(localGetSupportingVertex(vec)); + return (localGetSupportingVertex(vec)); } //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const - {} + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const + { + } + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const + { + } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const - {} + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - {} - - virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } + virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } //debugging - virtual const char* getName()const {return "SOFTCLUSTER";} + virtual const char* getName() const { return "SOFTCLUSTER"; } - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { btConvexInternalShape::setMargin(margin); } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { return btConvexInternalShape::getMargin(); } @@ -171,229 +181,247 @@ public: // template -static inline void ZeroInitialize(T& value) +static inline void ZeroInitialize(T& value) { - memset(&value,0,sizeof(T)); + memset(&value, 0, sizeof(T)); } // template -static inline bool CompLess(const T& a,const T& b) -{ return(a -static inline bool CompGreater(const T& a,const T& b) -{ return(a>b); } -// -template -static inline T Lerp(const T& a,const T& b,btScalar t) -{ return(a+(b-a)*t); } -// -template -static inline T InvLerp(const T& a,const T& b,btScalar t) -{ return((b+a*t-b*t)/(a*b)); } -// -static inline btMatrix3x3 Lerp( const btMatrix3x3& a, - const btMatrix3x3& b, - btScalar t) +static inline bool CompLess(const T& a, const T& b) { - btMatrix3x3 r; - r[0]=Lerp(a[0],b[0],t); - r[1]=Lerp(a[1],b[1],t); - r[2]=Lerp(a[2],b[2],t); - return(r); + return (a < b); } // -static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) +template +static inline bool CompGreater(const T& a, const T& b) { - const btScalar sql=v.length2(); - if(sql>(maxlength*maxlength)) - return((v*maxlength)/btSqrt(sql)); + return (a > b); +} +// +template +static inline T Lerp(const T& a, const T& b, btScalar t) +{ + return (a + (b - a) * t); +} +// +template +static inline T InvLerp(const T& a, const T& b, btScalar t) +{ + return ((b + a * t - b * t) / (a * b)); +} +// +static inline btMatrix3x3 Lerp(const btMatrix3x3& a, + const btMatrix3x3& b, + btScalar t) +{ + btMatrix3x3 r; + r[0] = Lerp(a[0], b[0], t); + r[1] = Lerp(a[1], b[1], t); + r[2] = Lerp(a[2], b[2], t); + return (r); +} +// +static inline btVector3 Clamp(const btVector3& v, btScalar maxlength) +{ + const btScalar sql = v.length2(); + if (sql > (maxlength * maxlength)) + return ((v * maxlength) / btSqrt(sql)); else - return(v); + return (v); } // template -static inline T Clamp(const T& x,const T& l,const T& h) -{ return(xh?h:x); } +static inline T Clamp(const T& x, const T& l, const T& h) +{ + return (x < l ? l : x > h ? h : x); +} // template -static inline T Sq(const T& x) -{ return(x*x); } +static inline T Sq(const T& x) +{ + return (x * x); +} // template -static inline T Cube(const T& x) -{ return(x*x*x); } +static inline T Cube(const T& x) +{ + return (x * x * x); +} // template -static inline T Sign(const T& x) -{ return((T)(x<0?-1:+1)); } +static inline T Sign(const T& x) +{ + return ((T)(x < 0 ? -1 : +1)); +} // template -static inline bool SameSign(const T& x,const T& y) -{ return((x*y)>0); } -// -static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) +static inline bool SameSign(const T& x, const T& y) { - const btVector3 d=x-y; - return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); + return ((x * y) > 0); } // -static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) +static inline btScalar ClusterMetric(const btVector3& x, const btVector3& y) { - const btScalar xx=a.x()*a.x(); - const btScalar yy=a.y()*a.y(); - const btScalar zz=a.z()*a.z(); - const btScalar xy=a.x()*a.y(); - const btScalar yz=a.y()*a.z(); - const btScalar zx=a.z()*a.x(); - btMatrix3x3 m; - m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx); - m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz); - m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s); - return(m); + const btVector3 d = x - y; + return (btFabs(d[0]) + btFabs(d[1]) + btFabs(d[2])); } // -static inline btMatrix3x3 Cross(const btVector3& v) +static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a, btScalar s) { - btMatrix3x3 m; - m[0]=btVector3(0,-v.z(),+v.y()); - m[1]=btVector3(+v.z(),0,-v.x()); - m[2]=btVector3(-v.y(),+v.x(),0); - return(m); + const btScalar xx = a.x() * a.x(); + const btScalar yy = a.y() * a.y(); + const btScalar zz = a.z() * a.z(); + const btScalar xy = a.x() * a.y(); + const btScalar yz = a.y() * a.z(); + const btScalar zx = a.z() * a.x(); + btMatrix3x3 m; + m[0] = btVector3(1 - xx + xx * s, xy * s - xy, zx * s - zx); + m[1] = btVector3(xy * s - xy, 1 - yy + yy * s, yz * s - yz); + m[2] = btVector3(zx * s - zx, yz * s - yz, 1 - zz + zz * s); + return (m); } // -static inline btMatrix3x3 Diagonal(btScalar x) +static inline btMatrix3x3 Cross(const btVector3& v) { - btMatrix3x3 m; - m[0]=btVector3(x,0,0); - m[1]=btVector3(0,x,0); - m[2]=btVector3(0,0,x); - return(m); + btMatrix3x3 m; + m[0] = btVector3(0, -v.z(), +v.y()); + m[1] = btVector3(+v.z(), 0, -v.x()); + m[2] = btVector3(-v.y(), +v.x(), 0); + return (m); } // -static inline btMatrix3x3 Add(const btMatrix3x3& a, - const btMatrix3x3& b) +static inline btMatrix3x3 Diagonal(btScalar x) { - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]+b[i]; - return(r); + btMatrix3x3 m; + m[0] = btVector3(x, 0, 0); + m[1] = btVector3(0, x, 0); + m[2] = btVector3(0, 0, x); + return (m); } // -static inline btMatrix3x3 Sub(const btMatrix3x3& a, - const btMatrix3x3& b) +static inline btMatrix3x3 Add(const btMatrix3x3& a, + const btMatrix3x3& b) { - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]-b[i]; - return(r); + btMatrix3x3 r; + for (int i = 0; i < 3; ++i) r[i] = a[i] + b[i]; + return (r); } // -static inline btMatrix3x3 Mul(const btMatrix3x3& a, - btScalar b) +static inline btMatrix3x3 Sub(const btMatrix3x3& a, + const btMatrix3x3& b) { - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]*b; - return(r); + btMatrix3x3 r; + for (int i = 0; i < 3; ++i) r[i] = a[i] - b[i]; + return (r); } // -static inline void Orthogonalize(btMatrix3x3& m) +static inline btMatrix3x3 Mul(const btMatrix3x3& a, + btScalar b) { - m[2]=btCross(m[0],m[1]).normalized(); - m[1]=btCross(m[2],m[0]).normalized(); - m[0]=btCross(m[1],m[2]).normalized(); + btMatrix3x3 r; + for (int i = 0; i < 3; ++i) r[i] = a[i] * b; + return (r); } // -static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) +static inline void Orthogonalize(btMatrix3x3& m) { - const btMatrix3x3 cr=Cross(r); - return(Sub(Diagonal(im),cr*iwi*cr)); + m[2] = btCross(m[0], m[1]).normalized(); + m[1] = btCross(m[2], m[0]).normalized(); + m[0] = btCross(m[1], m[2]).normalized(); +} +// +static inline btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3& iwi, const btVector3& r) +{ + const btMatrix3x3 cr = Cross(r); + return (Sub(Diagonal(im), cr * iwi * cr)); } // -static inline btMatrix3x3 ImpulseMatrix( btScalar dt, - btScalar ima, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) +static inline btMatrix3x3 ImpulseMatrix(btScalar dt, + btScalar ima, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) { - return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); + return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse()); } // -static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, - btScalar imb,const btMatrix3x3& iib,const btVector3& rb) +static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra, + btScalar imb, const btMatrix3x3& iib, const btVector3& rb) { - return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); + return (Add(MassMatrix(ima, iia, ra), MassMatrix(imb, iib, rb)).inverse()); } // -static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, - const btMatrix3x3& iib) +static inline btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3& iia, + const btMatrix3x3& iib) { - return(Add(iia,iib).inverse()); + return (Add(iia, iib).inverse()); } // -static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) +static inline btVector3 ProjectOnAxis(const btVector3& v, + const btVector3& a) { - return(a*btDot(v,a)); + return (a * btDot(v, a)); } // -static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) +static inline btVector3 ProjectOnPlane(const btVector3& v, + const btVector3& a) { - return(v-ProjectOnAxis(v,a)); + return (v - ProjectOnAxis(v, a)); } // -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - btVector3& prj, - btScalar& sqd) +static inline void ProjectOrigin(const btVector3& a, + const btVector3& b, + btVector3& prj, + btScalar& sqd) { - const btVector3 d=b-a; - const btScalar m2=d.length2(); - if(m2>SIMD_EPSILON) - { - const btScalar t=Clamp(-btDot(a,d)/m2,0,1); - const btVector3 p=a+d*t; - const btScalar l2=p.length2(); - if(l2 SIMD_EPSILON) + { + const btScalar t = Clamp(-btDot(a, d) / m2, 0, 1); + const btVector3 p = a + d * t; + const btScalar l2 = p.length2(); + if (l2 < sqd) { - prj=p; - sqd=l2; + prj = p; + sqd = l2; } } } // -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btVector3& prj, - btScalar& sqd) +static inline void ProjectOrigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + btVector3& prj, + btScalar& sqd) { - const btVector3& q=btCross(b-a,c-a); - const btScalar m2=q.length2(); - if(m2>SIMD_EPSILON) + const btVector3& q = btCross(b - a, c - a); + const btScalar m2 = q.length2(); + if (m2 > SIMD_EPSILON) { - const btVector3 n=q/btSqrt(m2); - const btScalar k=btDot(a,n); - const btScalar k2=k*k; - if(k20)&& - (btDot(btCross(b-p,c-p),q)>0)&& - (btDot(btCross(c-p,a-p),q)>0)) - { - prj=p; - sqd=k2; + const btVector3 p = n * k; + if ((btDot(btCross(a - p, b - p), q) > 0) && + (btDot(btCross(b - p, c - p), q) > 0) && + (btDot(btCross(c - p, a - p), q) > 0)) + { + prj = p; + sqd = k2; } else { - ProjectOrigin(a,b,prj,sqd); - ProjectOrigin(b,c,prj,sqd); - ProjectOrigin(c,a,prj,sqd); + ProjectOrigin(a, b, prj, sqd); + ProjectOrigin(b, c, prj, sqd); + ProjectOrigin(c, a, prj, sqd); } } } @@ -401,155 +429,159 @@ static inline void ProjectOrigin( const btVector3& a, // template -static inline T BaryEval( const T& a, - const T& b, - const T& c, - const btVector3& coord) +static inline T BaryEval(const T& a, + const T& b, + const T& c, + const btVector3& coord) { - return(a*coord.x()+b*coord.y()+c*coord.z()); + return (a * coord.x() + b * coord.y() + c * coord.z()); } // -static inline btVector3 BaryCoord( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& p) +static inline btVector3 BaryCoord(const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& p) { - const btScalar w[]={ btCross(a-p,b-p).length(), - btCross(b-p,c-p).length(), - btCross(c-p,a-p).length()}; - const btScalar isum=1/(w[0]+w[1]+w[2]); - return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); + const btScalar w[] = {btCross(a - p, b - p).length(), + btCross(b - p, c - p).length(), + btCross(c - p, a - p).length()}; + const btScalar isum = 1 / (w[0] + w[1] + w[2]); + return (btVector3(w[1] * isum, w[2] * isum, w[0] * isum)); } // -inline static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, - const btVector3& a, - const btVector3& b, - const btScalar accuracy, - const int maxiterations=256) +inline static btScalar ImplicitSolve(btSoftBody::ImplicitFn* fn, + const btVector3& a, + const btVector3& b, + const btScalar accuracy, + const int maxiterations = 256) { - btScalar span[2]={0,1}; - btScalar values[2]={fn->Eval(a),fn->Eval(b)}; - if(values[0]>values[1]) + btScalar span[2] = {0, 1}; + btScalar values[2] = {fn->Eval(a), fn->Eval(b)}; + if (values[0] > values[1]) { - btSwap(span[0],span[1]); - btSwap(values[0],values[1]); + btSwap(span[0], span[1]); + btSwap(values[0], values[1]); } - if(values[0]>-accuracy) return(-1); - if(values[1]<+accuracy) return(-1); - for(int i=0;i -accuracy) return (-1); + if (values[1] < +accuracy) return (-1); + for (int i = 0; i < maxiterations; ++i) { - const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1])); - const btScalar v=fn->Eval(Lerp(a,b,t)); - if((t<=0)||(t>=1)) break; - if(btFabs(v)air_density; - if(wfi->water_density>0) - { - const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset); - if(depth>0) + const btScalar t = Lerp(span[0], span[1], values[0] / (values[0] - values[1])); + const btScalar v = fn->Eval(Lerp(a, b, t)); + if ((t <= 0) || (t >= 1)) break; + if (btFabs(v) < accuracy) return (t); + if (v < 0) { - medium.m_density = wfi->water_density; - medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length(); + span[0] = t; + values[0] = v; + } + else + { + span[1] = t; + values[1] = v; + } + } + 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) +static inline btVector3 NormalizeAny(const btVector3& v) { - const btScalar l=v.length(); - if(l>SIMD_EPSILON) - return(v/l); + const btScalar l = v.length(); + if (l > SIMD_EPSILON) + return (v / l); else - return(btVector3(0,0,0)); + return (btVector3(0, 0, 0)); } // -static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f, - btScalar margin) +static inline btDbvtVolume VolumeOf(const btSoftBody::Face& f, + btScalar margin) { - const btVector3* pts[]={ &f.m_n[0]->m_x, - &f.m_n[1]->m_x, - &f.m_n[2]->m_x}; - btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); - vol.Expand(btVector3(margin,margin,margin)); - return(vol); + const btVector3* pts[] = {&f.m_n[0]->m_x, + &f.m_n[1]->m_x, + &f.m_n[2]->m_x}; + btDbvtVolume vol = btDbvtVolume::FromPoints(pts, 3); + vol.Expand(btVector3(margin, margin, margin)); + return (vol); } // -static inline btVector3 CenterOf( const btSoftBody::Face& f) +static inline btVector3 CenterOf(const btSoftBody::Face& f) { - return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); + return ((f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3); } // -static inline btScalar AreaOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2) +static inline btScalar AreaOf(const btVector3& x0, + const btVector3& x1, + const btVector3& x2) { - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 cr=btCross(a,b); - const btScalar area=cr.length(); - return(area); + const btVector3 a = x1 - x0; + const btVector3 b = x2 - x0; + const btVector3 cr = btCross(a, b); + const btScalar area = cr.length(); + return (area); } // -static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) +static inline btScalar VolumeOf(const btVector3& x0, + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) { - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 c=x3-x0; - return(btDot(a,btCross(b,c))); + const btVector3 a = x1 - x0; + const btVector3 b = x2 - x0; + const btVector3 c = x3 - x0; + return (btDot(a, btCross(b, c))); } // - // -static inline void ApplyClampedForce( btSoftBody::Node& n, - const btVector3& f, - btScalar dt) +static inline void ApplyClampedForce(btSoftBody::Node& n, + const btVector3& f, + btScalar dt) { - const btScalar dtim=dt*n.m_im; - if((f*dtim).length2()>n.m_v.length2()) - {/* Clamp */ - n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim; + const btScalar dtim = dt * n.m_im; + if ((f * dtim).length2() > n.m_v.length2()) + { /* Clamp */ + n.m_f -= ProjectOnAxis(n.m_v, f.normalized()) / dtim; } else - {/* Apply */ - n.m_f+=f; + { /* Apply */ + n.m_f += f; } } // -static inline int MatchEdge( const btSoftBody::Node* a, - const btSoftBody::Node* b, - const btSoftBody::Node* ma, - const btSoftBody::Node* mb) +static inline int MatchEdge(const btSoftBody::Node* a, + const btSoftBody::Node* b, + const btSoftBody::Node* ma, + const btSoftBody::Node* mb) { - if((a==ma)&&(b==mb)) return(0); - if((a==mb)&&(b==ma)) return(1); - return(-1); + if ((a == ma) && (b == mb)) return (0); + if ((a == mb) && (b == ma)) return (1); + return (-1); } // @@ -557,58 +589,72 @@ static inline int MatchEdge( const btSoftBody::Node* a, // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html // outputs are NOT sorted. // -struct btEigen +struct btEigen { - static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) + static int system(btMatrix3x3& a, btMatrix3x3* vectors, btVector3* values = 0) { - static const int maxiterations=16; - static const btScalar accuracy=(btScalar)0.0001; - btMatrix3x3& v=*vectors; - int iterations=0; + static const int maxiterations = 16; + static const btScalar accuracy = (btScalar)0.0001; + btMatrix3x3& v = *vectors; + int iterations = 0; vectors->setIdentity(); - do { - int p=0,q=1; - if(btFabs(a[p][q])accuracy) - { - const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); - const btScalar z=btFabs(w); - const btScalar t=w/(z*(btSqrt(1+w*w)+z)); - if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ - { - const btScalar c=1/btSqrt(t*t+1); - const btScalar s=c*t; - mulPQ(a,c,s,p,q); - mulTPQ(a,c,s,p,q); - mulPQ(v,c,s,p,q); - } else break; - } else break; - } while((++iterations) accuracy) + { + const btScalar w = (a[q][q] - a[p][p]) / (2 * a[p][q]); + const btScalar z = btFabs(w); + const btScalar t = w / (z * (btSqrt(1 + w * w) + z)); + if (t == t) /* [WARNING] let hope that one does not get thrown aways by some compilers... */ + { + const btScalar c = 1 / btSqrt(t * t + 1); + const btScalar s = c * t; + mulPQ(a, c, s, p, q); + mulTPQ(a, c, s, p, q); + mulPQ(v, c, s, p, q); + } + else + break; + } + else + break; + } while ((++iterations) < maxiterations); + if (values) + { + *values = btVector3(a[0][0], a[1][1], a[2][2]); } - return(iterations); + return (iterations); } + private: - static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) + static inline void mulTPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q) { - const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]}, - {a[q][0],a[q][1],a[q][2]}}; + const btScalar m[2][3] = {{a[p][0], a[p][1], a[p][2]}, + {a[q][0], a[q][1], a[q][2]}}; int i; - for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i]; - for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i]; + for (i = 0; i < 3; ++i) a[p][i] = c * m[0][i] - s * m[1][i]; + for (i = 0; i < 3; ++i) a[q][i] = c * m[1][i] + s * m[0][i]; } - static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) + static inline void mulPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q) { - const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]}, - {a[0][q],a[1][q],a[2][q]}}; + const btScalar m[2][3] = {{a[0][p], a[1][p], a[2][p]}, + {a[0][q], a[1][q], a[2][q]}}; int i; - for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i]; - for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i]; + for (i = 0; i < 3; ++i) a[i][p] = c * m[0][i] - s * m[1][i]; + for (i = 0; i < 3; ++i) a[i][q] = c * m[1][i] + s * m[0][i]; } }; @@ -616,9 +662,9 @@ private: // Polar decomposition, // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986. // -static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s) +static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) { - static const btPolarDecomposition polar; + static const btPolarDecomposition polar; return polar.decompose(m, q, s); } @@ -630,282 +676,284 @@ struct btSoftColliders // // ClusterBase // - struct ClusterBase : btDbvt::ICollide + struct ClusterBase : btDbvt::ICollide { - btScalar erp; - btScalar idt; - btScalar m_margin; - btScalar friction; - btScalar threshold; + btScalar erp; + btScalar idt; + btScalar m_margin; + btScalar friction; + btScalar threshold; ClusterBase() { - erp =(btScalar)1; - idt =0; - m_margin =0; - friction =0; - threshold =(btScalar)0; + erp = (btScalar)1; + idt = 0; + m_margin = 0; + friction = 0; + threshold = (btScalar)0; } - bool SolveContact( const btGjkEpaSolver2::sResults& res, - btSoftBody::Body ba,const btSoftBody::Body bb, - btSoftBody::CJoint& joint) + bool SolveContact(const btGjkEpaSolver2::sResults& res, + btSoftBody::Body ba, const btSoftBody::Body bb, + btSoftBody::CJoint& joint) { - if(res.distancedata; - btSoftClusterCollisionShape cshape(cluster); - - const btConvexShape* rshape=(const btConvexShape*)m_colObjWrap->getCollisionShape(); + btSoftBody::Cluster* cluster = (btSoftBody::Cluster*)leaf->data; + btSoftClusterCollisionShape cshape(cluster); + + const btConvexShape* rshape = (const btConvexShape*)m_colObjWrap->getCollisionShape(); ///don't collide an anchored cluster with a static/kinematic object - if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor) + if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor) return; - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), - rshape,m_colObjWrap->getWorldTransform(), - btVector3(1,0,0),res)) + btGjkEpaSolver2::sResults res; + if (btGjkEpaSolver2::SignedDistance(&cshape, btTransform::getIdentity(), + rshape, m_colObjWrap->getWorldTransform(), + btVector3(1, 0, 0), res)) { - btSoftBody::CJoint joint; - if(SolveContact(res,cluster,m_colObjWrap->getCollisionObject(),joint))//prb,joint)) + btSoftBody::CJoint joint; + if (SolveContact(res, cluster, m_colObjWrap->getCollisionObject(), joint)) //prb,joint)) { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;psb->m_joints.push_back(pj); - if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject()) + btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint(); + *pj = joint; + psb->m_joints.push_back(pj); + if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject()) { - pj->m_erp *= psb->m_cfg.kSKHR_CL; - pj->m_split *= psb->m_cfg.kSK_SPLT_CL; + pj->m_erp *= psb->m_cfg.kSKHR_CL; + pj->m_split *= psb->m_cfg.kSK_SPLT_CL; } else { - pj->m_erp *= psb->m_cfg.kSRHR_CL; - pj->m_split *= psb->m_cfg.kSR_SPLT_CL; + pj->m_erp *= psb->m_cfg.kSRHR_CL; + pj->m_split *= psb->m_cfg.kSR_SPLT_CL; } } } } - void ProcessColObj(btSoftBody* ps,const btCollisionObjectWrapper* colObWrap) + void ProcessColObj(btSoftBody* ps, const btCollisionObjectWrapper* colObWrap) { - psb = ps; - m_colObjWrap = colObWrap; - idt = ps->m_sst.isdt; - m_margin = m_colObjWrap->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin(); + psb = ps; + m_colObjWrap = colObWrap; + idt = ps->m_sst.isdt; + m_margin = m_colObjWrap->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin(); ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful. - friction = btMin(psb->m_cfg.kDF,m_colObjWrap->getCollisionObject()->getFriction()); - btVector3 mins; - btVector3 maxs; + friction = btMin(psb->m_cfg.kDF, m_colObjWrap->getCollisionObject()->getFriction()); + btVector3 mins; + btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(),mins,maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(1,1,1)*m_margin); - ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this); - } + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(), mins, maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(1, 1, 1) * m_margin); + ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root, volume, *this); + } }; // // CollideCL_SS // - struct CollideCL_SS : ClusterBase + struct CollideCL_SS : ClusterBase { - btSoftBody* bodies[2]; - void Process(const btDbvtNode* la,const btDbvtNode* lb) + btSoftBody* bodies[2]; + void Process(const btDbvtNode* la, const btDbvtNode* lb) { - btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; - btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; + btSoftBody::Cluster* cla = (btSoftBody::Cluster*)la->data; + btSoftBody::Cluster* clb = (btSoftBody::Cluster*)lb->data; - - bool connected=false; - if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size())) + bool connected = false; + if ((bodies[0] == bodies[1]) && (bodies[0]->m_clusterConnectivity.size())) { - connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex]; + connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex + bodies[0]->m_clusters.size() * clb->m_clusterIndex]; } if (!connected) { - btSoftClusterCollisionShape csa(cla); - btSoftClusterCollisionShape csb(clb); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), - &csb,btTransform::getIdentity(), - cla->m_com-clb->m_com,res)) + btSoftClusterCollisionShape csa(cla); + btSoftClusterCollisionShape csb(clb); + btGjkEpaSolver2::sResults res; + if (btGjkEpaSolver2::SignedDistance(&csa, btTransform::getIdentity(), + &csb, btTransform::getIdentity(), + cla->m_com - clb->m_com, res)) { - btSoftBody::CJoint joint; - if(SolveContact(res,cla,clb,joint)) + btSoftBody::CJoint joint; + if (SolveContact(res, cla, clb, joint)) { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;bodies[0]->m_joints.push_back(pj); - pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); - pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; + btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint(); + *pj = joint; + bodies[0]->m_joints.push_back(pj); + pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL, bodies[1]->m_cfg.kSSHR_CL); + pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL + bodies[1]->m_cfg.kSS_SPLT_CL) / 2; } } - } else + } + else { - static int count=0; + static int count = 0; count++; //printf("count=%d\n",count); - } } - void ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb) + void ProcessSoftSoft(btSoftBody* psa, btSoftBody* psb) { - idt = psa->m_sst.isdt; + idt = psa->m_sst.isdt; //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; - m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin()); - friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); - bodies[0] = psa; - bodies[1] = psb; - psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); - } + m_margin = (psa->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin()); + friction = btMin(psa->m_cfg.kDF, psb->m_cfg.kDF); + bodies[0] = psa; + bodies[1] = psb; + psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this); + } }; // // CollideSDF_RS // - struct CollideSDF_RS : btDbvt::ICollide + struct CollideSDF_RS : btDbvt::ICollide { - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; + btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; DoNode(*node); } - void DoNode(btSoftBody::Node& n) const + void DoNode(btSoftBody::Node& n) const { - const btScalar m=n.m_im>0?dynmargin:stamargin; - btSoftBody::RContact c; + const btScalar m = n.m_im > 0 ? dynmargin : stamargin; + btSoftBody::RContact c; - if( (!n.m_battach)&& - psb->checkContact(m_colObj1Wrap,n.m_x,m,c.m_cti)) + if ((!n.m_battach) && + psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) { - const btScalar ima=n.m_im; - const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f; - const btScalar ms=ima+imb; - if(ms>0) + const btScalar ima = n.m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) { - const btTransform& wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0); - const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra=n.m_x-wtr.getOrigin(); - const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0); - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=vb-va; - const btScalar dn=btDot(vr,c.m_cti.m_normal); - const btVector3 fv=vr-c.m_cti.m_normal*dn; - const btScalar fc=psb->m_cfg.kDF*m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); - c.m_c1 = ra; - c.m_c2 = ima*psb->m_sst.sdt; - c.m_c3 = fv.length2()<(dn*fc*dn*fc)?0:1-fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, c.m_cti.m_normal); + const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_c2 = ima * psb->m_sst.sdt; + c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; psb->m_rcontacts.push_back(c); if (m_rigidBody) m_rigidBody->activate(); } } } - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObj1Wrap; - btRigidBody* m_rigidBody; - btScalar dynmargin; - btScalar stamargin; + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; }; // // CollideVF_SS // - struct CollideVF_SS : btDbvt::ICollide + struct CollideVF_SS : btDbvt::ICollide { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) { - btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; - btSoftBody::Face* face=(btSoftBody::Face*)lface->data; - btVector3 o=node->m_x; - btVector3 p; - btScalar d=SIMD_INFINITY; - ProjectOrigin( face->m_n[0]->m_x-o, - face->m_n[1]->m_x-o, - face->m_n[2]->m_x-o, - p,d); - const btScalar m=mrg+(o-node->m_q).length()*2; - if(d<(m*m)) + btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 o = node->m_x; + btVector3 p; + btScalar d = SIMD_INFINITY; + ProjectOrigin(face->m_n[0]->m_x - o, + face->m_n[1]->m_x - o, + face->m_n[2]->m_x - o, + p, d); + const btScalar m = mrg + (o - node->m_q).length() * 2; + if (d < (m * m)) { - const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; - const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); - const btScalar ma=node->m_im; - btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); - if( (n[0]->m_im<=0)|| - (n[1]->m_im<=0)|| - (n[2]->m_im<=0)) + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o); + const btScalar ma = node->m_im; + btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + if ((n[0]->m_im <= 0) || + (n[1]->m_im <= 0) || + (n[2]->m_im <= 0)) { - mb=0; + mb = 0; } - const btScalar ms=ma+mb; - if(ms>0) + const btScalar ms = ma + mb; + if (ms > 0) { - btSoftBody::SContact c; - c.m_normal = p/-btSqrt(d); - c.m_margin = m; - c.m_node = node; - c.m_face = face; - c.m_weights = w; - c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); - c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; - c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; + btSoftBody::SContact c; + c.m_normal = p / -btSqrt(d); + c.m_margin = m; + c.m_node = node; + c.m_face = face; + c.m_weights = w; + c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR; + c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR; psb[0]->m_scontacts.push_back(c); } - } + } } - btSoftBody* psb[2]; - btScalar mrg; + btSoftBody* psb[2]; + btScalar mrg; }; }; -#endif //_BT_SOFT_BODY_INTERNALS_H +#endif //_BT_SOFT_BODY_INTERNALS_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp index f5a67f6d8..3127369cc 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp @@ -23,27 +23,27 @@ subject to the following restrictions: #define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1 btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) -:btDefaultCollisionConfiguration(constructionInfo) + : btDefaultCollisionConfiguration(constructionInfo) { void* mem; - mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); - m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc), 16); + m_softSoftCreateFunc = new (mem) btSoftSoftCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); - m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc), 16); + m_softRigidConvexCreateFunc = new (mem) btSoftRigidCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); - m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - m_swappedSoftRigidConvexCreateFunc->m_swapped=true; + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc), 16); + m_swappedSoftRigidConvexCreateFunc = new (mem) btSoftRigidCollisionAlgorithm::CreateFunc; + m_swappedSoftRigidConvexCreateFunc->m_swapped = true; #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS - mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); - m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc), 16); + m_softRigidConcaveCreateFunc = new (mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); - m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; - m_swappedSoftRigidConcaveCreateFunc->m_swapped=true; + mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc), 16); + m_swappedSoftRigidConcaveCreateFunc = new (mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; + m_swappedSoftRigidConcaveCreateFunc->m_swapped = true; #endif //replace pool by a new one, with potential larger size @@ -53,82 +53,79 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura int curElemSize = m_collisionAlgorithmPool->getElementSize(); ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool - int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm); int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm); int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm); - int collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); - + int collisionAlgorithmMaxElementSize = btMax(maxSize0, maxSize1); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize2); + if (collisionAlgorithmMaxElementSize > curElemSize) { m_collisionAlgorithmPool->~btPoolAllocator(); btAlignedFree(m_collisionAlgorithmPool); - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); + void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16); + m_collisionAlgorithmPool = new (mem) btPoolAllocator(collisionAlgorithmMaxElementSize, constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); } } - } btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration() { m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softSoftCreateFunc); + btAlignedFree(m_softSoftCreateFunc); m_softRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softRigidConvexCreateFunc); + btAlignedFree(m_softRigidConvexCreateFunc); m_swappedSoftRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedSoftRigidConvexCreateFunc); + btAlignedFree(m_swappedSoftRigidConvexCreateFunc); #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS m_softRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softRigidConcaveCreateFunc); + btAlignedFree(m_softRigidConcaveCreateFunc); m_swappedSoftRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedSoftRigidConcaveCreateFunc); + btAlignedFree(m_swappedSoftRigidConcaveCreateFunc); #endif } ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation -btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) +btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) { - ///try to handle the softbody interactions first - if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE)) + if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE) && (proxyType1 == SOFTBODY_SHAPE_PROXYTYPE)) { - return m_softSoftCreateFunc; + return m_softSoftCreateFunc; } ///softbody versus convex - if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConvex(proxyType1)) + if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConvex(proxyType1)) { - return m_softRigidConvexCreateFunc; + return m_softRigidConvexCreateFunc; } ///convex versus soft body - if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) + if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE) { - return m_swappedSoftRigidConvexCreateFunc; + return m_swappedSoftRigidConvexCreateFunc; } #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS ///softbody versus convex - if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConcave(proxyType1)) + if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConcave(proxyType1)) { - return m_softRigidConcaveCreateFunc; + return m_softRigidConcaveCreateFunc; } ///convex versus soft body - if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) + if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE) { - return m_swappedSoftRigidConcaveCreateFunc; + return m_swappedSoftRigidConcaveCreateFunc; } #endif ///fallback to the regular rigid collision shape - return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1); + return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0, proxyType1); } diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h index 21addcfe2..0396a52da 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h @@ -21,28 +21,23 @@ subject to the following restrictions: class btVoronoiSimplexSolver; class btGjkEpaPenetrationDepthSolver; - ///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of btDefaultCollisionConfiguration -class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration +class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration { - //default CreationFunctions, filling the m_doubleDispatch table - btCollisionAlgorithmCreateFunc* m_softSoftCreateFunc; - btCollisionAlgorithmCreateFunc* m_softRigidConvexCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConvexCreateFunc; - btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_softSoftCreateFunc; + btCollisionAlgorithmCreateFunc* m_softRigidConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc; public: - btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); virtual ~btSoftBodyRigidBodyCollisionConfiguration(); ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); - + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1); }; -#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION - +#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h index c4733d640..bc538db4a 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H #define BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H - class btVertexBufferDescriptor { public: @@ -27,8 +26,7 @@ public: OPENGL_BUFFER }; -protected: - +protected: bool m_hasVertexPositions; bool m_hasNormals; @@ -51,7 +49,6 @@ public: virtual ~btVertexBufferDescriptor() { - } virtual bool hasVertexPositions() const @@ -102,7 +99,6 @@ public: } }; - class btCPUVertexBufferDescriptor : public btVertexBufferDescriptor { protected: @@ -114,7 +110,7 @@ public: * vertexOffset is the offset in floats to the first vertex. * vertexStride is the stride in floats between vertices. */ - btCPUVertexBufferDescriptor( float *basePointer, int vertexOffset, int vertexStride ) + btCPUVertexBufferDescriptor(float *basePointer, int vertexOffset, int vertexStride) { m_basePointer = basePointer; m_vertexOffset = vertexOffset; @@ -127,7 +123,7 @@ public: * vertexOffset is the offset in floats to the first vertex. * vertexStride is the stride in floats between vertices. */ - btCPUVertexBufferDescriptor( float *basePointer, int vertexOffset, int vertexStride, int normalOffset, int normalStride ) + btCPUVertexBufferDescriptor(float *basePointer, int vertexOffset, int vertexStride, int normalOffset, int normalStride) { m_basePointer = basePointer; @@ -142,7 +138,6 @@ public: virtual ~btCPUVertexBufferDescriptor() { - } /** @@ -162,4 +157,4 @@ public: } }; -#endif // #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H +#endif // #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodySolvers.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodySolvers.h index 6947bc27d..dcf508265 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftBodySolvers.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftBodySolvers.h @@ -18,7 +18,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" - class btSoftBodyTriangleData; class btSoftBodyLinkData; class btSoftBodyVertexData; @@ -26,7 +25,6 @@ class btVertexBufferDescriptor; class btCollisionObject; class btSoftBody; - class btSoftBodySolver { public: @@ -40,17 +38,15 @@ public: DX_SIMD_SOLVER }; - protected: int m_numberOfPositionIterations; int m_numberOfVelocityIterations; // Simulation timescale float m_timeScale; - + public: - btSoftBodySolver() : - m_numberOfPositionIterations( 10 ), - m_timeScale( 1 ) + btSoftBodySolver() : m_numberOfPositionIterations(10), + m_timeScale(1) { m_numberOfVelocityIterations = 0; m_numberOfPositionIterations = 5; @@ -59,39 +55,38 @@ public: virtual ~btSoftBodySolver() { } - + /** * Return the type of the solver. */ virtual SolverTypes getSolverType() const = 0; - /** Ensure that this solver is initialized. */ virtual bool checkInitialized() = 0; /** Optimize soft bodies in this solver. */ - virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies , bool forceUpdate=false) = 0; + virtual void optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false) = 0; /** Copy necessary data back to the original soft body source objects. */ virtual void copyBackToSoftBodies(bool bMove = true) = 0; /** Predict motion of soft bodies into next timestep */ - virtual void predictMotion( float solverdt ) = 0; + virtual void predictMotion(float solverdt) = 0; /** Solve constraints for a set of soft bodies */ - virtual void solveConstraints( float solverdt ) = 0; + virtual void solveConstraints(float solverdt) = 0; /** Perform necessary per-step updates of soft bodies such as recomputing normals and bounding boxes */ virtual void updateSoftBodies() = 0; /** Process a collision between one of the world's soft bodies and another collision object */ - virtual void processCollision( btSoftBody *, const struct btCollisionObjectWrapper* ) = 0; + virtual void processCollision(btSoftBody *, const struct btCollisionObjectWrapper *) = 0; /** Process a collision between two soft bodies */ - virtual void processCollision( btSoftBody*, btSoftBody* ) = 0; + virtual void processCollision(btSoftBody *, btSoftBody *) = 0; /** Set the number of velocity constraint solver iterations this solver uses. */ - virtual void setNumberOfPositionIterations( int iterations ) + virtual void setNumberOfPositionIterations(int iterations) { m_numberOfPositionIterations = iterations; } @@ -103,7 +98,7 @@ public: } /** Set the number of velocity constraint solver iterations this solver uses. */ - virtual void setNumberOfVelocityIterations( int iterations ) + virtual void setNumberOfVelocityIterations(int iterations) { m_numberOfVelocityIterations = iterations; } @@ -135,7 +130,6 @@ public: class btSoftBodySolverOutput { protected: - public: btSoftBodySolverOutput() { @@ -145,10 +139,8 @@ public: { } - /** Output current computed vertex data to the vertex buffers for all cloths in the solver. */ - virtual void copySoftBodyToVertexBuffer( const btSoftBody * const softBody, btVertexBufferDescriptor *vertexBuffer ) = 0; + virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) = 0; }; - -#endif // #ifndef BT_SOFT_BODY_SOLVERS_H +#endif // #ifndef BT_SOFT_BODY_SOLVERS_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/Engine/lib/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp index 4e76dca9d..282dbf75f 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSoftMultiBodyDynamicsWorld.h" #include "LinearMath/btQuickprof.h" @@ -24,42 +23,38 @@ subject to the following restrictions: #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) + btSoftBodySolver* softBodySolver) : btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), + m_softBodySolver(softBodySolver), + m_ownsSolver(false) { - if( !m_softBodySolver ) + if (!m_softBodySolver) { - void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); - m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); + 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_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.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() @@ -71,82 +66,78 @@ btSoftMultiBodyDynamicsWorld::~btSoftMultiBodyDynamicsWorld() } } -void btSoftMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +void btSoftMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); { BT_PROFILE("predictUnconstraintMotionSoftBody"); - m_softBodySolver->predictMotion( float(timeStep) ); + m_softBodySolver->predictMotion(float(timeStep)); } } -void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) +void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - // Let the solver grab the soft bodies and if necessary optimize for it - m_softBodySolver->optimize( getSoftBodyArray() ); + m_softBodySolver->optimize(getSoftBodyArray()); - if( !m_softBodySolver->checkInitialized() ) + if (!m_softBodySolver->checkInitialized()) { - btAssert( "Solver initialization failed\n" ); + btAssert("Solver initialization failed\n"); } - btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); + btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep); ///solve soft bodies constraints - solveSoftBodiesConstraints( timeStep ); + solveSoftBodiesConstraints(timeStep); //self collisions - for ( int i=0;idefaultCollisionHandler(psb); } ///update soft bodies - m_softBodySolver->updateSoftBodies( ); - + m_softBodySolver->updateSoftBodies(); + // End solver-wise simulation step // /////////////////////////////// - } -void btSoftMultiBodyDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) +void btSoftMultiBodyDynamicsWorld::solveSoftBodiesConstraints(btScalar timeStep) { BT_PROFILE("solveSoftConstraints"); - if(m_softBodies.size()) + if (m_softBodies.size()) { btSoftBody::solveClusters(m_softBodies); } // Solve constraints solver-wise - m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); - + m_softBodySolver->solveConstraints(timeStep * m_softBodySolver->getTimeScale()); } -void btSoftMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) +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 ); + body->setSoftBodySolver(m_softBodySolver); btCollisionWorld::addCollisionObject(body, - collisionFilterGroup, - collisionFilterMask); - + collisionFilterGroup, + collisionFilterMask); } -void btSoftMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) +void btSoftMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) { m_softBodies.remove(body); btCollisionWorld::removeCollisionObject(body); } -void btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { btSoftBody* body = btSoftBody::upcast(collisionObject); if (body) @@ -155,60 +146,57 @@ void btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* coll btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); } -void btSoftMultiBodyDynamicsWorld::debugDrawWorld() +void btSoftMultiBodyDynamicsWorld::debugDrawWorld() { - btDiscreteDynamicsWorld::debugDrawWorld(); + btMultiBodyDynamicsWorld::debugDrawWorld(); if (getDebugDrawer()) { int i; - for ( i=0;im_softBodies.size();i++) + for (i = 0; i < this->m_softBodies.size(); i++) { - btSoftBody* psb=(btSoftBody*)this->m_softBodies[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); + 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); + 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; + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; - const btSoftMultiBodyDynamicsWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; + 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) + 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); + btVector3 rayDir = (rayToWorld - rayFromWorld); - rayDir.normalize (); + 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]; @@ -217,22 +205,19 @@ struct btSoftSingleRayCallback : public btBroadphaseRayCallback 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); - + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); } - - - virtual bool process(const btBroadphaseProxy* proxy) + 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; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; @@ -250,118 +235,116 @@ struct btSoftSingleRayCallback : public btBroadphaseRayCallback //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); + 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 +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); + btSoftSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); #ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); #else - for (int i=0;igetNumCollisionObjects();i++) + for (int i = 0; i < this->getNumCollisionObjects(); i++) { rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE - + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE } - -void btSoftMultiBodyDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) +void btSoftMultiBodyDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) { - if (collisionShape->isSoftBody()) { + if (collisionShape->isSoftBody()) + { btSoftBody* softBody = btSoftBody::upcast(collisionObject); - if (softBody) { + if (softBody) + { btSoftBody::sRayCast softResult; - if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) { - - if (softResult.fraction<= resultCallback.m_closestHitFraction) + 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; + btVector3 normal = -rayDir; normal.normalize(); if (softResult.feature == btSoftBody::eFeature::Face) { normal = softBody->m_faces[softResult.index].m_normal; - if (normal.dot(rayDir) > 0) { + 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); + + btCollisionWorld::LocalRayResult rayResult(collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult, normalInWorldSpace); } } } - } - else { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); + } + else + { + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); } } - -void btSoftMultiBodyDynamicsWorld::serializeSoftBodies(btSerializer* serializer) +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); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); + serializer->finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, colObj); } } - } -void btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer) +void btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer) { - serializer->startSerialization(); - serializeDynamicsWorldInfo( serializer); + serializeDynamicsWorldInfo(serializer); serializeSoftBodies(serializer); + serializeMultiBodies(serializer); + serializeRigidBodies(serializer); serializeCollisionObjects(serializer); + serializeContactManifolds(serializer); + serializer->finishSerialization(); } - - diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h index 6d46a21db..f295945a6 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h @@ -21,64 +21,61 @@ subject to the following restrictions: #include "BulletSoftBody/btSoftBody.h" #ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H -typedef btAlignedObjectArray btSoftBodyArray; +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; + 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; + btSoftBodySolver* m_softBodySolver; + bool m_ownsSolver; protected: + virtual void predictUnconstraintMotion(btScalar timeStep); - virtual void predictUnconstraintMotion(btScalar timeStep); + virtual void internalSingleStepSimulation(btScalar timeStep); - virtual void internalSingleStepSimulation( btScalar timeStep); + void solveSoftBodiesConstraints(btScalar timeStep); - void solveSoftBodiesConstraints( btScalar timeStep ); - - void serializeSoftBodies(btSerializer* serializer); + void serializeSoftBodies(btSerializer* serializer); public: - - btSoftMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); + btSoftMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver* softBodySolver = 0); virtual ~btSoftMultiBodyDynamicsWorld(); - virtual void debugDrawWorld(); + virtual void debugDrawWorld(); - void addSoftBody(btSoftBody* body, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter); + void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); - void removeSoftBody(btSoftBody* body); + 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); + virtual void removeCollisionObject(btCollisionObject* collisionObject); - int getDrawFlags() const { return(m_drawFlags); } - void setDrawFlags(int f) { m_drawFlags=f; } + int getDrawFlags() const { return (m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags = f; } - btSoftBodyWorldInfo& getWorldInfo() + btSoftBodyWorldInfo& getWorldInfo() { return m_sbi; } - const btSoftBodyWorldInfo& getWorldInfo() const + const btSoftBodyWorldInfo& getWorldInfo() const { return m_sbi; } - virtual btDynamicsWorldType getWorldType() const + virtual btDynamicsWorldType getWorldType() const { - return BT_SOFT_MULTIBODY_DYNAMICS_WORLD; + return BT_SOFT_MULTIBODY_DYNAMICS_WORLD; } btSoftBodyArray& getSoftBodyArray() @@ -91,20 +88,18 @@ public: return m_softBodies; } - - virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; + 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); + 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 +#endif //BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index 01c148a2c..56d8083f2 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -27,18 +27,16 @@ subject to the following restrictions: //#include -btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* , bool isSwapped) -: btCollisionAlgorithm(ci), -//m_ownManifold(false), -//m_manifoldPtr(mf), -m_isSwapped(isSwapped) +btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper*, const btCollisionObjectWrapper*, bool isSwapped) + : btCollisionAlgorithm(ci), + //m_ownManifold(false), + //m_manifoldPtr(mf), + m_isSwapped(isSwapped) { } - btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() { - //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject); /*if (m_ownManifold) @@ -47,31 +45,27 @@ btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() m_dispatcher->releaseManifold(m_manifoldPtr); } */ - } - #include -void btSoftRigidCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSoftRigidCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)dispatchInfo; (void)resultOut; //printf("btSoftRigidCollisionAlgorithm\n"); -// const btCollisionObjectWrapper* softWrap = m_isSwapped?body1Wrap:body0Wrap; -// const btCollisionObjectWrapper* rigidWrap = m_isSwapped?body0Wrap:body1Wrap; - btSoftBody* softBody = m_isSwapped? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject(); - const btCollisionObjectWrapper* rigidCollisionObjectWrap = m_isSwapped? body0Wrap : body1Wrap; - - if (softBody->m_collisionDisabledObjects.findLinearSearch(rigidCollisionObjectWrap->getCollisionObject())==softBody->m_collisionDisabledObjects.size()) + // const btCollisionObjectWrapper* softWrap = m_isSwapped?body1Wrap:body0Wrap; + // const btCollisionObjectWrapper* rigidWrap = m_isSwapped?body0Wrap:body1Wrap; + btSoftBody* softBody = m_isSwapped ? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject(); + const btCollisionObjectWrapper* rigidCollisionObjectWrap = m_isSwapped ? body0Wrap : body1Wrap; + + if (softBody->m_collisionDisabledObjects.findLinearSearch(rigidCollisionObjectWrap->getCollisionObject()) == softBody->m_collisionDisabledObjects.size()) { softBody->getSoftBodySolver()->processCollision(softBody, rigidCollisionObjectWrap); } - - } -btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; @@ -81,6 +75,3 @@ btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* //not yet return btScalar(1.); } - - - diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h index 93fcc6065..9773af19a 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h @@ -35,41 +35,37 @@ class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm //btCollisionObject* m_rigidCollisionObject; ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean - bool m_isSwapped; + bool m_isSwapped; public: - - btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0,const btCollisionObjectWrapper* col1Wrap, bool isSwapped); + btSoftRigidCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0, const btCollisionObjectWrapper* col1Wrap, bool isSwapped); virtual ~btSoftRigidCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { //we don't add any manifolds } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftRigidCollisionAlgorithm)); if (!m_swapped) { - return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false); - } else + return new (mem) btSoftRigidCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false); + } + else { - return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true); + return new (mem) btSoftRigidCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true); } } }; - }; -#endif //BT_SOFT_RIGID_COLLISION_ALGORITHM_H - - +#endif //BT_SOFT_RIGID_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index 204b4f576..510b731fc 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSoftRigidDynamicsWorld.h" #include "LinearMath/btQuickprof.h" @@ -24,42 +23,38 @@ subject to the following restrictions: #include "btDefaultSoftBodySolver.h" #include "LinearMath/btSerializer.h" - btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld( btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, - btSoftBodySolver *softBodySolver ) : - btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_softBodySolver( softBodySolver ), - m_ownsSolver(false) + btSoftBodySolver* softBodySolver) : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), + m_softBodySolver(softBodySolver), + m_ownsSolver(false) { - if( !m_softBodySolver ) + if (!m_softBodySolver) { - void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); - m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); + 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_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.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(); - - } btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() @@ -71,82 +66,78 @@ btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() } } -void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); { BT_PROFILE("predictUnconstraintMotionSoftBody"); - m_softBodySolver->predictMotion( float(timeStep) ); + m_softBodySolver->predictMotion(float(timeStep)); } } -void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) +void btSoftRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - // Let the solver grab the soft bodies and if necessary optimize for it - m_softBodySolver->optimize( getSoftBodyArray() ); + m_softBodySolver->optimize(getSoftBodyArray()); - if( !m_softBodySolver->checkInitialized() ) + if (!m_softBodySolver->checkInitialized()) { - btAssert( "Solver initialization failed\n" ); + btAssert("Solver initialization failed\n"); } - btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); + btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep); ///solve soft bodies constraints - solveSoftBodiesConstraints( timeStep ); + solveSoftBodiesConstraints(timeStep); //self collisions - for ( int i=0;idefaultCollisionHandler(psb); } ///update soft bodies - m_softBodySolver->updateSoftBodies( ); - + m_softBodySolver->updateSoftBodies(); + // End solver-wise simulation step // /////////////////////////////// - } -void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) +void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints(btScalar timeStep) { BT_PROFILE("solveSoftConstraints"); - if(m_softBodies.size()) + if (m_softBodies.size()) { btSoftBody::solveClusters(m_softBodies); } // Solve constraints solver-wise - m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); - + m_softBodySolver->solveConstraints(timeStep * m_softBodySolver->getTimeScale()); } -void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) +void btSoftRigidDynamicsWorld::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 ); + body->setSoftBodySolver(m_softBodySolver); btCollisionWorld::addCollisionObject(body, - collisionFilterGroup, - collisionFilterMask); - + collisionFilterGroup, + collisionFilterMask); } -void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) +void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) { m_softBodies.remove(body); btCollisionWorld::removeCollisionObject(body); } -void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { btSoftBody* body = btSoftBody::upcast(collisionObject); if (body) @@ -155,60 +146,57 @@ void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisio btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); } -void btSoftRigidDynamicsWorld::debugDrawWorld() +void btSoftRigidDynamicsWorld::debugDrawWorld() { btDiscreteDynamicsWorld::debugDrawWorld(); if (getDebugDrawer()) { int i; - for ( i=0;im_softBodies.size();i++) + for (i = 0; i < this->m_softBodies.size(); i++) { - btSoftBody* psb=(btSoftBody*)this->m_softBodies[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); + 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); + 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; + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; - const btSoftRigidDynamicsWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; + const btSoftRigidDynamicsWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; - btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) + btSoftSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btSoftRigidDynamicsWorld* 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); + btVector3 rayDir = (rayToWorld - rayFromWorld); - rayDir.normalize (); + 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]; @@ -217,22 +205,19 @@ struct btSoftSingleRayCallback : public btBroadphaseRayCallback 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); - + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); } - - - virtual bool process(const btBroadphaseProxy* proxy) + 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; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; @@ -250,110 +235,106 @@ struct btSoftSingleRayCallback : public btBroadphaseRayCallback //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); + m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); } } return true; } }; -void btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const +void btSoftRigidDynamicsWorld::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); + btSoftSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); #ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); #else - for (int i=0;igetNumCollisionObjects();i++) + for (int i = 0; i < this->getNumCollisionObjects(); i++) { rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE - + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE } - -void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) +void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) { - if (collisionShape->isSoftBody()) { + if (collisionShape->isSoftBody()) + { btSoftBody* softBody = btSoftBody::upcast(collisionObject); - if (softBody) { + if (softBody) + { btSoftBody::sRayCast softResult; - if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) { - - if (softResult.fraction<= resultCallback.m_closestHitFraction) + 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; + btVector3 normal = -rayDir; normal.normalize(); if (softResult.feature == btSoftBody::eFeature::Face) { normal = softBody->m_faces[softResult.index].m_normal; - if (normal.dot(rayDir) > 0) { + 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); + + btCollisionWorld::LocalRayResult rayResult(collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult, normalInWorldSpace); } } } - } - else { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); + } + else + { + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); } } - -void btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer) +void btSoftRigidDynamicsWorld::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); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); + serializer->finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, colObj); } } - } -void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) +void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) { - serializer->startSerialization(); - serializeDynamicsWorldInfo( serializer); + serializeDynamicsWorldInfo(serializer); serializeSoftBodies(serializer); @@ -363,5 +344,3 @@ void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) serializer->finishSerialization(); } - - diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index d921a6488..be49c444d 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -19,63 +19,60 @@ subject to the following restrictions: #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" #include "btSoftBody.h" -typedef btAlignedObjectArray btSoftBodyArray; +typedef btAlignedObjectArray btSoftBodyArray; class btSoftBodySolver; class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld { - - btSoftBodyArray m_softBodies; - int m_drawFlags; - bool m_drawNodeTree; - bool m_drawFaceTree; - bool m_drawClusterTree; + 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; + btSoftBodySolver* m_softBodySolver; + bool m_ownsSolver; protected: + virtual void predictUnconstraintMotion(btScalar timeStep); - virtual void predictUnconstraintMotion(btScalar timeStep); + virtual void internalSingleStepSimulation(btScalar timeStep); - virtual void internalSingleStepSimulation( btScalar timeStep); + void solveSoftBodiesConstraints(btScalar timeStep); - void solveSoftBodiesConstraints( btScalar timeStep ); - - void serializeSoftBodies(btSerializer* serializer); + void serializeSoftBodies(btSerializer* serializer); public: - - btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); + btSoftRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver* softBodySolver = 0); virtual ~btSoftRigidDynamicsWorld(); - virtual void debugDrawWorld(); + virtual void debugDrawWorld(); - void addSoftBody(btSoftBody* body, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter); + void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); - void removeSoftBody(btSoftBody* body); + 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); + virtual void removeCollisionObject(btCollisionObject* collisionObject); - int getDrawFlags() const { return(m_drawFlags); } - void setDrawFlags(int f) { m_drawFlags=f; } + int getDrawFlags() const { return (m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags = f; } - btSoftBodyWorldInfo& getWorldInfo() + btSoftBodyWorldInfo& getWorldInfo() { return m_sbi; } - const btSoftBodyWorldInfo& getWorldInfo() const + const btSoftBodyWorldInfo& getWorldInfo() const { return m_sbi; } - virtual btDynamicsWorldType getWorldType() const + virtual btDynamicsWorldType getWorldType() const { - return BT_SOFT_RIGID_DYNAMICS_WORLD; + return BT_SOFT_RIGID_DYNAMICS_WORLD; } btSoftBodyArray& getSoftBodyArray() @@ -88,20 +85,18 @@ public: return m_softBodies; } - - virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; + 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); + 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_H +#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp b/Engine/lib/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp index 72043e69e..9c3e904f6 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp @@ -23,8 +23,8 @@ subject to the following restrictions: #define USE_PERSISTENT_CONTACTS 1 -btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* /*obj0*/,const btCollisionObjectWrapper* /*obj1*/) -: btCollisionAlgorithm(ci) +btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* /*obj0*/, const btCollisionObjectWrapper* /*obj1*/) + : btCollisionAlgorithm(ci) //m_ownManifold(false), //m_manifoldPtr(mf) { @@ -34,14 +34,14 @@ btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm() { } -void btSoftSoftCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +void btSoftSoftCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/) { - btSoftBody* soft0 = (btSoftBody*)body0Wrap->getCollisionObject(); - btSoftBody* soft1 = (btSoftBody*)body1Wrap->getCollisionObject(); + btSoftBody* soft0 = (btSoftBody*)body0Wrap->getCollisionObject(); + btSoftBody* soft1 = (btSoftBody*)body1Wrap->getCollisionObject(); soft0->getSoftBodySolver()->processCollision(soft0, soft1); } -btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/) { //not yet return 1.f; diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/Engine/lib/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h index 4eab7aea2..6f871f5b8 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h @@ -27,43 +27,39 @@ class btSoftBody; ///collision detection between two btSoftBody shapes class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - -// btSoftBody* m_softBody0; -// btSoftBody* m_softBody1; + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + // btSoftBody* m_softBody0; + // btSoftBody* m_softBody1; public: btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) manifoldArray.push_back(m_manifoldPtr); } - btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btSoftSoftCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btSoftSoftCollisionAlgorithm(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { int bbsize = sizeof(btSoftSoftCollisionAlgorithm); void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btSoftSoftCollisionAlgorithm(0,ci,body0Wrap,body1Wrap); + return new (ptr) btSoftSoftCollisionAlgorithm(0, ci, body0Wrap, body1Wrap); } }; - }; -#endif //BT_SOFT_SOFT_COLLISION_ALGORITHM_H - - +#endif //BT_SOFT_SOFT_COLLISION_ALGORITHM_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/btSparseSDF.h b/Engine/lib/bullet/src/BulletSoftBody/btSparseSDF.h index ba437c28e..a52b2cb1c 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/btSparseSDF.h +++ b/Engine/lib/bullet/src/BulletSoftBody/btSparseSDF.h @@ -24,296 +24,320 @@ subject to the following restrictions: template unsigned int HsiehHash(const void* pdata) { - const unsigned short* data=(const unsigned short*)pdata; - unsigned hash=DWORDLEN<<2,tmp; - for(int i=0;i>11; + hash += data[0]; + tmp = (data[1] << 11) ^ hash; + hash = (hash << 16) ^ tmp; + data += 2; + hash += hash >> 11; } - hash^=hash<<3;hash+=hash>>5; - hash^=hash<<4;hash+=hash>>17; - hash^=hash<<25;hash+=hash>>6; - return(hash); + hash ^= hash << 3; + hash += hash >> 5; + hash ^= hash << 4; + hash += hash >> 17; + hash ^= hash << 25; + hash += hash >> 6; + return (hash); } template -struct btSparseSdf +struct btSparseSdf { // // Inner types // struct IntFrac { - int b; - int i; - btScalar f; + int b; + int i; + btScalar f; }; - struct Cell + struct Cell { - btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; - int c[3]; - int puid; - unsigned hash; - const btCollisionShape* pclient; - Cell* next; + btScalar d[CELLSIZE + 1][CELLSIZE + 1][CELLSIZE + 1]; + int c[3]; + int puid; + unsigned hash; + const btCollisionShape* pclient; + Cell* next; }; // // Fields // - btAlignedObjectArray cells; - btScalar voxelsz; - int puid; - int ncells; - int m_clampCells; - int nprobes; - int nqueries; + btAlignedObjectArray cells; + btScalar voxelsz; + int puid; + int ncells; + int m_clampCells; + int nprobes; + int nqueries; // // Methods // // - void Initialize(int hashsize=2383, int clampCells = 256*1024) + void Initialize(int hashsize = 2383, int clampCells = 256 * 1024) { //avoid a crash due to running out of memory, so clamp the maximum number of cells allocated //if this limit is reached, the SDF is reset (at the cost of some performance during the reset) m_clampCells = clampCells; - cells.resize(hashsize,0); + cells.resize(hashsize, 0); Reset(); } // - void Reset() + void Reset() { - for(int i=0,ni=cells.size();inext; + Cell* pn = pc->next; delete pc; - pc=pn; + pc = pn; } } - voxelsz =0.25; - puid =0; - ncells =0; - nprobes =1; - nqueries =1; + voxelsz = 0.25; + puid = 0; + ncells = 0; + nprobes = 1; + nqueries = 1; } // - void GarbageCollect(int lifetime=256) + void GarbageCollect(int lifetime = 256) { - const int life=puid-lifetime; - for(int i=0;inext; - if(pc->puidnext; + if (pc->puid < life) { - if(pp) pp->next=pn; else root=pn; - delete pc;pc=pp;--ncells; + if (pp) + pp->next = pn; + else + root = pn; + delete pc; + pc = pp; + --ncells; } - pp=pc;pc=pn; + pp = pc; + pc = pn; } } //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); - nqueries=1; - nprobes=1; - ++puid; ///@todo: Reset puid's when int range limit is reached */ - /* else setup a priority list... */ + nqueries = 1; + nprobes = 1; + ++puid; ///@todo: Reset puid's when int range limit is reached */ + /* else setup a priority list... */ } // - int RemoveReferences(btCollisionShape* pcs) + int RemoveReferences(btCollisionShape* pcs) { - int refcount=0; - for(int i=0;inext; - if(pc->pclient==pcs) + Cell* pn = pc->next; + if (pc->pclient == pcs) { - if(pp) pp->next=pn; else root=pn; - delete pc;pc=pp;++refcount; + if (pp) + pp->next = pn; + else + root = pn; + delete pc; + pc = pp; + ++refcount; } - pp=pc;pc=pn; + pp = pc; + pc = pn; } } - return(refcount); + return (refcount); } // - btScalar Evaluate( const btVector3& x, - const btCollisionShape* shape, - btVector3& normal, - btScalar margin) + btScalar Evaluate(const btVector3& x, + const btCollisionShape* shape, + btVector3& normal, + btScalar margin) { - /* Lookup cell */ - const btVector3 scx=x/voxelsz; - const IntFrac ix=Decompose(scx.x()); - const IntFrac iy=Decompose(scx.y()); - const IntFrac iz=Decompose(scx.z()); - const unsigned h=Hash(ix.b,iy.b,iz.b,shape); - Cell*& root=cells[static_cast(h%cells.size())]; - Cell* c=root; + /* Lookup cell */ + const btVector3 scx = x / voxelsz; + const IntFrac ix = Decompose(scx.x()); + const IntFrac iy = Decompose(scx.y()); + const IntFrac iz = Decompose(scx.z()); + const unsigned h = Hash(ix.b, iy.b, iz.b, shape); + Cell*& root = cells[static_cast(h % cells.size())]; + Cell* c = root; ++nqueries; - while(c) + while (c) { ++nprobes; - if( (c->hash==h) && - (c->c[0]==ix.b) && - (c->c[1]==iy.b) && - (c->c[2]==iz.b) && - (c->pclient==shape)) - { break; } + if ((c->hash == h) && + (c->c[0] == ix.b) && + (c->c[1] == iy.b) && + (c->c[2] == iz.b) && + (c->pclient == shape)) + { + break; + } else - { c=c->next; } + { + c = c->next; + } } - if(!c) + if (!c) { - ++nprobes; + ++nprobes; ++ncells; //int sz = sizeof(Cell); - if (ncells>m_clampCells) + if (ncells > m_clampCells) { - static int numResets=0; + static int numResets = 0; numResets++; -// printf("numResets=%d\n",numResets); + // printf("numResets=%d\n",numResets); Reset(); } - c=new Cell(); - c->next=root;root=c; - c->pclient=shape; - c->hash=h; - c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; + c = new Cell(); + c->next = root; + root = c; + c->pclient = shape; + c->hash = h; + c->c[0] = ix.b; + c->c[1] = iy.b; + c->c[2] = iz.b; BuildCell(*c); } - c->puid=puid; - /* Extract infos */ - const int o[]={ ix.i,iy.i,iz.i}; - const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+1][o[2]+1], - c->d[o[0]+0][o[1]+1][o[2]+1]}; - /* Normal */ + c->puid = puid; + /* Extract infos */ + const int o[] = {ix.i, iy.i, iz.i}; + const btScalar d[] = {c->d[o[0] + 0][o[1] + 0][o[2] + 0], + c->d[o[0] + 1][o[1] + 0][o[2] + 0], + c->d[o[0] + 1][o[1] + 1][o[2] + 0], + c->d[o[0] + 0][o[1] + 1][o[2] + 0], + c->d[o[0] + 0][o[1] + 0][o[2] + 1], + c->d[o[0] + 1][o[1] + 0][o[2] + 1], + c->d[o[0] + 1][o[1] + 1][o[2] + 1], + c->d[o[0] + 0][o[1] + 1][o[2] + 1]}; + /* Normal */ #if 1 - const btScalar gx[]={ d[1]-d[0],d[2]-d[3], - d[5]-d[4],d[6]-d[7]}; - const btScalar gy[]={ d[3]-d[0],d[2]-d[1], - d[7]-d[4],d[6]-d[5]}; - const btScalar gz[]={ d[4]-d[0],d[5]-d[1], - d[7]-d[3],d[6]-d[2]}; - normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), - Lerp(gx[2],gx[3],iy.f),iz.f)); - normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), - Lerp(gy[2],gy[3],ix.f),iz.f)); - normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), - Lerp(gz[2],gz[3],ix.f),iy.f)); - normal = normal.normalized(); + const btScalar gx[] = {d[1] - d[0], d[2] - d[3], + d[5] - d[4], d[6] - d[7]}; + const btScalar gy[] = {d[3] - d[0], d[2] - d[1], + d[7] - d[4], d[6] - d[5]}; + const btScalar gz[] = {d[4] - d[0], d[5] - d[1], + d[7] - d[3], d[6] - d[2]}; + normal.setX(Lerp(Lerp(gx[0], gx[1], iy.f), + Lerp(gx[2], gx[3], iy.f), iz.f)); + normal.setY(Lerp(Lerp(gy[0], gy[1], ix.f), + Lerp(gy[2], gy[3], ix.f), iz.f)); + normal.setZ(Lerp(Lerp(gz[0], gz[1], ix.f), + Lerp(gz[2], gz[3], ix.f), iy.f)); + normal = normal.normalized(); #else - normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); + normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized(); #endif - /* Distance */ - const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), - Lerp(d[3],d[2],ix.f),iy.f); - const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), - Lerp(d[7],d[6],ix.f),iy.f); - return(Lerp(d0,d1,iz.f)-margin); + /* Distance */ + const btScalar d0 = Lerp(Lerp(d[0], d[1], ix.f), + Lerp(d[3], d[2], ix.f), iy.f); + const btScalar d1 = Lerp(Lerp(d[4], d[5], ix.f), + Lerp(d[7], d[6], ix.f), iy.f); + return (Lerp(d0, d1, iz.f) - margin); } // - void BuildCell(Cell& c) + void BuildCell(Cell& c) { - const btVector3 org=btVector3( (btScalar)c.c[0], - (btScalar)c.c[1], - (btScalar)c.c[2]) * - CELLSIZE*voxelsz; - for(int k=0;k<=CELLSIZE;++k) + const btVector3 org = btVector3((btScalar)c.c[0], + (btScalar)c.c[1], + (btScalar)c.c[2]) * + CELLSIZE * voxelsz; + for (int k = 0; k <= CELLSIZE; ++k) { - const btScalar z=voxelsz*k+org.z(); - for(int j=0;j<=CELLSIZE;++j) + const btScalar z = voxelsz * k + org.z(); + for (int j = 0; j <= CELLSIZE; ++j) { - const btScalar y=voxelsz*j+org.y(); - for(int i=0;i<=CELLSIZE;++i) + const btScalar y = voxelsz * j + org.y(); + for (int i = 0; i <= CELLSIZE; ++i) { - const btScalar x=voxelsz*i+org.x(); - c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), - c.pclient); + const btScalar x = voxelsz * i + org.x(); + c.d[i][j][k] = DistanceToShape(btVector3(x, y, z), + c.pclient); } } } } // - static inline btScalar DistanceToShape(const btVector3& x, - const btCollisionShape* shape) + static inline btScalar DistanceToShape(const btVector3& x, + const btCollisionShape* shape) { - btTransform unit; + btTransform unit; unit.setIdentity(); - if(shape->isConvex()) + if (shape->isConvex()) { - btGjkEpaSolver2::sResults res; - const btConvexShape* csh=static_cast(shape); - return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); + btGjkEpaSolver2::sResults res; + const btConvexShape* csh = static_cast(shape); + return (btGjkEpaSolver2::SignedDistance(x, 0, csh, unit, res)); } - return(0); + return (0); } // - static inline IntFrac Decompose(btScalar x) + static inline IntFrac Decompose(btScalar x) { /* That one need a lot of improvements... */ - /* Remove test, faster floor... */ - IntFrac r; - x/=CELLSIZE; - const int o=x<0?(int)(-x+1):0; - x+=o;r.b=(int)x; - const btScalar k=(x-r.b)*CELLSIZE; - r.i=(int)k;r.f=k-r.i;r.b-=o; - return(r); + /* Remove test, faster floor... */ + IntFrac r; + x /= CELLSIZE; + const int o = x < 0 ? (int)(-x + 1) : 0; + x += o; + r.b = (int)x; + const btScalar k = (x - r.b) * CELLSIZE; + r.i = (int)k; + r.f = k - r.i; + r.b -= o; + return (r); } // - static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) + static inline btScalar Lerp(btScalar a, btScalar b, btScalar t) { - return(a+(b-a)*t); + return (a + (b - a) * t); } - - // - static inline unsigned int Hash(int x,int y,int z,const btCollisionShape* shape) + static inline unsigned int Hash(int x, int y, int z, const btCollisionShape* shape) { struct btS - { - int x,y,z; + { + int x, y, z; void* p; }; btS myset; - myset.x=x;myset.y=y;myset.z=z;myset.p=(void*)shape; + myset.x = x; + myset.y = y; + myset.z = z; + myset.p = (void*)shape; const void* ptr = &myset; - unsigned int result = HsiehHash (ptr); - + unsigned int result = HsiehHash(ptr); return result; } }; - -#endif //BT_SPARSE_SDF_H +#endif //BT_SPARSE_SDF_H diff --git a/Engine/lib/bullet/src/BulletSoftBody/premake4.lua b/Engine/lib/bullet/src/BulletSoftBody/premake4.lua index ce384de2c..6f09196fa 100644 --- a/Engine/lib/bullet/src/BulletSoftBody/premake4.lua +++ b/Engine/lib/bullet/src/BulletSoftBody/premake4.lua @@ -5,6 +5,9 @@ includedirs { "..", } + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "**.cpp", "**.h" diff --git a/Engine/lib/bullet/src/LinearMath/CMakeLists.txt b/Engine/lib/bullet/src/LinearMath/CMakeLists.txt index ede21d9a7..0c8c0133a 100644 --- a/Engine/lib/bullet/src/LinearMath/CMakeLists.txt +++ b/Engine/lib/bullet/src/LinearMath/CMakeLists.txt @@ -14,6 +14,9 @@ SET(LinearMath_SRCS btSerializer64.cpp btThreads.cpp btVector3.cpp + TaskScheduler/btTaskScheduler.cpp + TaskScheduler/btThreadSupportPosix.cpp + TaskScheduler/btThreadSupportWin32.cpp ) SET(LinearMath_HDRS @@ -44,6 +47,7 @@ SET(LinearMath_HDRS btTransform.h btTransformUtil.h btVector3.h + TaskScheduler/btThreadSupportInterface.h ) ADD_LIBRARY(LinearMath ${LinearMath_SRCS} ${LinearMath_HDRS}) diff --git a/Engine/lib/bullet/src/LinearMath/TaskScheduler/btTaskScheduler.cpp b/Engine/lib/bullet/src/LinearMath/TaskScheduler/btTaskScheduler.cpp new file mode 100644 index 000000000..5f1115c40 --- /dev/null +++ b/Engine/lib/bullet/src/LinearMath/TaskScheduler/btTaskScheduler.cpp @@ -0,0 +1,792 @@ + +#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/Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportInterface.h b/Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportInterface.h new file mode 100644 index 000000000..1fe49335a --- /dev/null +++ b/Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportInterface.h @@ -0,0 +1,64 @@ +/* +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/Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp b/Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp new file mode 100644 index 000000000..a03f6dc57 --- /dev/null +++ b/Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp @@ -0,0 +1,353 @@ + +/* +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 btMax(1u, btMin(BT_MAX_THREAD_COUNT, std::thread::hardware_concurrency())); +} + +#else + +int btGetNumHardwareThreads() +{ + return btMax(1, btMin(BT_MAX_THREAD_COUNT, 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; + btCriticalSection* m_cs; + // 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(); + btCriticalSection* m_cs; +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) +{ + m_cs = createCriticalSection(); + startThreads(threadConstructionInfo); +} + +// cleanup/shutdown Libspe2 +btThreadSupportPosix::~btThreadSupportPosix() +{ + stopThreads(); + deleteCriticalSection(m_cs); + m_cs=0; +} + +#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_cs->lock(); + status->m_status = 2; + status->m_cs->unlock(); + checkPThreadFunction(sem_post(status->m_mainSemaphore)); + status->threadUsed++; + } + else + { + //exit Thread + status->m_cs->lock(); + status->m_status = 3; + status->m_cs->unlock(); + checkPThreadFunction(sem_post(status->m_mainSemaphore)); + break; + } + } + + return 0; +} + +///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_cs = m_cs; + 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) + { + m_cs->lock(); + bool hasFinished = (2 == m_activeThreadStatus[t].m_status); + m_cs->unlock(); + if (hasFinished) + { + 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 + m_activeThreadStatus.resize(m_numThreads); + m_startedThreadsMask = 0; + + m_mainSemaphore = createSem("main"); + //checkPThreadFunction(sem_wait(mainSemaphore)); + + for (int i = 0; i < m_numThreads; i++) + { + btThreadStatus& threadStatus = m_activeThreadStatus[i]; + threadStatus.startSemaphore = createSem("threadLocal"); + threadStatus.m_userPtr = 0; + threadStatus.m_cs = m_cs; + 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; + checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus)); + + } +} + +///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]; + + threadStatus.m_userPtr = 0; + checkPThreadFunction(sem_post(threadStatus.startSemaphore)); + checkPThreadFunction(sem_wait(m_mainSemaphore)); + + checkPThreadFunction(pthread_join(threadStatus.thread, 0)); + destroySem(threadStatus.startSemaphore); + } + destroySem(m_mainSemaphore); + 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/Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp b/Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp new file mode 100644 index 000000000..922e449cc --- /dev/null +++ b/Engine/lib/bullet/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp @@ -0,0 +1,452 @@ +/* +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/Engine/lib/bullet/src/LinearMath/btAabbUtil2.h b/Engine/lib/bullet/src/LinearMath/btAabbUtil2.h index d2997b4e6..eea49dd33 100644 --- a/Engine/lib/bullet/src/LinearMath/btAabbUtil2.h +++ b/Engine/lib/bullet/src/LinearMath/btAabbUtil2.h @@ -12,8 +12,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_AABB_UTIL2 #define BT_AABB_UTIL2 @@ -21,20 +19,18 @@ subject to the following restrictions: #include "btVector3.h" #include "btMinMax.h" - - -SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin, - btVector3& aabbMax, - const btVector3& expansionMin, - const btVector3& expansionMax) +SIMD_FORCE_INLINE void AabbExpand(btVector3& aabbMin, + btVector3& aabbMax, + const btVector3& expansionMin, + const btVector3& expansionMax) { aabbMin = aabbMin + expansionMin; aabbMax = aabbMax + expansionMax; } /// conservative test for overlap between two aabbs -SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, - const btVector3 &point) +SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3& aabbMin1, const btVector3& aabbMax1, + const btVector3& point) { bool overlap = true; overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap; @@ -43,10 +39,9 @@ SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const bt return overlap; } - /// conservative test for overlap between two aabbs -SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, - const btVector3 &aabbMin2, const btVector3 &aabbMax2) +SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3& aabbMin1, const btVector3& aabbMax1, + const btVector3& aabbMin2, const btVector3& aabbMax2) { bool overlap = true; overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap; @@ -56,37 +51,34 @@ SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btV } /// conservative test for overlap between triangle and aabb -SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices, - const btVector3 &aabbMin, const btVector3 &aabbMax) +SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3* vertices, + const btVector3& aabbMin, const btVector3& aabbMax) { - const btVector3 &p1 = vertices[0]; - const btVector3 &p2 = vertices[1]; - const btVector3 &p3 = vertices[2]; + const btVector3& p1 = vertices[0]; + const btVector3& p2 = vertices[1]; + const btVector3& p3 = vertices[2]; if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false; if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false; if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false; if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false; - + if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false; if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false; return true; } - -SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent) +SIMD_FORCE_INLINE int btOutcode(const btVector3& p, const btVector3& 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); + 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); } - - SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, const btVector3& rayInvDirection, const unsigned int raySign[3], @@ -97,11 +89,11 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, { btScalar tmax, tymin, tymax, tzmin, tzmax; tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX(); - tmax = (bounds[1-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(); + tymax = (bounds[1 - raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY(); - if ( (tmin > tymax) || (tymin > tmax) ) + if ((tmin > tymax) || (tymin > tmax)) return false; if (tymin > tmin) @@ -111,59 +103,59 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, tmax = tymax; tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); - tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); + tzmax = (bounds[1 - raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); - if ( (tmin > tzmax) || (tzmin > tmax) ) + if ((tmin > tzmax) || (tzmin > tmax)) return false; if (tzmin > tmin) tmin = tzmin; if (tzmax < tmax) tmax = tzmax; - return ( (tmin < lambda_max) && (tmax > lambda_min) ); + return ((tmin < lambda_max) && (tmax > lambda_min)); } -SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& aabbMin, +SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& aabbMin, const btVector3& aabbMax, - btScalar& param, btVector3& normal) + btScalar& param, btVector3& normal) { - btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5); - btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5); - btVector3 source = rayFrom - aabbCenter; - btVector3 target = rayTo - aabbCenter; - int sourceOutcode = btOutcode(source,aabbHalfExtent); - int targetOutcode = btOutcode(target,aabbHalfExtent); + btVector3 aabbHalfExtent = (aabbMax - aabbMin) * btScalar(0.5); + btVector3 aabbCenter = (aabbMax + aabbMin) * btScalar(0.5); + btVector3 source = rayFrom - aabbCenter; + btVector3 target = rayTo - aabbCenter; + int sourceOutcode = btOutcode(source, aabbHalfExtent); + int targetOutcode = btOutcode(target, aabbHalfExtent); if ((sourceOutcode & targetOutcode) == 0x0) { btScalar lambda_enter = btScalar(0.0); - btScalar lambda_exit = param; + btScalar lambda_exit = param; btVector3 r = target - source; int i; - btScalar normSign = 1; - btVector3 hitNormal(0,0,0); - int bit=1; + btScalar normSign = 1; + btVector3 hitNormal(0, 0, 0); + int bit = 1; - for (int j=0;j<2;j++) + for (int j = 0; j < 2; j++) { for (i = 0; i != 3; ++i) { if (sourceOutcode & bit) { - btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i]; + btScalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i]; if (lambda_enter <= lambda) { lambda_enter = lambda; - hitNormal.setValue(0,0,0); + hitNormal.setValue(0, 0, 0); hitNormal[i] = normSign; } } - else if (targetOutcode & bit) + else if (targetOutcode & bit) { - btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i]; + btScalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i]; btSetMin(lambda_exit, lambda); } - bit<<=1; + bit <<= 1; } normSign = btScalar(-1.); } @@ -177,56 +169,49 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, return false; } - - -SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut) +SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin, const btTransform& t, btVector3& aabbMinOut, btVector3& aabbMaxOut) { - btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin); - btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 halfExtentsWithMargin = halfExtents + btVector3(margin, margin, margin); + btMatrix3x3 abs_b = t.getBasis().absolute(); btVector3 center = t.getOrigin(); - btVector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] ); + btVector3 extent = halfExtentsWithMargin.dot3(abs_b[0], abs_b[1], abs_b[2]); aabbMinOut = center - extent; aabbMaxOut = center + extent; } - -SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut) +SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin, const btVector3& localAabbMax, btScalar margin, const btTransform& trans, btVector3& aabbMinOut, btVector3& aabbMaxOut) { - btAssert(localAabbMin.getX() <= localAabbMax.getX()); - btAssert(localAabbMin.getY() <= localAabbMax.getY()); - btAssert(localAabbMin.getZ() <= localAabbMax.getZ()); - btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); - localHalfExtents+=btVector3(margin,margin,margin); + btAssert(localAabbMin.getX() <= localAabbMax.getX()); + btAssert(localAabbMin.getY() <= localAabbMax.getY()); + btAssert(localAabbMin.getZ() <= localAabbMax.getZ()); + btVector3 localHalfExtents = btScalar(0.5) * (localAabbMax - localAabbMin); + localHalfExtents += btVector3(margin, margin, margin); - btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); - btMatrix3x3 abs_b = trans.getBasis().absolute(); - btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] ); - aabbMinOut = center-extent; - aabbMaxOut = center+extent; + btVector3 localCenter = btScalar(0.5) * (localAabbMax + localAabbMin); + btMatrix3x3 abs_b = trans.getBasis().absolute(); + btVector3 center = trans(localCenter); + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + aabbMinOut = center - extent; + aabbMaxOut = center + extent; } #define USE_BANCHLESS 1 #ifdef USE_BANCHLESS - //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) - SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) - { - return static_cast(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) - & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) - & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), - 1, 0)); - } +//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) +SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2) +{ + return static_cast(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), + 1, 0)); +} #else - SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) - { - bool overlap = true; - overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; - return overlap; - } -#endif //USE_BANCHLESS - -#endif //BT_AABB_UTIL2 - +SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2) +{ + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; +} +#endif //USE_BANCHLESS +#endif //BT_AABB_UTIL2 diff --git a/Engine/lib/bullet/src/LinearMath/btAlignedAllocator.cpp b/Engine/lib/bullet/src/LinearMath/btAlignedAllocator.cpp index e5f6040c4..39b302b60 100644 --- a/Engine/lib/bullet/src/LinearMath/btAlignedAllocator.cpp +++ b/Engine/lib/bullet/src/LinearMath/btAlignedAllocator.cpp @@ -15,9 +15,11 @@ subject to the following restrictions: #include "btAlignedAllocator.h" +#ifdef BT_DEBUG_MEMORY_ALLOCATIONS int gNumAlignedAllocs = 0; int gNumAlignedFree = 0; -int gTotalBytesAlignedAllocs = 0;//detect memory leaks +int gTotalBytesAlignedAllocs = 0; //detect memory leaks +#endif //BT_DEBUG_MEMORY_ALLOCATIONST_DEBUG_ALLOCATIONS static void *btAllocDefault(size_t size) { @@ -32,9 +34,7 @@ static void btFreeDefault(void *ptr) static btAllocFunc *sAllocFunc = btAllocDefault; static btFreeFunc *sFreeFunc = btFreeDefault; - - -#if defined (BT_HAS_ALIGNED_ALLOCATOR) +#if defined(BT_HAS_ALIGNED_ALLOCATOR) #include static void *btAlignedAllocDefault(size_t size, int alignment) { @@ -59,49 +59,48 @@ static inline void btAlignedFreeDefault(void *ptr) } #else - - - - static inline void *btAlignedAllocDefault(size_t size, int alignment) { - void *ret; - char *real; - real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1)); - if (real) { - ret = btAlignPointer(real + sizeof(void *),alignment); - *((void **)(ret)-1) = (void *)(real); - } else { - ret = (void *)(real); - } - return (ret); + void *ret; + char *real; + real = (char *)sAllocFunc(size + sizeof(void *) + (alignment - 1)); + if (real) + { + ret = btAlignPointer(real + sizeof(void *), alignment); + *((void **)(ret)-1) = (void *)(real); + } + else + { + ret = (void *)(real); + } + return (ret); } static inline void btAlignedFreeDefault(void *ptr) { - void* real; + void *real; - if (ptr) { - real = *((void **)(ptr)-1); - sFreeFunc(real); - } + if (ptr) + { + real = *((void **)(ptr)-1); + sFreeFunc(real); + } } #endif - static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault; static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault; void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc) { - sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault; - sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault; + sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault; + sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault; } void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc) { - sAllocFunc = allocFunc ? allocFunc : btAllocDefault; - sFreeFunc = freeFunc ? freeFunc : btFreeDefault; + sAllocFunc = allocFunc ? allocFunc : btAllocDefault; + sFreeFunc = freeFunc ? freeFunc : btFreeDefault; } #ifdef BT_DEBUG_MEMORY_ALLOCATIONS @@ -114,15 +113,15 @@ static int mynumallocs = 0; int btDumpMemoryLeaks() { int totalLeak = 0; - - for (int i=0;i1024*1024) -// { -// printf("big alloc!%d\n", size); -// } + void *ret; + char *real; - gTotalBytesAlignedAllocs += size; - gNumAlignedAllocs++; + // 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); + // } - -int sz4prt = 4*sizeof(void *); - - real = (char *)sAllocFunc(size + sz4prt + (alignment-1)); - if (real) { - - 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 (total allocated = %d)\n",allocId,real, filename,line,size,gTotalBytesAlignedAllocs); - allocId++; - - int* ptr = (int*)ret; - *ptr = 12; - return (ret); -} - -void btAlignedFreeInternal (void* ptr,int line,char* filename) -{ - - void* real; - - if (ptr) { - 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;i -class btAlignedAllocator { - - typedef btAlignedAllocator< T , Alignment > self_type; - -public: +template +class btAlignedAllocator +{ + typedef btAlignedAllocator self_type; +public: //just going down a list: btAlignedAllocator() {} /* btAlignedAllocator( const self_type & ) {} */ - template < typename Other > - btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {} + template + btAlignedAllocator(const btAlignedAllocator&) + { + } - typedef const T* const_pointer; - typedef const T& const_reference; - typedef T* pointer; - typedef T& reference; - typedef T value_type; + 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 ( size_type n , const_pointer * hint = 0 ) { + pointer address(reference ref) const { return &ref; } + const_pointer address(const_reference ref) const { return &ref; } + pointer allocate(size_type n, const_pointer* hint = 0) + { (void)hint; - return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment )); + return reinterpret_cast(btAlignedAlloc(sizeof(value_type) * n, Alignment)); } - void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); } - void deallocate( pointer ptr ) { - btAlignedFree( reinterpret_cast< void * >( ptr ) ); + void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); } + void deallocate(pointer ptr) + { + btAlignedFree(reinterpret_cast(ptr)); } - void destroy ( pointer ptr ) { ptr->~value_type(); } - + void destroy(pointer ptr) { ptr->~value_type(); } - template < typename O > struct rebind { - typedef btAlignedAllocator< O , Alignment > other; + template + struct rebind + { + typedef btAlignedAllocator other; }; - template < typename O > - self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; } + template + self_type& operator=(const btAlignedAllocator&) + { + return *this; + } - friend bool operator==( const self_type & , const self_type & ) { return true; } + friend bool operator==(const self_type&, const self_type&) { return true; } }; - - -#endif //BT_ALIGNED_ALLOCATOR - +#endif //BT_ALIGNED_ALLOCATOR diff --git a/Engine/lib/bullet/src/LinearMath/btAlignedObjectArray.h b/Engine/lib/bullet/src/LinearMath/btAlignedObjectArray.h index f0b646529..b3d5d64b5 100644 --- a/Engine/lib/bullet/src/LinearMath/btAlignedObjectArray.h +++ b/Engine/lib/bullet/src/LinearMath/btAlignedObjectArray.h @@ -13,11 +13,10 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_OBJECT_ARRAY__ #define BT_OBJECT_ARRAY__ -#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE +#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE #include "btAlignedAllocator.h" ///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW @@ -28,394 +27,371 @@ subject to the following restrictions: #define BT_USE_PLACEMENT_NEW 1 //#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in or or otherwise... -#define BT_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful +#define BT_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful #ifdef BT_USE_MEMCPY #include #include -#endif //BT_USE_MEMCPY +#endif //BT_USE_MEMCPY #ifdef BT_USE_PLACEMENT_NEW -#include //for placement new -#endif //BT_USE_PLACEMENT_NEW - -// The register keyword is deprecated in C++11 so don't use it. -#if __cplusplus > 199711L -#define BT_REGISTER -#else -#define BT_REGISTER register -#endif +#include //for placement new +#endif //BT_USE_PLACEMENT_NEW ///The btAlignedObjectArray 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 +template +//template class btAlignedObjectArray { - btAlignedAllocator m_allocator; + btAlignedAllocator m_allocator; - int m_size; - int m_capacity; - T* m_data; + int m_size; + int m_capacity; + T* m_data; //PCK: added this line - bool m_ownsMemory; + bool m_ownsMemory; #ifdef BT_ALLOW_ARRAY_COPY_OPERATOR public: - SIMD_FORCE_INLINE btAlignedObjectArray& operator=(const btAlignedObjectArray &other) + SIMD_FORCE_INLINE btAlignedObjectArray& operator=(const btAlignedObjectArray& other) { copyFromArray(other); return *this; } -#else//BT_ALLOW_ARRAY_COPY_OPERATOR +#else //BT_ALLOW_ARRAY_COPY_OPERATOR private: - SIMD_FORCE_INLINE btAlignedObjectArray& operator=(const btAlignedObjectArray &other); -#endif//BT_ALLOW_ARRAY_COPY_OPERATOR + SIMD_FORCE_INLINE btAlignedObjectArray& operator=(const btAlignedObjectArray& other); +#endif //BT_ALLOW_ARRAY_COPY_OPERATOR protected: - SIMD_FORCE_INLINE int allocSize(int size) - { - return (size ? size*2 : 1); - } - SIMD_FORCE_INLINE void copy(int start,int end, T* dest) const - { - int i; - for (i=start;i= 0); + btAssert(n < size()); + return m_data[n]; + } + + SIMD_FORCE_INLINE T& at(int n) + { + btAssert(n >= 0); + btAssert(n < size()); + return m_data[n]; + } + + SIMD_FORCE_INLINE const T& operator[](int n) const + { + btAssert(n >= 0); + btAssert(n < size()); + return m_data[n]; + } + + SIMD_FORCE_INLINE T& operator[](int n) + { + btAssert(n >= 0); + btAssert(n < size()); + return m_data[n]; + } + + ///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations. + SIMD_FORCE_INLINE void clear() + { + destroy(0, size()); + + deallocate(); + + init(); + } + + SIMD_FORCE_INLINE void pop_back() + { + btAssert(m_size > 0); + 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. + SIMD_FORCE_INLINE void resizeNoInitialize(int newsize) + { + if (newsize > size()) + { + reserve(newsize); + } + m_size = newsize; + } + + SIMD_FORCE_INLINE void resize(int newsize, const T& fillData = T()) + { + const int curSize = size(); + + if (newsize < curSize) + { + for (int i = newsize; i < curSize; i++) { m_data[i].~T(); } } - - SIMD_FORCE_INLINE void* allocate(int size) + else { - if (size) - return m_allocator.allocate(size); - return 0; - } - - SIMD_FORCE_INLINE void deallocate() - { - if(m_data) { - //PCK: enclosed the deallocation in this block - if (m_ownsMemory) - { - m_allocator.deallocate(m_data); - } - m_data = 0; - } - } - - - - - public: - - btAlignedObjectArray() - { - init(); - } - - ~btAlignedObjectArray() - { - clear(); - } - - ///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead. - btAlignedObjectArray(const btAlignedObjectArray& otherArray) - { - init(); - - int otherSize = otherArray.size(); - resize (otherSize); - otherArray.copy(0, otherSize, m_data); - } - - - - /// return the number of elements in the array - SIMD_FORCE_INLINE int size() const - { - return m_size; - } - - SIMD_FORCE_INLINE const T& at(int n) const - { - btAssert(n>=0); - btAssert(n=0); - btAssert(n=0); - btAssert(n=0); - btAssert(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. - SIMD_FORCE_INLINE void resizeNoInitialize(int newsize) - { - if (newsize > size()) + if (newsize > curSize) { reserve(newsize); } - m_size = newsize; +#ifdef BT_USE_PLACEMENT_NEW + for (int i = curSize; i < newsize; i++) + { + new (&m_data[i]) T(fillData); + } +#endif //BT_USE_PLACEMENT_NEW } - - SIMD_FORCE_INLINE void resize(int newsize, const T& fillData=T()) + + m_size = newsize; + } + SIMD_FORCE_INLINE T& expandNonInitializing() + { + const int sz = size(); + if (sz == capacity()) { - const BT_REGISTER int curSize = size(); - - if (newsize < curSize) - { - for(int i = newsize; i < curSize; i++) - { - m_data[i].~T(); - } - } else - { - if (newsize > curSize) - { - reserve(newsize); - } -#ifdef BT_USE_PLACEMENT_NEW - for (int i=curSize;i - void quickSortInternal(const L& CompareFunc,int lo, int hi) - { + template + 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]; + 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) + // partition + do { - //don't sort 0 or 1 elements - if (size()>1) + while (CompareFunc(m_data[i], x)) + i++; + while (CompareFunc(x, m_data[j])) + j--; + if (i <= j) { - quickSortInternal(CompareFunc,0,size()-1); + swap(i, j); + i++; + j--; + } + } while (i <= j); + + // recursion + if (lo < j) + quickSortInternal(CompareFunc, lo, j); + if (i < hi) + quickSortInternal(CompareFunc, i, hi); + } + + template + 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*/ - - ///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) - { + void swap(int index0, int index1) + { #ifdef BT_USE_MEMCPY - char temp[sizeof(T)]; - memcpy(temp,&m_data[index0],sizeof(T)); - memcpy(&m_data[index0],&m_data[index1],sizeof(T)); - memcpy(&m_data[index1],temp,sizeof(T)); + char temp[sizeof(T)]; + memcpy(temp, &m_data[index0], sizeof(T)); + memcpy(&m_data[index0], &m_data[index1], sizeof(T)); + memcpy(&m_data[index1], temp, sizeof(T)); #else - T temp = m_data[index0]; - m_data[index0] = m_data[index1]; - m_data[index1] = temp; -#endif //BT_USE_PLACEMENT_NEW - - } + T temp = m_data[index0]; + m_data[index0] = m_data[index1]; + m_data[index1] = temp; +#endif //BT_USE_PLACEMENT_NEW + } template void heapSort(const L& CompareFunc) @@ -423,49 +399,48 @@ protected: /* sort a[0..N-1], N.B. 0 to N-1 */ int k; int n = m_size; - for (k = n/2; k > 0; k--) + for (k = n / 2; k > 0; k--) { downHeap(m_data, k, n, CompareFunc); } /* a[1..N] is now a heap */ - while ( n>=1 ) + while (n >= 1) { - swap(0,n-1); /* largest of a[0..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 findBinarySearch(const T& key) const { int first = 0; - int last = size()-1; + int last = size() - 1; //assume sorted array - while (first <= last) { + while (first <= last) + { int mid = (first + last) / 2; // compute mid point. - if (key > m_data[mid]) + 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 if (key < m_data[mid]) + last = mid - 1; // repeat search in bottom half. else - return mid; // found it. return position ///// + return mid; // found it. return position ///// } - return size(); // failed to find key + return size(); // failed to find key } - - int findLinearSearch(const T& key) const + int findLinearSearch(const T& key) const { - int index=size(); + int index = size(); int i; - for (i=0;i btScalar(0.000001)); - + quotient = btScalar(-1.) / quotient; n2n3 *= p0.dist; n3n1 *= p1.dist; @@ -74,105 +75,96 @@ btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btP potentialVertex += n1n2; potentialVertex *= quotient; - btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); + btVector3 result(potentialVertex.getX(), potentialVertex.getY(), potentialVertex.getZ()); return result; - } -btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL); -btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2); -btVector3 NormalOf(const btVector3 *vert, const int n); - +btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint = NULL, btVector3 *vpoint = NULL); +btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2); +btVector3 NormalOf(const btVector3 *vert, const int n); btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1) { // returns the point where the line p0-p1 intersects the plane n&d - btVector3 dif; - dif = p1-p0; - btScalar dn= btDot(plane.normal,dif); - btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn; - return p0 + (dif*t); + btVector3 dif; + dif = p1 - p0; + btScalar dn = btDot(plane.normal, dif); + btScalar t = -(plane.dist + btDot(plane.normal, p0)) / dn; + return p0 + (dif * t); } btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) { - return point - plane.normal * (btDot(point,plane.normal)+plane.dist); + return point - plane.normal * (btDot(point, plane.normal) + plane.dist); } btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) { // return the normal of the triangle // inscribed by v0, v1, and v2 - btVector3 cp=btCross(v1-v0,v2-v1); - btScalar m=cp.length(); - if(m==0) return btVector3(1,0,0); - return cp*(btScalar(1.0)/m); + btVector3 cp = btCross(v1 - v0, v2 - v1); + btScalar m = cp.length(); + if (m == 0) return btVector3(1, 0, 0); + return cp * (btScalar(1.0) / m); } - btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint) { btVector3 cp; - cp = btCross(udir,vdir).normalized(); + cp = btCross(udir, vdir).normalized(); - btScalar distu = -btDot(cp,ustart); - btScalar distv = -btDot(cp,vstart); - btScalar dist = (btScalar)fabs(distu-distv); - if(upoint) - { + btScalar distu = -btDot(cp, ustart); + btScalar distv = -btDot(cp, vstart); + btScalar dist = (btScalar)fabs(distu - distv); + if (upoint) + { btPlane plane; - plane.normal = btCross(vdir,cp).normalized(); - plane.dist = -btDot(plane.normal,vstart); - *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); + plane.normal = btCross(vdir, cp).normalized(); + plane.dist = -btDot(plane.normal, vstart); + *upoint = PlaneLineIntersection(plane, ustart, ustart + udir); } - if(vpoint) - { + if (vpoint) + { btPlane plane; - plane.normal = btCross(udir,cp).normalized(); - plane.dist = -btDot(plane.normal,ustart); - *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); + plane.normal = btCross(udir, cp).normalized(); + plane.dist = -btDot(plane.normal, ustart); + *vpoint = PlaneLineIntersection(plane, vstart, vstart + vdir); } return dist; } - - - - - - -#define COPLANAR (0) -#define UNDER (1) -#define OVER (2) -#define SPLIT (OVER|UNDER) +#define COPLANAR (0) +#define UNDER (1) +#define OVER (2) +#define SPLIT (OVER | UNDER) #define PAPERWIDTH (btScalar(0.001)) btScalar planetestepsilon = PAPERWIDTH; - - typedef ConvexH::HalfEdge HalfEdge; -ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) +ConvexH::ConvexH(int vertices_size, int edges_size, int facets_size) { vertices.resize(vertices_size); edges.resize(edges_size); facets.resize(facets_size); } - int PlaneTest(const btPlane &p, const btVector3 &v); -int PlaneTest(const btPlane &p, const btVector3 &v) { - btScalar a = btDot(v,p.normal)+p.dist; - int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); +int PlaneTest(const btPlane &p, const btVector3 &v) +{ + btScalar a = btDot(v, p.normal) + p.dist; + int flag = (a > planetestepsilon) ? OVER : ((a < -planetestepsilon) ? UNDER : COPLANAR); return flag; } -int SplitTest(ConvexH &convex,const btPlane &plane); -int SplitTest(ConvexH &convex,const btPlane &plane) { - int flag=0; - for(int i=0;i -int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray &allow) +template +int maxdirfiltered(const T *p, int count, const T &dir, btAlignedObjectArray &allow) { btAssert(count); - int m=-1; - for(int i=0;ibtDot(p[m],dir)) - m=i; + if (m == -1 || btDot(p[i], dir) > btDot(p[m], dir)) + m = i; } - btAssert(m!=-1); + btAssert(m != -1); return m; -} +} btVector3 orth(const btVector3 &v); btVector3 orth(const btVector3 &v) { - btVector3 a=btCross(v,btVector3(0,0,1)); - btVector3 b=btCross(v,btVector3(0,1,0)); + btVector3 a = btCross(v, btVector3(0, 0, 1)); + btVector3 b = btCross(v, btVector3(0, 1, 0)); if (a.length() > b.length()) { return a.normalized(); - } else { + } + else + { return b.normalized(); } } - -template -int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray &allow) +template +int maxdirsterid(const T *p, int count, const T &dir, btAlignedObjectArray &allow) { - int m=-1; - while(m==-1) + int m = -1; + while (m == -1) { - m = maxdirfiltered(p,count,dir,allow); - if(allow[m]==3) return m; + m = maxdirfiltered(p, count, dir, allow); + if (allow[m] == 3) return m; T u = orth(dir); - T v = btCross(u,dir); - int ma=-1; - for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) + T v = btCross(u, dir); + int ma = -1; + for (btScalar x = btScalar(0.0); x <= btScalar(360.0); x += btScalar(45.0)) { - btScalar s = btSin(SIMD_RADS_PER_DEG*(x)); - btScalar c = btCos(SIMD_RADS_PER_DEG*(x)); - int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); - if(ma==m && mb==m) + btScalar s = btSin(SIMD_RADS_PER_DEG * (x)); + btScalar c = btCos(SIMD_RADS_PER_DEG * (x)); + int mb = maxdirfiltered(p, count, dir + (u * s + v * c) * btScalar(0.025), allow); + if (ma == m && mb == m) { - allow[m]=3; + allow[m] = 3; return m; } - if(ma!=-1 && ma!=mb) // Yuck - this is really ugly + if (ma != -1 && ma != mb) // Yuck - this is really ugly { int mc = ma; - for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0)) + for (btScalar xx = x - btScalar(40.0); xx <= x; xx += btScalar(5.0)) { - btScalar s = btSin(SIMD_RADS_PER_DEG*(xx)); - btScalar c = btCos(SIMD_RADS_PER_DEG*(xx)); - int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); - if(mc==m && md==m) + btScalar s = btSin(SIMD_RADS_PER_DEG * (xx)); + btScalar c = btCos(SIMD_RADS_PER_DEG * (xx)); + int md = maxdirfiltered(p, count, dir + (u * s + v * c) * btScalar(0.025), allow); + if (mc == m && md == m) { - allow[m]=3; + allow[m] = 3; return m; } - mc=md; + mc = md; } } - ma=mb; + ma = mb; } - allow[m]=0; - m=-1; + allow[m] = 0; + m = -1; } btAssert(0); return m; -} +} - - - -int operator ==(const int3 &a,const int3 &b); -int operator ==(const int3 &a,const int3 &b) +int operator==(const int3 &a, const int3 &b); +int operator==(const int3 &a, const int3 &b) { - for(int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - if(a[i]!=b[i]) return 0; + if (a[i] != b[i]) return 0; } return 1; } - -int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon); -int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) +int above(btVector3 *vertices, const int3 &t, const btVector3 &p, btScalar epsilon); +int above(btVector3 *vertices, const int3 &t, const btVector3 &p, btScalar epsilon) { - btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); - return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? + btVector3 n = TriNormal(vertices[t[0]], vertices[t[1]], vertices[t[2]]); + return (btDot(n, p - vertices[t[0]]) > epsilon); // EPSILON??? } -int hasedge(const int3 &t, int a,int b); -int hasedge(const int3 &t, int a,int b) +int hasedge(const int3 &t, int a, int b); +int hasedge(const int3 &t, int a, int b) { - for(int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - int i1= (i+1)%3; - if(t[i]==a && t[i1]==b) return 1; + int i1 = (i + 1) % 3; + if (t[i] == a && t[i1] == b) return 1; } return 0; } int hasvert(const int3 &t, int v); int hasvert(const int3 &t, int v) { - return (t[0]==v || t[1]==v || t[2]==v) ; + return (t[0] == v || t[1] == v || t[2] == v); } -int shareedge(const int3 &a,const int3 &b); -int shareedge(const int3 &a,const int3 &b) +int shareedge(const int3 &a, const int3 &b); +int shareedge(const int3 &a, const int3 &b) { int i; - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - int i1= (i+1)%3; - if(hasedge(a,b[i1],b[i])) return 1; + int i1 = (i + 1) % 3; + if (hasedge(a, b[i1], b[i])) return 1; } return 0; } class btHullTriangle; - - class btHullTriangle : public int3 { public: @@ -346,51 +327,50 @@ public: int id; int vmax; btScalar rise; - btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) + btHullTriangle(int a, int b, int c) : int3(a, b, c), n(-1, -1, -1) { - vmax=-1; + vmax = -1; rise = btScalar(0.0); } ~btHullTriangle() { } - int &neib(int a,int b); + int &neib(int a, int b); }; - -int &btHullTriangle::neib(int a,int b) +int &btHullTriangle::neib(int a, int b) { - static int er=-1; + static int er = -1; int i; - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - int i1=(i+1)%3; - int i2=(i+2)%3; - if((*this)[i]==a && (*this)[i1]==b) return n[i2]; - if((*this)[i]==b && (*this)[i1]==a) return n[i2]; + int i1 = (i + 1) % 3; + int i2 = (i + 2) % 3; + if ((*this)[i] == a && (*this)[i1] == b) return n[i2]; + if ((*this)[i] == b && (*this)[i1] == a) return n[i2]; } btAssert(0); return er; } -void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t) +void HullLibrary::b2bfix(btHullTriangle *s, btHullTriangle *t) { int i; - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - int i1=(i+1)%3; - int i2=(i+2)%3; + int i1 = (i + 1) % 3; + int i2 = (i + 2) % 3; int a = (*s)[i1]; int b = (*s)[i2]; - btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id); - btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id); - m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a); - m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); + btAssert(m_tris[s->neib(a, b)]->neib(b, a) == s->id); + btAssert(m_tris[t->neib(a, b)]->neib(b, a) == t->id); + m_tris[s->neib(a, b)]->neib(b, a) = t->neib(b, a); + m_tris[t->neib(b, a)]->neib(a, b) = s->neib(a, b); } } -void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t) +void HullLibrary::removeb2b(btHullTriangle *s, btHullTriangle *t) { - b2bfix(s,t); + b2bfix(s, t); deAllocateTriangle(s); deAllocateTriangle(t); @@ -401,11 +381,11 @@ void HullLibrary::checkit(btHullTriangle *t) (void)t; int i; - btAssert(m_tris[t->id]==t); - for(i=0;i<3;i++) + btAssert(m_tris[t->id] == t); + for (i = 0; i < 3; i++) { - int i1=(i+1)%3; - int i2=(i+2)%3; + int i1 = (i + 1) % 3; + int i2 = (i + 2) % 3; int a = (*t)[i1]; int b = (*t)[i2]; @@ -415,226 +395,233 @@ void HullLibrary::checkit(btHullTriangle *t) (void)a; (void)b; - btAssert(a!=b); - btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); + btAssert(a != b); + btAssert(m_tris[t->n[i]]->neib(b, a) == t->id); } } -btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c) +btHullTriangle *HullLibrary::allocateTriangle(int a, int b, int c) { - void* mem = btAlignedAlloc(sizeof(btHullTriangle),16); - btHullTriangle* tr = new (mem)btHullTriangle(a,b,c); + void *mem = btAlignedAlloc(sizeof(btHullTriangle), 16); + btHullTriangle *tr = new (mem) btHullTriangle(a, b, c); tr->id = m_tris.size(); m_tris.push_back(tr); return tr; } -void HullLibrary::deAllocateTriangle(btHullTriangle* tri) +void HullLibrary::deAllocateTriangle(btHullTriangle *tri) { - btAssert(m_tris[tri->id]==tri); - m_tris[tri->id]=NULL; + btAssert(m_tris[tri->id] == tri); + m_tris[tri->id] = NULL; tri->~btHullTriangle(); btAlignedFree(tri); } - -void HullLibrary::extrude(btHullTriangle *t0,int v) +void HullLibrary::extrude(btHullTriangle *t0, int v) { - int3 t= *t0; + int3 t = *t0; int n = m_tris.size(); - btHullTriangle* ta = allocateTriangle(v,t[1],t[2]); - ta->n = int3(t0->n[0],n+1,n+2); - m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; - btHullTriangle* tb = allocateTriangle(v,t[2],t[0]); - tb->n = int3(t0->n[1],n+2,n+0); - m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; - btHullTriangle* tc = allocateTriangle(v,t[0],t[1]); - tc->n = int3(t0->n[2],n+0,n+1); - m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; + btHullTriangle *ta = allocateTriangle(v, t[1], t[2]); + ta->n = int3(t0->n[0], n + 1, n + 2); + m_tris[t0->n[0]]->neib(t[1], t[2]) = n + 0; + btHullTriangle *tb = allocateTriangle(v, t[2], t[0]); + tb->n = int3(t0->n[1], n + 2, n + 0); + m_tris[t0->n[1]]->neib(t[2], t[0]) = n + 1; + btHullTriangle *tc = allocateTriangle(v, t[0], t[1]); + tc->n = int3(t0->n[2], n + 0, n + 1); + m_tris[t0->n[2]]->neib(t[0], t[1]) = n + 2; checkit(ta); checkit(tb); checkit(tc); - if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]); - if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]); - if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]); + if (hasvert(*m_tris[ta->n[0]], v)) removeb2b(ta, m_tris[ta->n[0]]); + if (hasvert(*m_tris[tb->n[0]], v)) removeb2b(tb, m_tris[tb->n[0]]); + if (hasvert(*m_tris[tc->n[0]], v)) removeb2b(tc, m_tris[tc->n[0]]); deAllocateTriangle(t0); - } -btHullTriangle* HullLibrary::extrudable(btScalar epsilon) +btHullTriangle *HullLibrary::extrudable(btScalar epsilon) { int i; - btHullTriangle *t=NULL; - for(i=0;iriserise)) + if (!t || (m_tris[i] && t->rise < m_tris[i]->rise)) { t = m_tris[i]; } } - return (t->rise >epsilon)?t:NULL ; + return (t->rise > epsilon) ? t : NULL; } - - - -int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow) +int4 HullLibrary::FindSimplex(btVector3 *verts, int verts_count, btAlignedObjectArray &allow) { btVector3 basis[3]; - basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) ); - int p0 = maxdirsterid(verts,verts_count, basis[0],allow); - int p1 = maxdirsterid(verts,verts_count,-basis[0],allow); - basis[0] = verts[p0]-verts[p1]; - if(p0==p1 || basis[0]==btVector3(0,0,0)) - return int4(-1,-1,-1,-1); - basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); - basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]); + basis[0] = btVector3(btScalar(0.01), btScalar(0.02), btScalar(1.0)); + int p0 = maxdirsterid(verts, verts_count, basis[0], allow); + int p1 = maxdirsterid(verts, verts_count, -basis[0], allow); + basis[0] = verts[p0] - verts[p1]; + if (p0 == p1 || basis[0] == btVector3(0, 0, 0)) + return int4(-1, -1, -1, -1); + basis[1] = btCross(btVector3(btScalar(1), btScalar(0.02), btScalar(0)), basis[0]); + basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)), basis[0]); if (basis[1].length() > basis[2].length()) { basis[1].normalize(); - } else { - basis[1] = basis[2]; - basis[1].normalize (); } - int p2 = maxdirsterid(verts,verts_count,basis[1],allow); - if(p2 == p0 || p2 == p1) + else { - p2 = maxdirsterid(verts,verts_count,-basis[1],allow); + basis[1] = basis[2]; + basis[1].normalize(); } - if(p2 == p0 || p2 == p1) - return int4(-1,-1,-1,-1); + int p2 = maxdirsterid(verts, verts_count, basis[1], allow); + if (p2 == p0 || p2 == p1) + { + p2 = maxdirsterid(verts, verts_count, -basis[1], allow); + } + if (p2 == p0 || p2 == p1) + return int4(-1, -1, -1, -1); basis[1] = verts[p2] - verts[p0]; - basis[2] = btCross(basis[1],basis[0]).normalized(); - int p3 = maxdirsterid(verts,verts_count,basis[2],allow); - if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow); - if(p3==p0||p3==p1||p3==p2) - return int4(-1,-1,-1,-1); - btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); - if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {btSwap(p2,p3);} - return int4(p0,p1,p2,p3); + basis[2] = btCross(basis[1], basis[0]).normalized(); + int p3 = maxdirsterid(verts, verts_count, basis[2], allow); + if (p3 == p0 || p3 == p1 || p3 == p2) p3 = maxdirsterid(verts, verts_count, -basis[2], allow); + if (p3 == p0 || p3 == p1 || p3 == p2) + return int4(-1, -1, -1, -1); + btAssert(!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3)); + if (btDot(verts[p3] - verts[p0], btCross(verts[p1] - verts[p0], verts[p2] - verts[p0])) < 0) + { + btSwap(p2, p3); + } + return int4(p0, p1, p2, p3); } -int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) +int HullLibrary::calchullgen(btVector3 *verts, int verts_count, int vlimit) { - if(verts_count <4) return 0; - if(vlimit==0) vlimit=1000000000; + if (verts_count < 4) return 0; + if (vlimit == 0) vlimit = 1000000000; int j; - btVector3 bmin(*verts),bmax(*verts); + btVector3 bmin(*verts), bmax(*verts); btAlignedObjectArray isextreme; isextreme.reserve(verts_count); btAlignedObjectArray allow; allow.reserve(verts_count); - for(j=0;jn = int3(2, 3, 1); + btHullTriangle *t1 = allocateTriangle(p[3], p[2], p[0]); + t1->n = int3(3, 2, 0); + btHullTriangle *t2 = allocateTriangle(p[0], p[1], p[3]); + t2->n = int3(0, 1, 3); + btHullTriangle *t3 = allocateTriangle(p[1], p[0], p[2]); + t3->n = int3(1, 0, 2); + isextreme[p[0]] = isextreme[p[1]] = isextreme[p[2]] = isextreme[p[3]] = 1; + checkit(t0); + checkit(t1); + checkit(t2); + checkit(t3); - - - btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point - btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1); - btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); - btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); - btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); - isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; - checkit(t0);checkit(t1);checkit(t2);checkit(t3); - - for(j=0;jvmax<0); - btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); - t->vmax = maxdirsterid(verts,verts_count,n,allow); - t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]); + btAssert(t->vmax < 0); + btVector3 n = TriNormal(verts[(*t)[0]], verts[(*t)[1]], verts[(*t)[2]]); + t->vmax = maxdirsterid(verts, verts_count, n, allow); + t->rise = btDot(n, verts[t->vmax] - verts[(*t)[0]]); } btHullTriangle *te; - vlimit-=4; - while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) + vlimit -= 4; + while (vlimit > 0 && ((te = extrudable(epsilon)) != 0)) { //int3 ti=*te; - int v=te->vmax; + int v = te->vmax; btAssert(v != -1); btAssert(!isextreme[v]); // wtf we've already done this vertex - isextreme[v]=1; + isextreme[v] = 1; //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already - j=m_tris.size(); - while(j--) { - if(!m_tris[j]) continue; - int3 t=*m_tris[j]; - if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) + j = m_tris.size(); + while (j--) + { + if (!m_tris[j]) continue; + int3 t = *m_tris[j]; + if (above(verts, t, verts[v], btScalar(0.01) * epsilon)) { - extrude(m_tris[j],v); + extrude(m_tris[j], v); } } // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle - j=m_tris.size(); - while(j--) + j = m_tris.size(); + while (j--) { - if(!m_tris[j]) continue; - if(!hasvert(*m_tris[j],v)) break; - int3 nt=*m_tris[j]; - if(above(verts,nt,center,btScalar(0.01)*epsilon) || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) + if (!m_tris[j]) continue; + if (!hasvert(*m_tris[j], v)) break; + int3 nt = *m_tris[j]; + if (above(verts, nt, center, btScalar(0.01) * epsilon) || btCross(verts[nt[1]] - verts[nt[0]], verts[nt[2]] - verts[nt[1]]).length() < epsilon * epsilon * btScalar(0.1)) { btHullTriangle *nb = m_tris[m_tris[j]->n[0]]; - btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->idid < j); + extrude(nb, v); + j = m_tris.size(); } - } - j=m_tris.size(); - while(j--) + } + j = m_tris.size(); + while (j--) { - btHullTriangle *t=m_tris[j]; - if(!t) continue; - if(t->vmax>=0) break; - btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); - t->vmax = maxdirsterid(verts,verts_count,n,allow); - if(isextreme[t->vmax]) + btHullTriangle *t = m_tris[j]; + if (!t) continue; + if (t->vmax >= 0) break; + btVector3 n = TriNormal(verts[(*t)[0]], verts[(*t)[1]], verts[(*t)[2]]); + t->vmax = maxdirsterid(verts, verts_count, n, allow); + if (isextreme[t->vmax]) { - t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate. + t->vmax = -1; // already done that vertex - algorithm needs to be able to terminate. } else { - t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]); + t->rise = btDot(n, verts[t->vmax] - verts[(*t)[0]]); } } - vlimit --; + vlimit--; } return 1; } -int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit) +int HullLibrary::calchull(btVector3 *verts, int verts_count, TUIntArray &tris_out, int &tris_count, int vlimit) { - int rc=calchullgen(verts,verts_count, vlimit) ; - if(!rc) return 0; + int rc = calchullgen(verts, verts_count, vlimit); + if (!rc) return 0; btAlignedObjectArray ts; int i; - for(i=0;i(ts[i]); } @@ -643,29 +630,22 @@ int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out return 1; } - - - - -bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) +bool HullLibrary::ComputeHull(unsigned int vcount, const btVector3 *vertices, PHullResult &result, unsigned int vlimit) { - - int tris_count; - int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast(vlimit) ); - if(!ret) return false; - result.mIndexCount = (unsigned int) (tris_count*3); - result.mFaceCount = (unsigned int) tris_count; - result.mVertices = (btVector3*) vertices; - result.mVcount = (unsigned int) vcount; + int tris_count; + int ret = calchull((btVector3 *)vertices, (int)vcount, result.m_Indices, tris_count, static_cast(vlimit)); + if (!ret) return false; + result.mIndexCount = (unsigned int)(tris_count * 3); + result.mFaceCount = (unsigned int)tris_count; + result.mVertices = (btVector3 *)vertices; + result.mVcount = (unsigned int)vcount; return true; - } - void ReleaseHull(PHullResult &result); void ReleaseHull(PHullResult &result) { - if ( result.m_Indices.size() ) + if (result.m_Indices.size()) { result.m_Indices.clear(); } @@ -675,7 +655,6 @@ void ReleaseHull(PHullResult &result) result.mVertices = 0; } - //********************************************************************* //********************************************************************* //******** HullLib header @@ -688,16 +667,15 @@ void ReleaseHull(PHullResult &result) //********************************************************************* //********************************************************************* -HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request - HullResult &result) // contains the resulst +HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request + HullResult &result) // contains the resulst { HullError ret = QE_FAIL; - PHullResult hr; unsigned int vcount = desc.mVcount; - if ( vcount < 8 ) vcount = 8; + if (vcount < 8) vcount = 8; btAlignedObjectArray vertexSource; vertexSource.resize(static_cast(vcount)); @@ -706,87 +684,82 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // unsigned int ovcount; - bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! + bool ok = CleanupVertices(desc.mVcount, desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale); // normalize point cloud, remove duplicates! - if ( ok ) + if (ok) { - - -// if ( 1 ) // scale vertices back to their original size. + // if ( 1 ) // scale vertices back to their original size. { - for (unsigned int i=0; i(i)]; - v[0]*=scale[0]; - v[1]*=scale[1]; - v[2]*=scale[2]; + btVector3 &v = vertexSource[static_cast(i)]; + v[0] *= scale[0]; + v[1] *= scale[1]; + v[2] *= scale[2]; } } - ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices); + ok = ComputeHull(ovcount, &vertexSource[0], hr, desc.mMaxVertices); - if ( ok ) + if (ok) { - // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. - btAlignedObjectArray vertexScratch; + btAlignedObjectArray vertexScratch; vertexScratch.resize(static_cast(hr.mVcount)); - BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount ); + BringOutYourDead(hr.mVertices, hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount); ret = QE_OK; - if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle! + if (desc.HasHullFlag(QF_TRIANGLES)) // if he wants the results as triangle! { - result.mPolygons = false; + result.mPolygons = false; result.mNumOutputVertices = ovcount; result.m_OutputVertices.resize(static_cast(ovcount)); - result.mNumFaces = hr.mFaceCount; - result.mNumIndices = hr.mIndexCount; + result.mNumFaces = hr.mFaceCount; + result.mNumIndices = hr.mIndexCount; result.m_Indices.resize(static_cast(hr.mIndexCount)); - memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3) * ovcount); - if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) + if (desc.HasHullFlag(QF_REVERSE_ORDER)) { - const unsigned int *source = &hr.m_Indices[0]; - unsigned int *dest = &result.m_Indices[0]; + unsigned int *dest = &result.m_Indices[0]; - for (unsigned int i=0; i(ovcount)); - result.mNumFaces = hr.mFaceCount; - result.mNumIndices = hr.mIndexCount+hr.mFaceCount; + result.mNumFaces = hr.mFaceCount; + result.mNumIndices = hr.mIndexCount + hr.mFaceCount; result.m_Indices.resize(static_cast(result.mNumIndices)); - memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3) * ovcount); -// if ( 1 ) + // if ( 1 ) { const unsigned int *source = &hr.m_Indices[0]; - unsigned int *dest = &result.m_Indices[0]; - for (unsigned int i=0; i bmax[j] ) bmax[j] = p[j]; + if (p[j] < bmin[j]) bmin[j] = p[j]; + if (p[j] > bmax[j]) bmax[j] = p[j]; } } } @@ -905,28 +871,27 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, btVector3 center; - center[0] = dx*btScalar(0.5) + bmin[0]; - center[1] = dy*btScalar(0.5) + bmin[1]; - center[2] = dz*btScalar(0.5) + bmin[2]; + center[0] = dx * btScalar(0.5) + bmin[0]; + center[1] = dy * btScalar(0.5) + bmin[1]; + center[2] = dz * btScalar(0.5) + bmin[2]; - if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 ) + if (dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3) { - btScalar len = FLT_MAX; - if ( dx > EPSILON && dx < len ) len = dx; - if ( dy > EPSILON && dy < len ) len = dy; - if ( dz > EPSILON && dz < len ) len = dz; + if (dx > EPSILON && dx < len) len = dx; + if (dy > EPSILON && dy < len) len = dy; + if (dz > EPSILON && dz < len) len = dz; - if ( len == FLT_MAX ) + if (len == FLT_MAX) { - dx = dy = dz = btScalar(0.01); // one centimeter + dx = dy = dz = btScalar(0.01); // one centimeter } else { - if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. - if ( dy < EPSILON ) dy = len * btScalar(0.05); - if ( dz < EPSILON ) dz = len * btScalar(0.05); + if (dx < EPSILON) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. + if (dy < EPSILON) dy = len * btScalar(0.05); + if (dz < EPSILON) dz = len * btScalar(0.05); } btScalar x1 = center[0] - dx; @@ -938,22 +903,20 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, btScalar z1 = center[2] - dz; btScalar z2 = center[2] + dz; - addPoint(vcount,vertices,x1,y1,z1); - addPoint(vcount,vertices,x2,y1,z1); - addPoint(vcount,vertices,x2,y2,z1); - addPoint(vcount,vertices,x1,y2,z1); - addPoint(vcount,vertices,x1,y1,z2); - addPoint(vcount,vertices,x2,y1,z2); - addPoint(vcount,vertices,x2,y2,z2); - addPoint(vcount,vertices,x1,y2,z2); - - return true; // return cube - + addPoint(vcount, vertices, x1, y1, z1); + addPoint(vcount, vertices, x2, y1, z1); + addPoint(vcount, vertices, x2, y2, z1); + addPoint(vcount, vertices, x1, y2, z1); + addPoint(vcount, vertices, x1, y1, z2); + addPoint(vcount, vertices, x2, y1, z2); + addPoint(vcount, vertices, x2, y2, z2); + addPoint(vcount, vertices, x1, y2, z2); + return true; // return cube } else { - if ( scale ) + if (scale) { scale[0] = dx; scale[1] = dy; @@ -963,75 +926,70 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, recip[1] = 1 / dy; recip[2] = 1 / dz; - center[0]*=recip[0]; - center[1]*=recip[1]; - center[2]*=recip[2]; - + center[0] *= recip[0]; + center[1] *= recip[1]; + center[2] *= recip[2]; } - } + vtx = (const char *)svertices; - - vtx = (const char *) svertices; - - for (unsigned int i=0; igetX(); btScalar py = p->getY(); btScalar pz = p->getZ(); - if ( scale ) + if (scale) { - px = px*recip[0]; // normalize - py = py*recip[1]; // normalize - pz = pz*recip[2]; // normalize + px = px * recip[0]; // normalize + py = py * recip[1]; // normalize + pz = pz * recip[2]; // normalize } -// if ( 1 ) + // if ( 1 ) { unsigned int j; - for (j=0; j dist2 ) + if (dist1 > dist2) { v[0] = px; v[1] = py; v[2] = pz; - } break; } } - if ( j == vcount ) + if (j == vcount) { - btVector3& dest = vertices[vcount]; + btVector3 &dest = vertices[vcount]; dest[0] = px; dest[1] = py; dest[2] = pz; @@ -1042,18 +1000,18 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, } // ok..now make sure we didn't prune so many vertices it is now invalid. -// if ( 1 ) + // if ( 1 ) { - btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; - btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; + btScalar bmin[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + btScalar bmax[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - for (unsigned int i=0; i bmax[j] ) bmax[j] = p[j]; + if (p[j] < bmin[j]) bmin[j] = p[j]; + if (p[j] > bmax[j]) bmax[j] = p[j]; } } @@ -1061,27 +1019,27 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, btScalar dy = bmax[1] - bmin[1]; btScalar dz = bmax[2] - bmin[2]; - if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) + if (dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) { - btScalar cx = dx*btScalar(0.5) + bmin[0]; - btScalar cy = dy*btScalar(0.5) + bmin[1]; - btScalar cz = dz*btScalar(0.5) + bmin[2]; + btScalar cx = dx * btScalar(0.5) + bmin[0]; + btScalar cy = dy * btScalar(0.5) + bmin[1]; + btScalar cz = dz * btScalar(0.5) + bmin[2]; btScalar len = FLT_MAX; - if ( dx >= EPSILON && dx < len ) len = dx; - if ( dy >= EPSILON && dy < len ) len = dy; - if ( dz >= EPSILON && dz < len ) len = dz; + if (dx >= EPSILON && dx < len) len = dx; + if (dy >= EPSILON && dy < len) len = dy; + if (dz >= EPSILON && dz < len) len = dz; - if ( len == FLT_MAX ) + if (len == FLT_MAX) { - dx = dy = dz = btScalar(0.01); // one centimeter + dx = dy = dz = btScalar(0.01); // one centimeter } else { - if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. - if ( dy < EPSILON ) dy = len * btScalar(0.05); - if ( dz < EPSILON ) dz = len * btScalar(0.05); + if (dx < EPSILON) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. + if (dy < EPSILON) dy = len * btScalar(0.05); + if (dz < EPSILON) dz = len * btScalar(0.05); } btScalar x1 = cx - dx; @@ -1093,16 +1051,16 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, btScalar z1 = cz - dz; btScalar z2 = cz + dz; - vcount = 0; // add box + vcount = 0; // add box - addPoint(vcount,vertices,x1,y1,z1); - addPoint(vcount,vertices,x2,y1,z1); - addPoint(vcount,vertices,x2,y2,z1); - addPoint(vcount,vertices,x1,y2,z1); - addPoint(vcount,vertices,x1,y1,z2); - addPoint(vcount,vertices,x2,y1,z2); - addPoint(vcount,vertices,x2,y2,z2); - addPoint(vcount,vertices,x1,y2,z2); + addPoint(vcount, vertices, x1, y1, z1); + addPoint(vcount, vertices, x2, y1, z1); + addPoint(vcount, vertices, x2, y2, z1); + addPoint(vcount, vertices, x1, y2, z1); + addPoint(vcount, vertices, x1, y1, z2); + addPoint(vcount, vertices, x2, y1, z2); + addPoint(vcount, vertices, x2, y2, z2); + addPoint(vcount, vertices, x1, y2, z2); return true; } @@ -1111,57 +1069,52 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, return true; } -void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount) +void HullLibrary::BringOutYourDead(const btVector3 *verts, unsigned int vcount, btVector3 *overts, unsigned int &ocount, unsigned int *indices, unsigned indexcount) { - btAlignedObjectArraytmpIndices; + btAlignedObjectArray tmpIndices; tmpIndices.resize(m_vertexIndexMapping.size()); int i; - for (i=0;i(vcount)); - memset(&usedIndices[0],0,sizeof(unsigned int)*vcount); + memset(&usedIndices[0], 0, sizeof(unsigned int) * vcount); ocount = 0; - for (i=0; i= 0 && v < vcount ); + btAssert(v >= 0 && v < vcount); - if ( usedIndices[static_cast(v)] ) // if already remapped + if (usedIndices[static_cast(v)]) // if already remapped { - indices[i] = usedIndices[static_cast(v)]-1; // index to new array + indices[i] = usedIndices[static_cast(v)] - 1; // index to new array } else { + indices[i] = ocount; // new index mapping - indices[i] = ocount; // new index mapping - - overts[ocount][0] = verts[v][0]; // copy old vert to new vert array + overts[ocount][0] = verts[v][0]; // copy old vert to new vert array overts[ocount][1] = verts[v][1]; overts[ocount][2] = verts[v][2]; - for (int k=0;k=0 && ocount <= vcount ); + btAssert(ocount >= 0 && ocount <= vcount); - usedIndices[static_cast(v)] = ocount; // assign new index remapping - - + usedIndices[static_cast(v)] = ocount; // assign new index remapping } } - - } diff --git a/Engine/lib/bullet/src/LinearMath/btConvexHull.h b/Engine/lib/bullet/src/LinearMath/btConvexHull.h index 69c52bc6f..f890d75ea 100644 --- a/Engine/lib/bullet/src/LinearMath/btConvexHull.h +++ b/Engine/lib/bullet/src/LinearMath/btConvexHull.h @@ -34,106 +34,102 @@ public: mNumFaces = 0; mNumIndices = 0; } - bool mPolygons; // true if indices represents polygons, false indices are triangles - unsigned int mNumOutputVertices; // number of vertices in the output hull - btAlignedObjectArray m_OutputVertices; // array of vertices - unsigned int mNumFaces; // the number of faces produced - unsigned int mNumIndices; // the total number of indices - btAlignedObjectArray m_Indices; // pointer to indices. + bool mPolygons; // true if indices represents polygons, false indices are triangles + unsigned int mNumOutputVertices; // number of vertices in the output hull + btAlignedObjectArray m_OutputVertices; // array of vertices + unsigned int mNumFaces; // the number of faces produced + unsigned int mNumIndices; // the total number of indices + btAlignedObjectArray m_Indices; // pointer to indices. -// If triangles, then indices are array indexes into the vertex list. -// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. + // If triangles, then indices are array indexes into the vertex list. + // If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. }; enum HullFlag { - QF_TRIANGLES = (1<<0), // report results as triangles, not polygons. - QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices. - QF_DEFAULT = QF_TRIANGLES + QF_TRIANGLES = (1 << 0), // report results as triangles, not polygons. + QF_REVERSE_ORDER = (1 << 1), // reverse order of the triangle indices. + QF_DEFAULT = QF_TRIANGLES }; - class HullDesc { public: HullDesc(void) { - mFlags = QF_DEFAULT; - mVcount = 0; - mVertices = 0; - mVertexStride = sizeof(btVector3); - mNormalEpsilon = 0.001f; - mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. - mMaxFaces = 4096; + mFlags = QF_DEFAULT; + mVcount = 0; + mVertices = 0; + mVertexStride = sizeof(btVector3); + mNormalEpsilon = 0.001f; + mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. + mMaxFaces = 4096; }; HullDesc(HullFlag flag, - unsigned int vcount, - const btVector3 *vertices, - unsigned int stride = sizeof(btVector3)) + unsigned int vcount, + const btVector3* vertices, + unsigned int stride = sizeof(btVector3)) { - mFlags = flag; - mVcount = vcount; - mVertices = vertices; - mVertexStride = stride; - mNormalEpsilon = btScalar(0.001); - mMaxVertices = 4096; + mFlags = flag; + mVcount = vcount; + mVertices = vertices; + mVertexStride = stride; + mNormalEpsilon = btScalar(0.001); + mMaxVertices = 4096; } bool HasHullFlag(HullFlag flag) const { - if ( mFlags & flag ) return true; + if (mFlags & flag) return true; return false; } void SetHullFlag(HullFlag flag) { - mFlags|=flag; + mFlags |= flag; } void ClearHullFlag(HullFlag flag) { - mFlags&=~flag; + mFlags &= ~flag; } - unsigned int mFlags; // flags to use when generating the convex hull. - unsigned int mVcount; // number of vertices in the input point cloud - const btVector3 *mVertices; // the array of vertices. - unsigned int mVertexStride; // the stride of each vertex, in bytes. - btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. - unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! - unsigned int mMaxFaces; + unsigned int mFlags; // flags to use when generating the convex hull. + unsigned int mVcount; // number of vertices in the input point cloud + const btVector3* mVertices; // the array of vertices. + unsigned int mVertexStride; // the stride of each vertex, in bytes. + btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. + unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! + unsigned int mMaxFaces; }; enum HullError { - QE_OK, // success! - QE_FAIL // failed. + QE_OK, // success! + QE_FAIL // failed. }; class btPlane { - public: - btVector3 normal; - btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 - btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){} - btPlane():normal(),dist(0){} - +public: + btVector3 normal; + btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + btPlane(const btVector3& n, btScalar d) : normal(n), dist(d) {} + btPlane() : normal(), dist(0) {} }; - - -class ConvexH +class ConvexH { - public: +public: class HalfEdge { - public: + public: short ea; // the other half of the edge (index into edges list) unsigned char v; // the vertex at the start of this edge (index into vertices list) unsigned char p; // the facet on which this edge lies (index into facets list) - HalfEdge(){} - HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){} + HalfEdge() {} + HalfEdge(short _ea, unsigned char _v, unsigned char _p) : ea(_ea), v(_v), p(_p) {} }; ConvexH() { @@ -143,25 +139,29 @@ class ConvexH } btAlignedObjectArray vertices; btAlignedObjectArray edges; - btAlignedObjectArray facets; - ConvexH(int vertices_size,int edges_size,int facets_size); + btAlignedObjectArray facets; + ConvexH(int vertices_size, int edges_size, int facets_size); }; - class int4 { public: - int x,y,z,w; + int x, y, z, w; int4(){}; - int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;} - const int& operator[](int i) const {return (&x)[i];} - int& operator[](int i) {return (&x)[i];} + int4(int _x, int _y, int _z, int _w) + { + x = _x; + y = _y; + z = _z; + w = _w; + } + const int& operator[](int i) const { return (&x)[i]; } + int& operator[](int i) { return (&x)[i]; } }; class PHullResult { public: - PHullResult(void) { mVcount = 0; @@ -173,69 +173,61 @@ public: unsigned int mVcount; unsigned int mIndexCount; unsigned int mFaceCount; - btVector3* mVertices; + btVector3* mVertices; TUIntArray m_Indices; }; - - ///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method. ///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape. class HullLibrary { - btAlignedObjectArray m_tris; public: - btAlignedObjectArray m_vertexIndexMapping; - - HullError CreateConvexHull(const HullDesc& desc, // describes the input request - HullResult& result); // contains the resulst - HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it. + HullError CreateConvexHull(const HullDesc& desc, // describes the input request + HullResult& result); // contains the resulst + HullError ReleaseResult(HullResult& result); // release memory allocated for this result, we are done with it. private: + bool ComputeHull(unsigned int vcount, const btVector3* vertices, PHullResult& result, unsigned int vlimit); - bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit); + class btHullTriangle* allocateTriangle(int a, int b, int c); + void deAllocateTriangle(btHullTriangle*); + void b2bfix(btHullTriangle* s, btHullTriangle* t); - class btHullTriangle* allocateTriangle(int a,int b,int c); - void deAllocateTriangle(btHullTriangle*); - void b2bfix(btHullTriangle* s,btHullTriangle*t); + void removeb2b(btHullTriangle* s, btHullTriangle* t); - void removeb2b(btHullTriangle* s,btHullTriangle*t); - - void checkit(btHullTriangle *t); + void checkit(btHullTriangle* t); btHullTriangle* extrudable(btScalar epsilon); - int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit); + int calchull(btVector3* verts, int verts_count, TUIntArray& tris_out, int& tris_count, int vlimit); - int calchullgen(btVector3 *verts,int verts_count, int vlimit); + int calchullgen(btVector3* verts, int verts_count, int vlimit); - int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow); + int4 FindSimplex(btVector3* verts, int verts_count, btAlignedObjectArray& allow); - class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); + class ConvexH* ConvexHCrop(ConvexH& convex, const btPlane& slice); - void extrude(class btHullTriangle* t0,int v); + void extrude(class btHullTriangle* t0, int v); ConvexH* test_cube(); - //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. + //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. //After the hull is generated it give you back a set of polygon faces which index the *original* point cloud. //The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull. //The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation. - void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount); + void BringOutYourDead(const btVector3* verts, unsigned int vcount, btVector3* overts, unsigned int& ocount, unsigned int* indices, unsigned indexcount); bool CleanupVertices(unsigned int svcount, - const btVector3* svertices, - unsigned int stride, - unsigned int &vcount, // output number of vertices - btVector3* vertices, // location to store the results. - btScalar normalepsilon, - btVector3& scale); + const btVector3* svertices, + unsigned int stride, + unsigned int& vcount, // output number of vertices + btVector3* vertices, // location to store the results. + btScalar normalepsilon, + btVector3& scale); }; - -#endif //BT_CD_HULL_H - +#endif //BT_CD_HULL_H diff --git a/Engine/lib/bullet/src/LinearMath/btConvexHullComputer.cpp b/Engine/lib/bullet/src/LinearMath/btConvexHullComputer.cpp index 2ea22cbe3..8bbfdc5f2 100644 --- a/Engine/lib/bullet/src/LinearMath/btConvexHullComputer.cpp +++ b/Engine/lib/bullet/src/LinearMath/btConvexHullComputer.cpp @@ -20,846 +20,847 @@ subject to the following restrictions: #include "btVector3.h" #ifdef __GNUC__ - #include +#include #elif defined(_MSC_VER) - typedef __int32 int32_t; - typedef __int64 int64_t; - typedef unsigned __int32 uint32_t; - typedef unsigned __int64 uint64_t; +typedef __int32 int32_t; +typedef __int64 int64_t; +typedef unsigned __int32 uint32_t; +typedef unsigned __int64 uint64_t; #else - typedef int int32_t; - typedef long long int int64_t; - typedef unsigned int uint32_t; - typedef unsigned long long int uint64_t; +typedef int int32_t; +typedef long long int int64_t; +typedef unsigned int uint32_t; +typedef unsigned long long int uint64_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 +#include #endif // Convex hull implementation based on Preparata and Hong // Ole Kniemeyer, MAXON Computer GmbH class btConvexHullInternal { +public: + class Point64 + { public: - - class Point64 + int64_t x; + int64_t y; + int64_t z; + + Point64(int64_t x, int64_t y, int64_t z) : x(x), y(y), z(z) { - public: - int64_t x; - int64_t y; - int64_t z; - - Point64(int64_t x, int64_t y, int64_t z): x(x), y(y), z(z) - { - } - - bool isZero() - { - return (x == 0) && (y == 0) && (z == 0); - } - - int64_t dot(const Point64& b) const - { - return x * b.x + y * b.y + z * b.z; - } - }; - - class Point32 - { - public: - int32_t x; - int32_t y; - int32_t z; - int index; - - Point32() - { - } - - Point32(int32_t x, int32_t y, int32_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); - } - - int64_t dot(const Point32& b) const - { - return x * b.x + y * b.y + z * b.z; - } - - int64_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: - uint64_t low; - uint64_t high; - - Int128() - { - } - - Int128(uint64_t low, uint64_t high): low(low), high(high) - { - } - - Int128(uint64_t low): low(low), high(0) - { - } - - Int128(int64_t value): low(value), high((value >= 0) ? 0 : (uint64_t) -1LL) - { - } - - static Int128 mul(int64_t a, int64_t b); - - static Int128 mul(uint64_t a, uint64_t b); - - Int128 operator-() const - { - return Int128((uint64_t) -(int64_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 - uint64_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 - uint64_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*(int64_t b) const; - - btScalar toScalar() const - { - return ((int64_t) high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low) - : -(-*this).toScalar(); - } - - int getSign() const - { - return ((int64_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: - uint64_t m_numerator; - uint64_t m_denominator; - int sign; - - public: - Rational64(int64_t numerator, int64_t denominator) - { - if (numerator > 0) - { - sign = 1; - m_numerator = (uint64_t) numerator; - } - else if (numerator < 0) - { - sign = -1; - m_numerator = (uint64_t) -numerator; - } - else - { - sign = 0; - m_numerator = 0; - } - if (denominator > 0) - { - m_denominator = (uint64_t) denominator; - } - else if (denominator < 0) - { - sign = -sign; - m_denominator = (uint64_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; - - btScalar toScalar() const - { - return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar) m_numerator / m_denominator); - } - }; - - - class Rational128 - { - private: - Int128 numerator; - Int128 denominator; - int sign; - bool isInt64; - - public: - Rational128(int64_t value) - { - if (value > 0) - { - sign = 1; - this->numerator = value; - } - else if (value < 0) - { - sign = -1; - this->numerator = -value; - } - else - { - sign = 0; - this->numerator = (uint64_t) 0; - } - this->denominator = (uint64_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(int64_t b) const; - - btScalar toScalar() const - { - return sign * ((denominator.getSign() == 0) ? SIMD_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) - { - } - - btScalar xvalue() const - { - return x.toScalar() / denominator.toScalar(); - } - - btScalar yvalue() const - { - return y.toScalar() / denominator.toScalar(); - } - - btScalar 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() - { - printf("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); - } - - btScalar xvalue() const - { - return (point.index >= 0) ? btScalar(point.x) : point128.xvalue(); - } - - btScalar yvalue() const - { - return (point.index >= 0) ? btScalar(point.y) : point128.yvalue(); - } - - btScalar zvalue() const - { - return (point.index >= 0) ? btScalar(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) - { - btAssert(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) - { - btAssert(reverse->target == n->reverse->target); - next = n; - n->prev = this; - } - -#ifdef DEBUG_CONVEX_HULL - void print() - { - printf("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 uint32_t high(uint64_t value) - { - return (uint32_t) (value >> 32); - } - - static uint32_t low(uint64_t value) - { - return (uint32_t) value; - } - - static uint64_t mul(uint32_t a, uint32_t b) - { - return (uint64_t) a * (uint64_t) b; - } - - static void shlHalf(uint64_t& value) - { - value <<= 32; - } - - static uint64_t high(Int128 value) - { - return value.high; - } - - static uint64_t low(Int128 value) - { - return value.low; - } - - static Int128 mul(uint64_t a, uint64_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*) btAlignedAlloc(sizeof(T) * size, 16); - } - - ~PoolArray() - { - btAlignedFree(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(); - btAlignedFree(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(btAlignedAlloc(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; - } - }; - - btVector3 scaling; - btVector3 center; - Pool vertexPool; - Pool edgePool; - Pool facePool; - btAlignedObjectArray 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; - - btAssert(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); - btVector3 toBtVector(const Point32& v); + bool isZero() + { + return (x == 0) && (y == 0) && (z == 0); + } - btVector3 getBtNormal(Face* face); + int64_t dot(const Point64& b) const + { + return x * b.x + y * b.y + z * b.z; + } + }; - bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray stack); + class Point32 + { + public: + int32_t x; + int32_t y; + int32_t z; + int index; + + Point32() + { + } + + Point32(int32_t x, int32_t y, int32_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); + } + + int64_t dot(const Point32& b) const + { + return x * b.x + y * b.y + z * b.z; + } + + int64_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: + uint64_t low; + uint64_t high; + + Int128() + { + } + + Int128(uint64_t low, uint64_t high) : low(low), high(high) + { + } + + Int128(uint64_t low) : low(low), high(0) + { + } + + Int128(int64_t value) : low(value), high((value >= 0) ? 0 : (uint64_t)-1LL) + { + } + + static Int128 mul(int64_t a, int64_t b); + + static Int128 mul(uint64_t a, uint64_t b); + + Int128 operator-() const + { + return Int128((uint64_t) - (int64_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 + uint64_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 + uint64_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*(int64_t b) const; + + btScalar toScalar() const + { + return ((int64_t)high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low) + : -(-*this).toScalar(); + } + + int getSign() const + { + return ((int64_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: + uint64_t m_numerator; + uint64_t m_denominator; + int sign; public: - Vertex* vertexList; + Rational64(int64_t numerator, int64_t denominator) + { + if (numerator > 0) + { + sign = 1; + m_numerator = (uint64_t)numerator; + } + else if (numerator < 0) + { + sign = -1; + m_numerator = (uint64_t)-numerator; + } + else + { + sign = 0; + m_numerator = 0; + } + if (denominator > 0) + { + m_denominator = (uint64_t)denominator; + } + else if (denominator < 0) + { + sign = -sign; + m_denominator = (uint64_t)-denominator; + } + else + { + m_denominator = 0; + } + } - void compute(const void* coords, bool doubleCoords, int stride, int count); + bool isNegativeInfinity() const + { + return (sign < 0) && (m_denominator == 0); + } - btVector3 getCoordinates(const Vertex* v); + bool isNaN() const + { + return (sign == 0) && (m_denominator == 0); + } - btScalar shrink(btScalar amount, btScalar clampAmount); + int compare(const Rational64& b) const; + + btScalar toScalar() const + { + return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar)m_numerator / m_denominator); + } + }; + + class Rational128 + { + private: + Int128 numerator; + Int128 denominator; + int sign; + bool isInt64; + + public: + Rational128(int64_t value) + { + if (value > 0) + { + sign = 1; + this->numerator = value; + } + else if (value < 0) + { + sign = -1; + this->numerator = -value; + } + else + { + sign = 0; + this->numerator = (uint64_t)0; + } + this->denominator = (uint64_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(int64_t b) const; + + btScalar toScalar() const + { + return sign * ((denominator.getSign() == 0) ? SIMD_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) + { + } + + btScalar xvalue() const + { + return x.toScalar() / denominator.toScalar(); + } + + btScalar yvalue() const + { + return y.toScalar() / denominator.toScalar(); + } + + btScalar 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() + { + printf("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); + } + + btScalar xvalue() const + { + return (point.index >= 0) ? btScalar(point.x) : point128.xvalue(); + } + + btScalar yvalue() const + { + return (point.index >= 0) ? btScalar(point.y) : point128.yvalue(); + } + + btScalar zvalue() const + { + return (point.index >= 0) ? btScalar(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) + { + btAssert(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) + { + btAssert(reverse->target == n->reverse->target); + next = n; + n->prev = this; + } + +#ifdef DEBUG_CONVEX_HULL + void print() + { + printf("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 uint32_t high(uint64_t value) + { + return (uint32_t)(value >> 32); + } + + static uint32_t low(uint64_t value) + { + return (uint32_t)value; + } + + static uint64_t mul(uint32_t a, uint32_t b) + { + return (uint64_t)a * (uint64_t)b; + } + + static void shlHalf(uint64_t& value) + { + value <<= 32; + } + + static uint64_t high(Int128 value) + { + return value.high; + } + + static uint64_t low(Int128 value) + { + return value.low; + } + + static Int128 mul(uint64_t a, uint64_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*)btAlignedAlloc(sizeof(T) * size, 16); + } + + ~PoolArray() + { + btAlignedFree(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(); + btAlignedFree(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 (btAlignedAlloc(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; + } + }; + + btVector3 scaling; + btVector3 center; + Pool vertexPool; + Pool edgePool; + Pool facePool; + btAlignedObjectArray 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; + + btAssert(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); + + btVector3 toBtVector(const Point32& v); + + btVector3 getBtNormal(Face* face); + + bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray stack); + +public: + Vertex* vertexList; + + void compute(const void* coords, bool doubleCoords, int stride, int count); + + btVector3 getCoordinates(const Vertex* v); + + btScalar shrink(btScalar amount, btScalar clampAmount); }; - btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const { - bool negative = (int64_t) high < 0; + bool negative = (int64_t)high < 0; Int128 a = negative ? -*this : *this; if (b < 0) { negative = !negative; b = -b; } - Int128 result = mul(a.low, (uint64_t) b); - result.high += a.high * (uint64_t) b; + Int128 result = mul(a.low, (uint64_t)b); + result.high += a.high * (uint64_t)b; return negative ? -result : result; } btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b) { Int128 result; - + #ifdef USE_X86_64_ASM - __asm__ ("imulq %[b]" - : "=a" (result.low), "=d" (result.high) - : "0"(a), [b] "r"(b) - : "cc" ); + __asm__("imulq %[b]" + : "=a"(result.low), "=d"(result.high) + : "0"(a), [b] "r"(b) + : "cc"); return result; - + #else bool negative = a < 0; if (negative) @@ -871,7 +872,7 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_ negative = !negative; b = -b; } - DMul::mul((uint64_t) a, (uint64_t) b, result.low, result.high); + DMul::mul((uint64_t)a, (uint64_t)b, result.low, result.high); return negative ? -result : result; #endif } @@ -881,10 +882,10 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint6 Int128 result; #ifdef USE_X86_64_ASM - __asm__ ("mulq %[b]" - : "=a" (result.low), "=d" (result.high) - : "0"(a), [b] "r"(b) - : "cc" ); + __asm__("mulq %[b]" + : "=a"(result.low), "=d"(result.high) + : "0"(a), [b] "r"(b) + : "cc"); #else DMul::mul(a, b, result.low, result.high); @@ -911,24 +912,25 @@ int btConvexHullInternal::Rational64::compare(const Rational64& b) const int result; int64_t tmp; int64_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; + __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"(m_denominator), [bn] "g"(b.m_numerator), [tn] "g"(m_numerator), [bd] "g"(b.m_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 @@ -949,7 +951,7 @@ int btConvexHullInternal::Rational128::compare(const Rational128& b) const } if (isInt64) { - return -b.compare(sign * (int64_t) numerator.low); + return -b.compare(sign * (int64_t)numerator.low); } Int128 nbdLow, nbdHigh, dbnLow, dbnHigh; @@ -968,7 +970,7 @@ int btConvexHullInternal::Rational128::compare(int64_t b) const { if (isInt64) { - int64_t a = sign * (int64_t) numerator.low; + int64_t a = sign * (int64_t)numerator.low; return (a > b) ? 1 : (a < b) ? -1 : 0; } if (b > 0) @@ -994,7 +996,6 @@ int btConvexHullInternal::Rational128::compare(int64_t b) const return numerator.ucmp(denominator * b) * sign; } - btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to) { btAssert(from && to); @@ -1062,7 +1063,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul } } } - + v0 = h0.maxXy; v1 = h1.maxXy; Vertex* v00 = NULL; @@ -1070,7 +1071,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul int32_t sign = 1; for (int side = 0; side <= 1; side++) - { + { int32_t dx = (v1->point.x - v0->point.x) * sign; if (dx > 0) { @@ -1113,7 +1114,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul while (true) { int32_t dy = v1->point.y - v0->point.y; - + Vertex* w1 = side ? v1->prev : v1->next; if (w1 != v1) { @@ -1126,7 +1127,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul continue; } } - + Vertex* w0 = side ? v0->prev : v0->next; if (w0 != v0) { @@ -1140,7 +1141,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul continue; } } - + break; } } @@ -1166,7 +1167,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul } v1 = w1; } - + if (side == 0) { v00 = v0; @@ -1192,7 +1193,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul { h0.maxXy = h1.maxXy; } - + h0.maxYx = h1.maxYx; c0 = v00; @@ -1279,19 +1280,19 @@ void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& } { Vertex* v = originalVertices[start]; - v->edges = NULL; - v->next = v; - v->prev = v; + v->edges = NULL; + v->next = v; + v->prev = v; - result.minXy = v; - result.maxXy = v; - result.minYx = v; - result.maxYx = v; + result.minXy = v; + result.maxXy = v; + result.minYx = v; + result.maxYx = v; } - + return; } - + case 1: { Vertex* v = originalVertices[start]; @@ -1309,7 +1310,7 @@ void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& } int split0 = start + n / 2; - Point32 p = originalVertices[split0-1]->point; + Point32 p = originalVertices[split0 - 1]->point; int split1 = split0; while ((split1 < end) && (originalVertices[split1]->point == p)) { @@ -1334,7 +1335,7 @@ void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& void btConvexHullInternal::IntermediateHull::print() { printf(" Hull\n"); - for (Vertex* v = minXy; v; ) + for (Vertex* v = minXy; v;) { printf(" "); v->print(); @@ -1362,7 +1363,7 @@ void btConvexHullInternal::IntermediateHull::print() } } if (minXy) - { + { minXy->copy = (minXy->copy == -1) ? -2 : -1; minXy->printGraph(); } @@ -1438,7 +1439,7 @@ btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const V Point32 t = *e->target - *start; Rational64 cot(t.dot(sxrxs), t.dot(rxs)); #ifdef DEBUG_CONVEX_HULL - printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN()); + printf(" Angle is %f (%d) for ", (float)btAtan(cot.toScalar()), (int)cot.isNaN()); e->print(); #endif if (cot.isNaN()) @@ -1485,7 +1486,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge btAssert(!start1 || (start1->target->point.dot(normal) == dist)); Point64 perp = s.cross(normal); btAssert(!perp.isZero()); - + #ifdef DEBUG_CONVEX_HULL printf(" 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 @@ -1515,7 +1516,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge et0 = e->target->point; } } - + int64_t maxDot1 = et1.dot(perp); if (e1) { @@ -1552,7 +1553,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge while (true) { int64_t dy = (et1 - et0).dot(s); - + if (e0 && (e0->target != stop0)) { Edge* f0 = e0->next->reverse; @@ -1569,7 +1570,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge } } } - + if (e1 && (e1->target != stop1)) { Edge* f1 = e1->reverse->next; @@ -1604,7 +1605,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge while (true) { int64_t dy = (et1 - et0).dot(s); - + if (e1 && (e1->target != stop1)) { Edge* f1 = e1->prev->reverse; @@ -1621,7 +1622,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge } } } - + if (e0 && (e0->target != stop0)) { Edge* f0 = e0->reverse->prev; @@ -1656,7 +1657,6 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge #endif } - void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) { if (!h1.maxXy) @@ -1668,7 +1668,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) h0 = h1; return; } - + mergeStamp--; Vertex* c0 = NULL; @@ -1708,7 +1708,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) e = e->next; } while (e != c0->edges); } - + e = c1->edges; Edge* start1 = NULL; if (e) @@ -1760,7 +1760,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) Point32 r = prevPoint - c0->point; Point64 rxs = r.cross(s); Point64 sxrxs = s.cross(rxs); - + #ifdef DEBUG_CONVEX_HULL printf("\n Checking %d %d\n", c0->point.index, c1->point.index); #endif @@ -1811,7 +1811,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) e->prev = pendingTail1; pendingTail1 = e; } - + Edge* e0 = min0; Edge* e1 = min1; @@ -1828,7 +1828,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) { if (toPrev1) { - for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n) + for (Edge *e = toPrev1->next, *n = NULL; e != min1; e = n) { n = e->next; removeEdgePair(e); @@ -1864,7 +1864,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) { if (toPrev0) { - for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n) + for (Edge *e = toPrev0->prev, *n = NULL; e != min0; e = n) { n = e->prev; removeEdgePair(e); @@ -1906,7 +1906,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) } else { - for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) + for (Edge *e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) { n = e->prev; removeEdgePair(e); @@ -1925,7 +1925,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) } else { - for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n) + for (Edge *e = toPrev1->next, *n = NULL; e != firstNew1; e = n) { n = e->next; removeEdgePair(e); @@ -1936,7 +1936,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) pendingTail1->link(firstNew1); } } - + return; } @@ -1946,24 +1946,23 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) class pointCmp { - public: - - bool operator() ( const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q ) const - { - return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z)))); - } +public: + bool operator()(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q) const + { + return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z)))); + } }; void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count) { btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30)); - const char* ptr = (const char*) coords; + const char* ptr = (const char*)coords; if (doubleCoords) { for (int i = 0; i < count; i++) { - const double* v = (const double*) ptr; - btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]); + const double* v = (const double*)ptr; + btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]); ptr += stride; min.setMin(p); max.setMax(p); @@ -1973,7 +1972,7 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st { for (int i = 0; i < count; i++) { - const float* v = (const float*) ptr; + const float* v = (const float*)ptr; btVector3 p(v[0], v[1], v[2]); ptr += stride; min.setMin(p); @@ -2014,18 +2013,18 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st btAlignedObjectArray points; points.resize(count); - ptr = (const char*) coords; + ptr = (const char*)coords; if (doubleCoords) { for (int i = 0; i < count; i++) { - const double* v = (const double*) ptr; - btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]); + const double* v = (const double*)ptr; + btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]); ptr += stride; p = (p - center) * s; - points[i].x = (int32_t) p[medAxis]; - points[i].y = (int32_t) p[maxAxis]; - points[i].z = (int32_t) p[minAxis]; + points[i].x = (int32_t)p[medAxis]; + points[i].y = (int32_t)p[maxAxis]; + points[i].z = (int32_t)p[minAxis]; points[i].index = i; } } @@ -2033,13 +2032,13 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st { for (int i = 0; i < count; i++) { - const float* v = (const float*) ptr; + const float* v = (const float*)ptr; btVector3 p(v[0], v[1], v[2]); ptr += stride; p = (p - center) * s; - points[i].x = (int32_t) p[medAxis]; - points[i].y = (int32_t) p[maxAxis]; - points[i].z = (int32_t) p[minAxis]; + points[i].x = (int32_t)p[medAxis]; + points[i].y = (int32_t)p[maxAxis]; + points[i].z = (int32_t)p[minAxis]; points[i].index = i; } } @@ -2193,7 +2192,7 @@ btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount) minDist = dist; } } - + if (minDist <= 0) { return 0; @@ -2234,7 +2233,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec { origShift[2] /= scaling[2]; } - Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]); + Point32 shift((int32_t)origShift[medAxis], (int32_t)origShift[maxAxis], (int32_t)origShift[minAxis]); if (shift.isZero()) { return true; @@ -2242,7 +2241,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec Point64 normal = face->getNormal(); #ifdef DEBUG_CONVEX_HULL printf("\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); + 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 int64_t origDot = face->origin.dot(normal); Point32 shiftedOrigin = face->origin + shift; @@ -2279,7 +2278,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec #ifdef DEBUG_CONVEX_HULL printf("Moving downwards, edge is "); e->print(); - printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot); + printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot); #endif if (dot.compare(optDot) < 0) { @@ -2315,7 +2314,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec #ifdef DEBUG_CONVEX_HULL printf("Moving upwards, edge is "); e->print(); - printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot); + printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot); #endif if (dot.compare(optDot) > 0) { @@ -2331,7 +2330,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec } e = e->prev; } while (e != startEdge); - + if (!intersection) { return true; @@ -2368,7 +2367,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec printf("Needed %d iterations to check for complete containment\n", n); #endif } - + Edge* firstIntersection = NULL; Edge* faceEdge = NULL; Edge* firstFaceEdge = NULL; @@ -2477,7 +2476,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec #ifdef DEBUG_CONVEX_HULL printf("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(); int64_t m00 = face->dir0.dot(n0); @@ -2491,16 +2490,13 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec 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 = (int32_t) v->point128.xvalue(); - v->point.y = (int32_t) v->point128.yvalue(); - v->point.z = (int32_t) v->point128.zvalue(); + 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 = (int32_t)v->point128.xvalue(); + v->point.y = (int32_t)v->point128.yvalue(); + v->point.z = (int32_t)v->point128.zvalue(); intersection->target = v; v->edges = e; @@ -2639,7 +2635,6 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec return true; } - static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray& vertices) { int index = vertex->copy; @@ -2761,8 +2756,3 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in return shift; } - - - - - diff --git a/Engine/lib/bullet/src/LinearMath/btConvexHullComputer.h b/Engine/lib/bullet/src/LinearMath/btConvexHullComputer.h index 7240ac4fb..cba684f2d 100644 --- a/Engine/lib/bullet/src/LinearMath/btConvexHullComputer.h +++ b/Engine/lib/bullet/src/LinearMath/btConvexHullComputer.h @@ -23,58 +23,56 @@ subject to the following restrictions: /// Ole Kniemeyer, MAXON Computer GmbH class btConvexHullComputer { +private: + btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp); + +public: + class Edge + { private: - btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp); + int next; + int reverse; + int targetVertex; + + friend class btConvexHullComputer; public: - - class Edge + int getSourceVertex() const { - private: - int next; - int reverse; - int targetVertex; + return (this + reverse)->targetVertex; + } - friend class btConvexHullComputer; + int getTargetVertex() const + { + return targetVertex; + } - public: - int getSourceVertex() const - { - return (this + reverse)->targetVertex; - } + const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex + { + return this + next; + } - int getTargetVertex() const - { - return targetVertex; - } + const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face + { + return (this + reverse)->getNextEdgeOfVertex(); + } - const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex - { - return this + next; - } + const Edge* getReverseEdge() const + { + return this + reverse; + } + }; - const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face - { - return (this + reverse)->getNextEdgeOfVertex(); - } + // Vertices of the output hull + btAlignedObjectArray vertices; - const Edge* getReverseEdge() const - { - return this + reverse; - } - }; + // Edges of the output hull + btAlignedObjectArray 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 + btAlignedObjectArray faces; - // Vertices of the output hull - btAlignedObjectArray vertices; - - // Edges of the output hull - btAlignedObjectArray 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 - btAlignedObjectArray 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). @@ -86,18 +84,16 @@ class btConvexHullComputer The output convex hull can be found in the member variables "vertices", "edges", "faces". */ - btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp) - { - return compute(coords, false, stride, count, shrink, shrinkClamp); - } + btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp) + { + return compute(coords, false, stride, count, shrink, shrinkClamp); + } - // same as above, but double precision - btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp) - { - return compute(coords, true, stride, count, shrink, shrinkClamp); - } + // same as above, but double precision + btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp) + { + return compute(coords, true, stride, count, shrink, shrinkClamp); + } }; - -#endif //BT_CONVEX_HULL_COMPUTER_H - +#endif //BT_CONVEX_HULL_COMPUTER_H diff --git a/Engine/lib/bullet/src/LinearMath/btCpuFeatureUtility.h b/Engine/lib/bullet/src/LinearMath/btCpuFeatureUtility.h index d2cab52d4..5e4b9a313 100644 --- a/Engine/lib/bullet/src/LinearMath/btCpuFeatureUtility.h +++ b/Engine/lib/bullet/src/LinearMath/btCpuFeatureUtility.h @@ -4,20 +4,20 @@ #include "LinearMath/btScalar.h" -#include //memset -#ifdef USE_SIMD +#include //memset +#ifdef USE_SIMD #include #ifdef BT_ALLOW_SSE4 #include -#endif //BT_ALLOW_SSE4 -#endif //USE_SIMD +#endif //BT_ALLOW_SSE4 +#endif //USE_SIMD #if defined BT_USE_NEON -#define ARM_NEON_GCC_COMPATIBILITY 1 +#define ARM_NEON_GCC_COMPATIBILITY 1 #include #include -#include //for sysctlbyname -#endif //BT_USE_NEON +#include //for sysctlbyname +#endif //BT_USE_NEON ///Rudimentary btCpuFeatureUtility for CPU features: only report the features that Bullet actually uses (SSE4/FMA3, NEON_HPFP) ///We assume SSE2 in case BT_USE_SSE2 is defined in LinearMath/btScalar.h @@ -26,14 +26,13 @@ class btCpuFeatureUtility public: enum btCpuFeature { - CPU_FEATURE_FMA3=1, - CPU_FEATURE_SSE4_1=2, - CPU_FEATURE_NEON_HPFP=4 + CPU_FEATURE_FMA3 = 1, + CPU_FEATURE_SSE4_1 = 2, + CPU_FEATURE_NEON_HPFP = 4 }; static int getCpuFeatures() { - static int capabilities = 0; static bool testedCapabilities = false; if (0 != testedCapabilities) @@ -49,15 +48,15 @@ public: if (0 == err && hasFeature) capabilities |= CPU_FEATURE_NEON_HPFP; } -#endif //BT_USE_NEON +#endif //BT_USE_NEON -#ifdef BT_ALLOW_SSE4 +#ifdef BT_ALLOW_SSE4 { - int cpuInfo[4]; + int cpuInfo[4]; memset(cpuInfo, 0, sizeof(cpuInfo)); - unsigned long long sseExt = 0; + unsigned long long sseExt = 0; __cpuid(cpuInfo, 1); - + bool osUsesXSAVE_XRSTORE = cpuInfo[2] & (1 << 27) || false; bool cpuAVXSuport = cpuInfo[2] & (1 << 28) || false; @@ -79,14 +78,11 @@ public: capabilities |= btCpuFeatureUtility::CPU_FEATURE_SSE4_1; } } -#endif//BT_ALLOW_SSE4 +#endif //BT_ALLOW_SSE4 testedCapabilities = true; return capabilities; } - - }; - -#endif //BT_CPU_UTILITY_H +#endif //BT_CPU_UTILITY_H diff --git a/Engine/lib/bullet/src/LinearMath/btDefaultMotionState.h b/Engine/lib/bullet/src/LinearMath/btDefaultMotionState.h index 01c5f8d93..14c40d36b 100644 --- a/Engine/lib/bullet/src/LinearMath/btDefaultMotionState.h +++ b/Engine/lib/bullet/src/LinearMath/btDefaultMotionState.h @@ -4,39 +4,37 @@ #include "btMotionState.h" ///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets. -ATTRIBUTE_ALIGNED16(struct) btDefaultMotionState : public btMotionState +ATTRIBUTE_ALIGNED16(struct) +btDefaultMotionState : public btMotionState { btTransform m_graphicsWorldTrans; - btTransform m_centerOfMassOffset; + btTransform m_centerOfMassOffset; btTransform m_startWorldTrans; - void* m_userPointer; + void* m_userPointer; BT_DECLARE_ALIGNED_ALLOCATOR(); - btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity()) + btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(), const btTransform& centerOfMassOffset = btTransform::getIdentity()) : m_graphicsWorldTrans(startTrans), - m_centerOfMassOffset(centerOfMassOffset), - m_startWorldTrans(startTrans), - m_userPointer(0) + m_centerOfMassOffset(centerOfMassOffset), + m_startWorldTrans(startTrans), + m_userPointer(0) { } ///synchronizes world transform from user to physics - virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const + virtual void getWorldTransform(btTransform & centerOfMassWorldTrans) const { - centerOfMassWorldTrans = m_graphicsWorldTrans * m_centerOfMassOffset.inverse() ; + centerOfMassWorldTrans = m_graphicsWorldTrans * m_centerOfMassOffset.inverse(); } ///synchronizes world transform from physics to user ///Bullet only calls the update of worldtransform for active objects - virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans) + virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans) { - m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset; + m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset; } - - - }; -#endif //BT_DEFAULT_MOTION_STATE_H +#endif //BT_DEFAULT_MOTION_STATE_H diff --git a/Engine/lib/bullet/src/LinearMath/btGeometryUtil.cpp b/Engine/lib/bullet/src/LinearMath/btGeometryUtil.cpp index 5ac230f71..115e3eab8 100644 --- a/Engine/lib/bullet/src/LinearMath/btGeometryUtil.cpp +++ b/Engine/lib/bullet/src/LinearMath/btGeometryUtil.cpp @@ -12,49 +12,43 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btGeometryUtil.h" - /* Make sure this dummy function never changes so that it can be used by probes that are checking whether the library is actually installed. */ extern "C" -{ - void btBulletMathProbe (); +{ + void btBulletMathProbe(); - void btBulletMathProbe () {} + void btBulletMathProbe() {} } - -bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin) +bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin) { int numbrushes = planeEquations.size(); - for (int i=0;ibtScalar(0.)) + btScalar dist = btScalar(N1.dot(point)) + btScalar(N1[3]) - margin; + if (dist > btScalar(0.)) { return false; } } return true; - } - -bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin) +bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin) { int numvertices = vertices.size(); - for (int i=0;ibtScalar(0.)) + btScalar dist = btScalar(planeNormal.dot(N1)) + btScalar(planeNormal[3]) - margin; + if (dist > btScalar(0.)) { return false; } @@ -62,102 +56,98 @@ bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const return true; } -bool notExist(const btVector3& planeEquation,const btAlignedObjectArray& planeEquations); +bool notExist(const btVector3& planeEquation, const btAlignedObjectArray& planeEquations); -bool notExist(const btVector3& planeEquation,const btAlignedObjectArray& planeEquations) +bool notExist(const btVector3& planeEquation, const btAlignedObjectArray& planeEquations) { int numbrushes = planeEquations.size(); - for (int i=0;i btScalar(0.999)) { return false; - } + } } return true; } -void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray& vertices, btAlignedObjectArray& planeEquationsOut ) +void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray& vertices, btAlignedObjectArray& planeEquationsOut) { - const int numvertices = vertices.size(); + const int numvertices = vertices.size(); // brute force: - for (int i=0;i btScalar(0.0001)) { planeEquation.normalize(); - if (notExist(planeEquation,planeEquationsOut)) + if (notExist(planeEquation, planeEquationsOut)) { planeEquation[3] = -planeEquation.dot(N1); - - //check if inside, and replace supportingVertexOut if needed - if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01))) - { - planeEquationsOut.push_back(planeEquation); - } + + //check if inside, and replace supportingVertexOut if needed + if (areVerticesBehindPlane(planeEquation, vertices, btScalar(0.01))) + { + planeEquationsOut.push_back(planeEquation); + } } } normalSign = btScalar(-1.); } - } } } - } -void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations , btAlignedObjectArray& verticesOut ) +void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations, btAlignedObjectArray& verticesOut) { const int numbrushes = planeEquations.size(); // brute force: - for (int i=0;i btScalar(0.0001) ) && - ( n3n1.length2() > btScalar(0.0001) ) && - ( n1n2.length2() > btScalar(0.0001) ) ) + btVector3 n2n3; + n2n3 = N2.cross(N3); + btVector3 n3n1; + n3n1 = N3.cross(N1); + btVector3 n1n2; + n1n2 = N1.cross(N2); + + if ((n2n3.length2() > btScalar(0.0001)) && + (n3n1.length2() > btScalar(0.0001)) && + (n1n2.length2() > btScalar(0.0001))) { //point P out of 3 plane equations: - // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) - //P = ------------------------------------------------------------------------- - // N1 . ( N2 * N3 ) - + // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) + //P = ------------------------------------------------------------------------- + // N1 . ( N2 * N3 ) btScalar quotient = (N1.dot(n2n3)); if (btFabs(quotient) > btScalar(0.000001)) @@ -172,7 +162,7 @@ void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray& vertices, btAlignedObjectArray& planeEquationsOut ); +public: + static void getPlaneEquationsFromVertices(btAlignedObjectArray& vertices, btAlignedObjectArray& planeEquationsOut); - static void getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations , btAlignedObjectArray& verticesOut ); - - static bool isInside(const btAlignedObjectArray& vertices, const btVector3& planeNormal, btScalar margin); - - static bool isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin); + static void getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations, btAlignedObjectArray& verticesOut); - static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin); + static bool isInside(const btAlignedObjectArray& vertices, const btVector3& planeNormal, btScalar margin); + static bool isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin); + + static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin); }; - -#endif //BT_GEOMETRY_UTIL_H - +#endif //BT_GEOMETRY_UTIL_H diff --git a/Engine/lib/bullet/src/LinearMath/btGrahamScan2dConvexHull.h b/Engine/lib/bullet/src/LinearMath/btGrahamScan2dConvexHull.h index 13a79aa58..0fcb28597 100644 --- a/Engine/lib/bullet/src/LinearMath/btGrahamScan2dConvexHull.h +++ b/Engine/lib/bullet/src/LinearMath/btGrahamScan2dConvexHull.h @@ -13,41 +13,40 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GRAHAM_SCAN_2D_CONVEX_HULL_H #define GRAHAM_SCAN_2D_CONVEX_HULL_H - #include "btVector3.h" #include "btAlignedObjectArray.h" struct GrahamVector3 : public btVector3 { GrahamVector3(const btVector3& org, int orgIndex) - :btVector3(org), - m_orgIndex(orgIndex) + : btVector3(org), + m_orgIndex(orgIndex) { } - btScalar m_angle; + btScalar m_angle; int m_orgIndex; }; - -struct btAngleCompareFunc { +struct btAngleCompareFunc +{ btVector3 m_anchor; btAngleCompareFunc(const btVector3& anchor) - : m_anchor(anchor) + : m_anchor(anchor) { } - bool operator()(const GrahamVector3& a, const GrahamVector3& b) const { + bool operator()(const GrahamVector3& a, const GrahamVector3& b) const + { if (a.m_angle != b.m_angle) return a.m_angle < b.m_angle; else { - btScalar al = (a-m_anchor).length2(); - btScalar bl = (b-m_anchor).length2(); + btScalar al = (a - m_anchor).length2(); + btScalar bl = (b - m_anchor).length2(); if (al != bl) - return al < bl; + return al < bl; else { return a.m_orgIndex < b.m_orgIndex; @@ -58,73 +57,73 @@ struct btAngleCompareFunc { inline void GrahamScanConvexHull2D(btAlignedObjectArray& originalPoints, btAlignedObjectArray& hull, const btVector3& normalAxis) { - btVector3 axis0,axis1; - btPlaneSpace1(normalAxis,axis0,axis1); - + btVector3 axis0, axis1; + btPlaneSpace1(normalAxis, axis0, axis1); - if (originalPoints.size()<=1) + if (originalPoints.size() <= 1) { - for (int i=0;i1) { - btVector3& a = hull[hull.size()-2]; - btVector3& b = hull[hull.size()-1]; - isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0; + while (!isConvex && hull.size() > 1) + { + btVector3& a = hull[hull.size() - 2]; + btVector3& b = hull[hull.size() - 1]; + isConvex = btCross(a - b, a - originalPoints[i]).dot(normalAxis) > 0; if (!isConvex) hull.pop_back(); - else + else hull.push_back(originalPoints[i]); } - if( hull.size() == 1 ) - { - hull.push_back( originalPoints[i] ); - } + if (hull.size() == 1) + { + hull.push_back(originalPoints[i]); + } } } -#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H +#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H diff --git a/Engine/lib/bullet/src/LinearMath/btHashMap.h b/Engine/lib/bullet/src/LinearMath/btHashMap.h index 5e9cdb605..1fca0fb73 100644 --- a/Engine/lib/bullet/src/LinearMath/btHashMap.h +++ b/Engine/lib/bullet/src/LinearMath/btHashMap.h @@ -13,87 +13,73 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #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; - unsigned int m_hash; + std::string m_string1; + unsigned int m_hash; - SIMD_FORCE_INLINE unsigned int getHash()const + SIMD_FORCE_INLINE unsigned int getHash() const { 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; + static const unsigned int InitialFNV = 2166136261u; static const unsigned int FNVMultiple = 16777619u; /* 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 * FNVMultiple; /* multiply by the magic number */ + 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 = *(const unsigned char *)src - *(const 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; - +const int BT_HASH_NULL = 0xffffffff; class btHashInt { - int m_uid; -public: + int m_uid; +public: btHashInt() { } - btHashInt(int uid) :m_uid(uid) + btHashInt(int uid) : m_uid(uid) { } - int getUid1() const + int getUid1() const { return m_uid; } - void setUid1(int uid) + void setUid1(int uid) { m_uid = uid; } @@ -103,35 +89,35 @@ public: return getUid1() == other.getUid1(); } //to our success - SIMD_FORCE_INLINE unsigned int getHash()const + SIMD_FORCE_INLINE unsigned int getHash() const { 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); - + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return key; } }; - - class btHashPtr { - - union - { - const void* m_pointer; - unsigned int m_hashValues[2]; + union { + const void* m_pointer; + unsigned int m_hashValues[2]; }; public: - btHashPtr(const void* ptr) - :m_pointer(ptr) + : m_pointer(ptr) { } - const void* getPointer() const + const void* getPointer() const { return m_pointer; } @@ -142,64 +128,68 @@ public: } //to our success - SIMD_FORCE_INLINE unsigned int getHash()const + SIMD_FORCE_INLINE unsigned int getHash() const { - const bool VOID_IS_8 = ((sizeof(void*)==8)); - - unsigned int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0]; + const bool VOID_IS_8 = ((sizeof(void*) == 8)); + + 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); + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); return key; } - - }; - template class btHashKeyPtr { - int m_uid; + int m_uid; + public: + btHashKeyPtr(int uid) : m_uid(uid) + { + } - btHashKeyPtr(int uid) :m_uid(uid) - { - } + int getUid1() const + { + return m_uid; + } - int getUid1() const - { - return m_uid; - } + bool equals(const btHashKeyPtr& other) const + { + return getUid1() == other.getUid1(); + } - bool equals(const btHashKeyPtr& other) const - { - return getUid1() == other.getUid1(); - } - - //to our success - SIMD_FORCE_INLINE unsigned int getHash()const - { - 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; - } - - + //to our success + SIMD_FORCE_INLINE unsigned int getHash() const + { + 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; + } }; - template class btHashKey { - int m_uid; -public: + int m_uid; - btHashKey(int uid) :m_uid(uid) +public: + btHashKey(int uid) : m_uid(uid) { } - int getUid1() const + int getUid1() const { return m_uid; } @@ -209,30 +199,33 @@ public: return getUid1() == other.getUid1(); } //to our success - SIMD_FORCE_INLINE unsigned int getHash()const + SIMD_FORCE_INLINE unsigned int getHash() const { 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); + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); return key; } }; - ///The btHashMap template class implements a generic and lightweight hashmap. ///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp template class btHashMap { - protected: - btAlignedObjectArray m_hashTable; - btAlignedObjectArray m_next; - - btAlignedObjectArray m_valueArray; - btAlignedObjectArray m_keyArray; + btAlignedObjectArray m_hashTable; + btAlignedObjectArray m_next; - void growTables(const Key& /*key*/) + btAlignedObjectArray m_valueArray; + btAlignedObjectArray m_keyArray; + + void growTables(const Key& /*key*/) { int newCapacity = m_valueArray.capacity(); @@ -246,7 +239,7 @@ protected: int i; - for (i= 0; i < newCapacity; ++i) + for (i = 0; i < newCapacity; ++i) { m_hashTable[i] = BT_HASH_NULL; } @@ -255,30 +248,28 @@ protected: m_next[i] = BT_HASH_NULL; } - for(i=0;i=0); - if (index>=0 && index < m_valueArray.size()) + btAssert(index >= 0); + if (index >= 0 && index < m_valueArray.size()) { return &m_valueArray[index]; } @@ -400,38 +389,39 @@ protected: Value* getAtIndex(int index) { btAssert(index < m_valueArray.size()); - btAssert(index>=0); - if (index>=0 && index < m_valueArray.size()) + 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); + 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); + } + + const Key getKeyAtIndex(int index) const + { + btAssert(index < m_keyArray.size()); + btAssert(index >= 0); return m_keyArray[index]; - } + } - - Value* operator[](const Key& key) { + Value* operator[](const Key& key) + { return find(key); } - const Value* operator[](const Key& key) const { + const Value* operator[](const Key& key) const + { return find(key); } - const Value* find(const Key& key) const + const Value* find(const Key& key) const { int index = findIndex(key); if (index == BT_HASH_NULL) @@ -441,7 +431,7 @@ protected: return &m_valueArray[index]; } - Value* find(const Key& key) + Value* find(const Key& key) { int index = findIndex(key); if (index == BT_HASH_NULL) @@ -451,10 +441,9 @@ protected: return &m_valueArray[index]; } - - int findIndex(const Key& key) const + int findIndex(const Key& key) const { - unsigned int hash = key.getHash() & (m_valueArray.capacity()-1); + unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1); if (hash >= (unsigned int)m_hashTable.size()) { @@ -469,14 +458,13 @@ protected: return index; } - void clear() + void clear() { m_hashTable.clear(); m_next.clear(); m_valueArray.clear(); m_keyArray.clear(); } - }; -#endif //BT_HASH_MAP_H +#endif //BT_HASH_MAP_H diff --git a/Engine/lib/bullet/src/LinearMath/btIDebugDraw.h b/Engine/lib/bullet/src/LinearMath/btIDebugDraw.h index 936aaa896..82ec19a69 100644 --- a/Engine/lib/bullet/src/LinearMath/btIDebugDraw.h +++ b/Engine/lib/bullet/src/LinearMath/btIDebugDraw.h @@ -13,86 +13,84 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_IDEBUG_DRAW__H #define BT_IDEBUG_DRAW__H #include "btVector3.h" #include "btTransform.h" - - ///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations. ///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld. ///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum. ///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1] -class btIDebugDraw +class btIDebugDraw { - public: - - ATTRIBUTE_ALIGNED16(struct) DefaultColors +public: + ATTRIBUTE_ALIGNED16(struct) + DefaultColors { - btVector3 m_activeObject; - btVector3 m_deactivatedObject; - btVector3 m_wantsDeactivationObject; - btVector3 m_disabledDeactivationObject; - btVector3 m_disabledSimulationObject; - btVector3 m_aabb; + btVector3 m_activeObject; + btVector3 m_deactivatedObject; + btVector3 m_wantsDeactivationObject; + btVector3 m_disabledDeactivationObject; + btVector3 m_disabledSimulationObject; + btVector3 m_aabb; btVector3 m_contactPoint; - + DefaultColors() - : m_activeObject(1,1,1), - m_deactivatedObject(0,1,0), - m_wantsDeactivationObject(0,1,1), - m_disabledDeactivationObject(1,0,0), - m_disabledSimulationObject(1,1,0), - m_aabb(1,0,0), - m_contactPoint(1,1,0) + : m_activeObject(1, 1, 1), + m_deactivatedObject(0, 1, 0), + m_wantsDeactivationObject(0, 1, 1), + m_disabledDeactivationObject(1, 0, 0), + m_disabledSimulationObject(1, 1, 0), + m_aabb(1, 0, 0), + m_contactPoint(1, 1, 0) { } }; - - enum DebugDrawModes + enum DebugDrawModes { - DBG_NoDebug=0, + DBG_NoDebug = 0, DBG_DrawWireframe = 1, - DBG_DrawAabb=2, - DBG_DrawFeaturesText=4, - DBG_DrawContactPoints=8, - DBG_NoDeactivation=16, + DBG_DrawAabb = 2, + DBG_DrawFeaturesText = 4, + DBG_DrawContactPoints = 8, + DBG_NoDeactivation = 16, DBG_NoHelpText = 32, - DBG_DrawText=64, + DBG_DrawText = 64, DBG_ProfileTimings = 128, DBG_EnableSatComparison = 256, DBG_DisableBulletLCP = 512, DBG_EnableCCD = 1024, DBG_DrawConstraints = (1 << 11), DBG_DrawConstraintLimits = (1 << 12), - DBG_FastWireframe = (1<<13), - DBG_DrawNormals = (1<<14), - DBG_DrawFrames = (1<<15), + DBG_FastWireframe = (1 << 13), + DBG_DrawNormals = (1 << 14), + DBG_DrawFrames = (1 << 15), DBG_MAX_DEBUG_DRAW_MODE }; - virtual ~btIDebugDraw() {}; + virtual ~btIDebugDraw(){}; - - virtual DefaultColors getDefaultColors() const { DefaultColors colors; return colors; } + virtual DefaultColors getDefaultColors() const + { + DefaultColors colors; + return colors; + } ///the default implementation for setDefaultColors has no effect. A derived class can implement it and store the colors. virtual void setDefaultColors(const DefaultColors& /*colors*/) {} - - virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0; - - virtual void drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor) + + virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& color) = 0; + + virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& fromColor, const btVector3& toColor) { - (void) toColor; - drawLine (from, to, fromColor); + (void)toColor; + drawLine(from, to, fromColor); } - virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color) + virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color) { - btVector3 center = transform.getOrigin(); btVector3 up = transform.getBasis().getColumn(1); btVector3 axis = transform.getBasis().getColumn(0); @@ -101,103 +99,102 @@ class btIDebugDraw btScalar minPs = -SIMD_HALF_PI; btScalar maxPs = SIMD_HALF_PI; btScalar stepDegrees = 30.f; - drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees ,false); - drawSpherePatch(center, up, -axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees,false ); + drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, stepDegrees, false); + drawSpherePatch(center, up, -axis, radius, minTh, maxTh, minPs, maxPs, color, stepDegrees, false); } - - virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color) + + virtual void drawSphere(const btVector3& p, btScalar radius, const btVector3& color) { btTransform tr; tr.setIdentity(); tr.setOrigin(p); - drawSphere(radius,tr,color); - } - - virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha) - { - drawTriangle(v0,v1,v2,color,alpha); - } - virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& color, btScalar /*alpha*/) - { - drawLine(v0,v1,color); - drawLine(v1,v2,color); - drawLine(v2,v0,color); + drawSphere(radius, tr, color); } - virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0; - - virtual void reportErrorWarning(const char* warningString) = 0; - - virtual void draw3dText(const btVector3& location,const char* textString) = 0; - - virtual void setDebugMode(int debugMode) =0; - - virtual int getDebugMode() const = 0; - - virtual void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color) + virtual void drawTriangle(const btVector3& v0, const btVector3& v1, const btVector3& v2, const btVector3& /*n0*/, const btVector3& /*n1*/, const btVector3& /*n2*/, const btVector3& color, btScalar alpha) { + drawTriangle(v0, v1, v2, color, alpha); + } + virtual void drawTriangle(const btVector3& v0, const btVector3& v1, const btVector3& v2, const btVector3& color, btScalar /*alpha*/) + { + drawLine(v0, v1, color); + drawLine(v1, v2, color); + drawLine(v2, v0, color); + } - btVector3 halfExtents = (to-from)* 0.5f; - btVector3 center = (to+from) *0.5f; - int i,j; + virtual void drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance, int lifeTime, const btVector3& color) = 0; - btVector3 edgecoord(1.f,1.f,1.f),pa,pb; - for (i=0;i<4;i++) + virtual void reportErrorWarning(const char* warningString) = 0; + + virtual void draw3dText(const btVector3& location, const char* textString) = 0; + + virtual void setDebugMode(int debugMode) = 0; + + virtual int getDebugMode() const = 0; + + virtual void drawAabb(const btVector3& from, const btVector3& to, const btVector3& color) + { + btVector3 halfExtents = (to - from) * 0.5f; + btVector3 center = (to + from) * 0.5f; + int i, j; + + btVector3 edgecoord(1.f, 1.f, 1.f), pa, pb; + for (i = 0; i < 4; i++) { - for (j=0;j<3;j++) + for (j = 0; j < 3; j++) { - pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], - edgecoord[2]*halfExtents[2]); - pa+=center; + pa = btVector3(edgecoord[0] * halfExtents[0], edgecoord[1] * halfExtents[1], + edgecoord[2] * halfExtents[2]); + pa += center; - int othercoord = j%3; - edgecoord[othercoord]*=-1.f; - pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], - edgecoord[2]*halfExtents[2]); - pb+=center; + int othercoord = j % 3; + edgecoord[othercoord] *= -1.f; + pb = btVector3(edgecoord[0] * halfExtents[0], edgecoord[1] * halfExtents[1], + edgecoord[2] * halfExtents[2]); + pb += center; - drawLine(pa,pb,color); + drawLine(pa, pb, color); } - edgecoord = btVector3(-1.f,-1.f,-1.f); - if (i<3) - edgecoord[i]*=-1.f; + edgecoord = btVector3(-1.f, -1.f, -1.f); + if (i < 3) + edgecoord[i] *= -1.f; } } virtual void drawTransform(const btTransform& transform, btScalar orthoLen) { btVector3 start = transform.getOrigin(); - 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)); + drawLine(start, start + transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(btScalar(1.), btScalar(0.3), btScalar(0.3))); + drawLine(start, start + transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(btScalar(0.3), btScalar(1.), btScalar(0.3))); + drawLine(start, start + transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(btScalar(0.3), btScalar(0.3), btScalar(1.))); } - virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, - const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f)) + virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, + const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f)) { const btVector3& vx = axis; btVector3 vy = normal.cross(axis); btScalar step = stepDegrees * SIMD_RADS_PER_DEG; int nSteps = (int)btFabs((maxAngle - minAngle) / step); - if(!nSteps) nSteps = 1; + if (!nSteps) nSteps = 1; btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle); - if(drawSect) + if (drawSect) { drawLine(center, prev, color); } - for(int i = 1; i <= nSteps; i++) + for (int i = 1; i <= nSteps; i++) { btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps); btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle); drawLine(prev, next, color); prev = next; } - if(drawSect) + if (drawSect) { drawLine(center, prev, color); } } - virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, - btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f),bool drawCenter = true) + virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, + btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f), bool drawCenter = true) { btVector3 vA[74]; btVector3 vB[74]; @@ -211,33 +208,33 @@ class btIDebugDraw btVector3 jv = kv.cross(iv); bool drawN = false; bool drawS = false; - if(minTh <= -SIMD_HALF_PI) + if (minTh <= -SIMD_HALF_PI) { minTh = -SIMD_HALF_PI + step; drawN = true; } - if(maxTh >= SIMD_HALF_PI) + if (maxTh >= SIMD_HALF_PI) { maxTh = SIMD_HALF_PI - step; drawS = true; } - if(minTh > maxTh) + if (minTh > maxTh) { minTh = -SIMD_HALF_PI + step; - maxTh = SIMD_HALF_PI - step; + maxTh = SIMD_HALF_PI - step; drawN = drawS = true; } int n_hor = (int)((maxTh - minTh) / step) + 1; - if(n_hor < 2) n_hor = 2; + if (n_hor < 2) n_hor = 2; btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1); bool isClosed = false; - if(minPs > maxPs) + if (minPs > maxPs) { minPs = -SIMD_PI + step; - maxPs = SIMD_PI; + maxPs = SIMD_PI; isClosed = true; } - else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f)) + else if ((maxPs - minPs) >= SIMD_PI * btScalar(2.f)) { isClosed = true; } @@ -246,63 +243,64 @@ class btIDebugDraw isClosed = false; } int n_vert = (int)((maxPs - minPs) / step) + 1; - if(n_vert < 2) n_vert = 2; + if (n_vert < 2) n_vert = 2; btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1); - for(int i = 0; i < n_hor; i++) + for (int i = 0; i < n_hor; i++) { btScalar th = minTh + btScalar(i) * step_h; btScalar sth = radius * btSin(th); btScalar cth = radius * btCos(th); - for(int j = 0; j < n_vert; j++) + for (int j = 0; j < n_vert; j++) { btScalar psi = minPs + btScalar(j) * step_v; btScalar sps = btSin(psi); btScalar cps = btCos(psi); pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv; - if(i) + if (i) { drawLine(pvA[j], pvB[j], color); } - else if(drawS) + else if (drawS) { drawLine(spole, pvB[j], color); } - if(j) + if (j) { - drawLine(pvB[j-1], pvB[j], color); + drawLine(pvB[j - 1], pvB[j], color); } else { arcStart = pvB[j]; } - if((i == (n_hor - 1)) && drawN) + if ((i == (n_hor - 1)) && drawN) { drawLine(npole, pvB[j], color); } - + if (drawCenter) { - if(isClosed) + if (isClosed) { - if(j == (n_vert-1)) + if (j == (n_vert - 1)) { drawLine(arcStart, pvB[j], color); } } else { - if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1)))) + if (((!i) || (i == (n_hor - 1))) && ((!j) || (j == (n_vert - 1)))) { drawLine(center, pvB[j], color); } } } } - pT = pvA; pvA = pvB; pvB = pT; + pT = pvA; + pvA = pvB; + pvB = pT; } } - - + virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color) { drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color); @@ -338,31 +336,27 @@ class btIDebugDraw { int stepDegrees = 30; - btVector3 capStart(0.f,0.f,0.f); + btVector3 capStart(0.f, 0.f, 0.f); capStart[upAxis] = -halfHeight; - btVector3 capEnd(0.f,0.f,0.f); + btVector3 capEnd(0.f, 0.f, 0.f); capEnd[upAxis] = halfHeight; // Draw the ends { - btTransform childTransform = transform; childTransform.getOrigin() = transform * capStart; { btVector3 center = childTransform.getOrigin(); - btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3); + btVector3 up = childTransform.getBasis().getColumn((upAxis + 1) % 3); btVector3 axis = -childTransform.getBasis().getColumn(upAxis); btScalar minTh = -SIMD_HALF_PI; btScalar maxTh = SIMD_HALF_PI; btScalar minPs = -SIMD_HALF_PI; btScalar maxPs = SIMD_HALF_PI; - - drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false); + + drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees), false); } - - - } { @@ -370,52 +364,51 @@ class btIDebugDraw childTransform.getOrigin() = transform * capEnd; { btVector3 center = childTransform.getOrigin(); - btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3); + btVector3 up = childTransform.getBasis().getColumn((upAxis + 1) % 3); btVector3 axis = childTransform.getBasis().getColumn(upAxis); btScalar minTh = -SIMD_HALF_PI; btScalar maxTh = SIMD_HALF_PI; btScalar minPs = -SIMD_HALF_PI; btScalar maxPs = SIMD_HALF_PI; - drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false); + drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees), false); } } // Draw some additional lines btVector3 start = transform.getOrigin(); - for (int i=0;i<360;i+=stepDegrees) + for (int i = 0; i < 360; i += stepDegrees) { - capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius; - capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius; - drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); + capEnd[(upAxis + 1) % 3] = capStart[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius; + capEnd[(upAxis + 2) % 3] = capStart[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius; + drawLine(start + transform.getBasis() * capStart, start + transform.getBasis() * capEnd, color); } - } virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) { btVector3 start = transform.getOrigin(); - btVector3 offsetHeight(0,0,0); + btVector3 offsetHeight(0, 0, 0); offsetHeight[upAxis] = halfHeight; - int stepDegrees=30; - btVector3 capStart(0.f,0.f,0.f); + int stepDegrees = 30; + btVector3 capStart(0.f, 0.f, 0.f); capStart[upAxis] = -halfHeight; - btVector3 capEnd(0.f,0.f,0.f); + btVector3 capEnd(0.f, 0.f, 0.f); capEnd[upAxis] = halfHeight; - for (int i=0;i<360;i+=stepDegrees) + for (int i = 0; i < 360; i += stepDegrees) { - capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius; - capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius; - drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); + capEnd[(upAxis + 1) % 3] = capStart[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius; + capEnd[(upAxis + 2) % 3] = capStart[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius; + drawLine(start + transform.getBasis() * capStart, start + transform.getBasis() * capEnd, color); } // Drawing top and bottom caps of the cylinder - btVector3 yaxis(0,0,0); + btVector3 yaxis(0, 0, 0); yaxis[upAxis] = btScalar(1.0); - btVector3 xaxis(0,0,0); - xaxis[(upAxis+1)%3] = btScalar(1.0); - drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0)); - drawArc(start+transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0)); + btVector3 xaxis(0, 0, 0); + xaxis[(upAxis + 1) % 3] = btScalar(1.0); + drawArc(start - transform.getBasis() * (offsetHeight), transform.getBasis() * yaxis, transform.getBasis() * xaxis, radius, radius, 0, SIMD_2_PI, color, false, btScalar(10.0)); + drawArc(start + transform.getBasis() * (offsetHeight), transform.getBasis() * yaxis, transform.getBasis() * xaxis, radius, radius, 0, SIMD_2_PI, color, false, btScalar(10.0)); } virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color) @@ -423,50 +416,49 @@ class btIDebugDraw int stepDegrees = 30; btVector3 start = transform.getOrigin(); - btVector3 offsetHeight(0,0,0); + btVector3 offsetHeight(0, 0, 0); btScalar halfHeight = height * btScalar(0.5); offsetHeight[upAxis] = halfHeight; - btVector3 offsetRadius(0,0,0); - offsetRadius[(upAxis+1)%3] = radius; - btVector3 offset2Radius(0,0,0); - offset2Radius[(upAxis+2)%3] = radius; + btVector3 offsetRadius(0, 0, 0); + offsetRadius[(upAxis + 1) % 3] = radius; + btVector3 offset2Radius(0, 0, 0); + offset2Radius[(upAxis + 2) % 3] = radius; - - btVector3 capEnd(0.f,0.f,0.f); + btVector3 capEnd(0.f, 0.f, 0.f); capEnd[upAxis] = -halfHeight; - for (int i=0;i<360;i+=stepDegrees) + for (int i = 0; i < 360; i += stepDegrees) { - capEnd[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius; - capEnd[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius; - drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * capEnd, color); + capEnd[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius; + capEnd[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius; + drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * capEnd, color); } - drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offsetRadius),color); - drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offsetRadius),color); - drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offset2Radius),color); - drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offset2Radius),color); + drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight + offsetRadius), color); + drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight - offsetRadius), color); + drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight + offset2Radius), color); + drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight - offset2Radius), color); // Drawing the base of the cone - btVector3 yaxis(0,0,0); + btVector3 yaxis(0, 0, 0); yaxis[upAxis] = btScalar(1.0); - btVector3 xaxis(0,0,0); - xaxis[(upAxis+1)%3] = btScalar(1.0); - drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0); + btVector3 xaxis(0, 0, 0); + xaxis[(upAxis + 1) % 3] = btScalar(1.0); + drawArc(start - transform.getBasis() * (offsetHeight), transform.getBasis() * yaxis, transform.getBasis() * xaxis, radius, radius, 0, SIMD_2_PI, color, false, 10.0); } virtual void drawPlane(const btVector3& planeNormal, btScalar planeConst, const btTransform& transform, const btVector3& color) { btVector3 planeOrigin = planeNormal * planeConst; - btVector3 vec0,vec1; - btPlaneSpace1(planeNormal,vec0,vec1); + btVector3 vec0, vec1; + btPlaneSpace1(planeNormal, vec0, vec1); btScalar vecLen = 100.f; - btVector3 pt0 = planeOrigin + vec0*vecLen; - btVector3 pt1 = planeOrigin - vec0*vecLen; - btVector3 pt2 = planeOrigin + vec1*vecLen; - btVector3 pt3 = planeOrigin - vec1*vecLen; - drawLine(transform*pt0,transform*pt1,color); - drawLine(transform*pt2,transform*pt3,color); + btVector3 pt0 = planeOrigin + vec0 * vecLen; + btVector3 pt1 = planeOrigin - vec0 * vecLen; + btVector3 pt2 = planeOrigin + vec1 * vecLen; + btVector3 pt3 = planeOrigin - vec1 * vecLen; + drawLine(transform * pt0, transform * pt1, color); + drawLine(transform * pt2, transform * pt3, color); } virtual void clearLines() @@ -478,6 +470,4 @@ class btIDebugDraw } }; - -#endif //BT_IDEBUG_DRAW__H - +#endif //BT_IDEBUG_DRAW__H diff --git a/Engine/lib/bullet/src/LinearMath/btList.h b/Engine/lib/bullet/src/LinearMath/btList.h index eec80a706..b255938c3 100644 --- a/Engine/lib/bullet/src/LinearMath/btList.h +++ b/Engine/lib/bullet/src/LinearMath/btList.h @@ -12,62 +12,62 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_GEN_LIST_H #define BT_GEN_LIST_H -class btGEN_Link { +class btGEN_Link +{ public: - btGEN_Link() : m_next(0), m_prev(0) {} - btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {} - - btGEN_Link *getNext() const { return m_next; } - btGEN_Link *getPrev() const { return m_prev; } + btGEN_Link() : m_next(0), m_prev(0) {} + btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {} - bool isHead() const { return m_prev == 0; } - bool isTail() const { return m_next == 0; } + btGEN_Link *getNext() const { return m_next; } + btGEN_Link *getPrev() const { return m_prev; } - void insertBefore(btGEN_Link *link) { - m_next = link; - m_prev = link->m_prev; - m_next->m_prev = this; - m_prev->m_next = this; - } + bool isHead() const { return m_prev == 0; } + bool isTail() const { return m_next == 0; } - void insertAfter(btGEN_Link *link) { - m_next = link->m_next; - m_prev = link; - m_next->m_prev = this; - m_prev->m_next = this; - } + void insertBefore(btGEN_Link *link) + { + m_next = link; + m_prev = link->m_prev; + m_next->m_prev = this; + m_prev->m_next = this; + } - void remove() { - m_next->m_prev = m_prev; - m_prev->m_next = m_next; - } + void insertAfter(btGEN_Link *link) + { + m_next = link->m_next; + m_prev = link; + m_next->m_prev = this; + m_prev->m_next = this; + } -private: - btGEN_Link *m_next; - btGEN_Link *m_prev; -}; + void remove() + { + m_next->m_prev = m_prev; + m_prev->m_next = m_next; + } -class btGEN_List { -public: - btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {} - - btGEN_Link *getHead() const { return m_head.getNext(); } - btGEN_Link *getTail() const { return m_tail.getPrev(); } - - void addHead(btGEN_Link *link) { link->insertAfter(&m_head); } - void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); } - private: - btGEN_Link m_head; - btGEN_Link m_tail; + btGEN_Link *m_next; + btGEN_Link *m_prev; }; -#endif //BT_GEN_LIST_H +class btGEN_List +{ +public: + btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {} + btGEN_Link *getHead() const { return m_head.getNext(); } + btGEN_Link *getTail() const { return m_tail.getPrev(); } + void addHead(btGEN_Link *link) { link->insertAfter(&m_head); } + void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); } +private: + btGEN_Link m_head; + btGEN_Link m_tail; +}; + +#endif //BT_GEN_LIST_H diff --git a/Engine/lib/bullet/src/LinearMath/btMatrix3x3.h b/Engine/lib/bullet/src/LinearMath/btMatrix3x3.h index 9f642a177..0a08ae409 100644 --- a/Engine/lib/bullet/src/LinearMath/btMatrix3x3.h +++ b/Engine/lib/bullet/src/LinearMath/btMatrix3x3.h @@ -12,8 +12,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - -#ifndef BT_MATRIX3x3_H +#ifndef BT_MATRIX3x3_H #define BT_MATRIX3x3_H #include "btVector3.h" @@ -23,13 +22,13 @@ subject to the following restrictions: #ifdef BT_USE_SSE //const __m128 ATTRIBUTE_ALIGNED16(v2220) = {2.0f, 2.0f, 2.0f, 0.0f}; //const __m128 ATTRIBUTE_ALIGNED16(vMPPP) = {-0.0f, +0.0f, +0.0f, +0.0f}; -#define vMPPP (_mm_set_ps (+0.0f, +0.0f, +0.0f, -0.0f)) +#define vMPPP (_mm_set_ps(+0.0f, +0.0f, +0.0f, -0.0f)) #endif #if defined(BT_USE_SSE) -#define v1000 (_mm_set_ps(0.0f,0.0f,0.0f,1.0f)) -#define v0100 (_mm_set_ps(0.0f,0.0f,1.0f,0.0f)) -#define v0010 (_mm_set_ps(0.0f,1.0f,0.0f,0.0f)) +#define v1000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 1.0f)) +#define v0100 (_mm_set_ps(0.0f, 0.0f, 1.0f, 0.0f)) +#define v0010 (_mm_set_ps(0.0f, 1.0f, 0.0f, 0.0f)) #elif defined(BT_USE_NEON) const btSimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f}; const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f}; @@ -37,22 +36,22 @@ const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f}; #endif #ifdef BT_USE_DOUBLE_PRECISION -#define btMatrix3x3Data btMatrix3x3DoubleData +#define btMatrix3x3Data btMatrix3x3DoubleData #else -#define btMatrix3x3Data btMatrix3x3FloatData -#endif //BT_USE_DOUBLE_PRECISION - +#define btMatrix3x3Data btMatrix3x3FloatData +#endif //BT_USE_DOUBLE_PRECISION /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3. * Make sure to only include a pure orthogonal matrix without scaling. */ -ATTRIBUTE_ALIGNED16(class) btMatrix3x3 { - +ATTRIBUTE_ALIGNED16(class) +btMatrix3x3 +{ ///Data storage for the matrix, each vector is a row of the matrix btVector3 m_el[3]; public: /** @brief No initializaion constructor */ - btMatrix3x3 () {} + btMatrix3x3() {} // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } @@ -67,27 +66,27 @@ public: */ /** @brief Constructor with row major formatting */ btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, - const btScalar& yx, const btScalar& yy, const btScalar& yz, - const btScalar& zx, const btScalar& zy, const btScalar& zz) - { - setValue(xx, xy, xz, - yx, yy, yz, - zx, zy, zz); + const btScalar& yx, const btScalar& yy, const btScalar& yz, + const btScalar& zx, const btScalar& zy, const btScalar& zz) + { + setValue(xx, xy, xz, + yx, yy, yz, + zx, zy, zz); } -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON) - SIMD_FORCE_INLINE btMatrix3x3 (const btSimdFloat4 v0, const btSimdFloat4 v1, const btSimdFloat4 v2 ) +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) + SIMD_FORCE_INLINE btMatrix3x3(const btSimdFloat4 v0, const btSimdFloat4 v1, const btSimdFloat4 v2) { - m_el[0].mVec128 = v0; - m_el[1].mVec128 = v1; - m_el[2].mVec128 = v2; + m_el[0].mVec128 = v0; + m_el[1].mVec128 = v1; + m_el[2].mVec128 = v2; } - SIMD_FORCE_INLINE btMatrix3x3 (const btVector3& v0, const btVector3& v1, const btVector3& v2 ) + SIMD_FORCE_INLINE btMatrix3x3(const btVector3& v0, const btVector3& v1, const btVector3& v2) { - m_el[0] = v0; - m_el[1] = v1; - m_el[2] = v2; + m_el[0] = v0; + m_el[1] = v1; + m_el[2] = v2; } // Copy constructor @@ -99,25 +98,25 @@ public: } // Assignment Operator - SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& m) + SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& 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 */ - SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) + SIMD_FORCE_INLINE btMatrix3x3(const btMatrix3x3& other) { m_el[0] = other.m_el[0]; m_el[1] = other.m_el[1]; m_el[2] = other.m_el[2]; } - + /** @brief Assignment Operator */ SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) { @@ -133,10 +132,9 @@ public: * @param i Column number 0 indexed */ SIMD_FORCE_INLINE btVector3 getColumn(int i) const { - return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); + return btVector3(m_el[0][i], m_el[1][i], m_el[2][i]); } - /** @brief Get a row of the matrix as a vector * @param i Row number 0 indexed */ SIMD_FORCE_INLINE const btVector3& getRow(int i) const @@ -147,10 +145,10 @@ public: /** @brief Get a mutable reference to a row of the matrix as a vector * @param i Row number 0 indexed */ - SIMD_FORCE_INLINE btVector3& operator[](int i) - { + SIMD_FORCE_INLINE btVector3& operator[](int i) + { btFullAssert(0 <= i && i < 3); - return m_el[i]; + return m_el[i]; } /** @brief Get a const reference to a row of the matrix as a vector @@ -158,32 +156,31 @@ public: SIMD_FORCE_INLINE const btVector3& operator[](int i) const { btFullAssert(0 <= i && i < 3); - return m_el[i]; + return m_el[i]; } /** @brief Multiply by the target matrix on the right * @param m Rotation matrix to be applied * Equivilant to this = this * m */ - btMatrix3x3& operator*=(const btMatrix3x3& m); + btMatrix3x3& operator*=(const btMatrix3x3& m); /** @brief Adds by the target matrix on the right * @param m matrix to be applied * Equivilant to this = this + m */ - btMatrix3x3& operator+=(const btMatrix3x3& m); + btMatrix3x3& operator+=(const btMatrix3x3& m); /** @brief Substractss by the target matrix on the right * @param m matrix to be applied * Equivilant to this = this - m */ - btMatrix3x3& operator-=(const btMatrix3x3& m); + btMatrix3x3& operator-=(const btMatrix3x3& 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 btScalar *m) + void setFromOpenGLSubMatrix(const btScalar* 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]); - + 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 @@ -195,93 +192,92 @@ public: * @param zx Bottom Left * @param zy Bottom Middle * @param zz Bottom Right*/ - void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, - const btScalar& yx, const btScalar& yy, const btScalar& yz, - const btScalar& zx, const btScalar& zy, const btScalar& zz) + void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, + const btScalar& yx, const btScalar& yy, const btScalar& yz, + const btScalar& zx, const btScalar& zy, const btScalar& zz) { - m_el[0].setValue(xx,xy,xz); - m_el[1].setValue(yx,yy,yz); - m_el[2].setValue(zx,zy,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 btQuaternion& q) + * @param q The Quaternion to match */ + void setRotation(const btQuaternion& q) { btScalar d = q.length2(); btFullAssert(d != btScalar(0.0)); btScalar s = btScalar(2.0) / d; - - #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - __m128 vs, Q = q.get128(); + +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) + __m128 vs, Q = q.get128(); __m128i Qi = btCastfTo128i(Q); - __m128 Y, Z; - __m128 V1, V2, V3; - __m128 V11, V21, V31; - __m128 NQ = _mm_xor_ps(Q, btvMzeroMask); + __m128 Y, Z; + __m128 V1, V2, V3; + __m128 V11, V21, V31; + __m128 NQ = _mm_xor_ps(Q, btvMzeroMask); __m128i NQi = btCastfTo128i(NQ); - - V1 = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,0,2,3))); // Y X Z W - V2 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(0,0,1,3)); // -X -X Y W - V3 = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(2,1,0,3))); // Z Y X W - V1 = _mm_xor_ps(V1, vMPPP); // change the sign of the first element - - V11 = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_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, BT_SHUFFLE(0,2,0,3)); // X Z -X -W - V2 = V2 * V1; // - V1 = V1 * V11; // - V3 = V3 * V31; // + V1 = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_SHUFFLE(1, 0, 2, 3))); // Y X Z W + V2 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(0, 0, 1, 3)); // -X -X Y W + V3 = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_SHUFFLE(2, 1, 0, 3))); // Z Y X W + V1 = _mm_xor_ps(V1, vMPPP); // change the sign of the first element - V11 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(2,3,1,3)); // -Z -W Y W - V11 = V11 * V21; // - V21 = _mm_xor_ps(V21, vMPPP); // change the sign of the first element - V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(3,3,1,3)); // W W -Y -W - V31 = _mm_xor_ps(V31, vMPPP); // change the sign of the first element - Y = btCastiTo128f(_mm_shuffle_epi32 (NQi, BT_SHUFFLE(3,2,0,3))); // -W -Z -X -W - Z = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,0,1,3))); // Y X Y W + V11 = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_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, BT_SHUFFLE(0, 2, 0, 3)); // X Z -X -W + + V2 = V2 * V1; // + V1 = V1 * V11; // + V3 = V3 * V31; // + + V11 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(2, 3, 1, 3)); // -Z -W Y W + V11 = V11 * V21; // + V21 = _mm_xor_ps(V21, vMPPP); // change the sign of the first element + V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(3, 3, 1, 3)); // W W -Y -W + V31 = _mm_xor_ps(V31, vMPPP); // change the sign of the first element + Y = btCastiTo128f(_mm_shuffle_epi32(NQi, BT_SHUFFLE(3, 2, 0, 3))); // -W -Z -X -W + Z = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_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; + V2 = V2 + V21; + V3 = V3 + V31; - vs = bt_splat3_ps(vs, 0); - // s ready - V1 = V1 * vs; - V2 = V2 * vs; - V3 = V3 * vs; - - V1 = V1 + v1000; - V2 = V2 + v0100; - V3 = V3 + v0010; - - m_el[0] = V1; - m_el[1] = V2; - m_el[2] = V3; - #else - btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; - btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; - btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; - btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; + vs = bt_splat3_ps(vs, 0); + // s ready + V1 = V1 * vs; + V2 = V2 * vs; + V3 = V3 * vs; + + V1 = V1 + v1000; + V2 = V2 + v0100; + V3 = V3 + v0010; + + m_el[0] = V1; + m_el[1] = V2; + m_el[2] = V3; +#else + btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; + btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; + btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; + btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; setValue( - btScalar(1.0) - (yy + zz), xy - wz, xz + wy, + btScalar(1.0) - (yy + zz), xy - wz, xz + wy, xy + wz, btScalar(1.0) - (xx + zz), yz - wx, xz - wy, yz + wx, btScalar(1.0) - (xx + yy)); - #endif - } - +#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 btScalar& yaw, const btScalar& pitch, const btScalar& roll) + void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { setEulerZYX(roll, pitch, yaw); } @@ -289,188 +285,203 @@ public: /** @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 * about X then Y and then Z **/ - void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { + void setEulerZYX(btScalar eulerX, btScalar eulerY, btScalar eulerZ) + { ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code - btScalar ci ( btCos(eulerX)); - btScalar cj ( btCos(eulerY)); - btScalar ch ( btCos(eulerZ)); - btScalar si ( btSin(eulerX)); - btScalar sj ( btSin(eulerY)); - btScalar sh ( btSin(eulerZ)); - btScalar cc = ci * ch; - btScalar cs = ci * sh; - btScalar sc = si * ch; + btScalar ci(btCos(eulerX)); + btScalar cj(btCos(eulerY)); + btScalar ch(btCos(eulerZ)); + btScalar si(btSin(eulerX)); + btScalar sj(btSin(eulerY)); + btScalar sh(btSin(eulerZ)); + btScalar cc = ci * ch; + btScalar cs = ci * sh; + btScalar sc = si * ch; btScalar 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); + cj * sh, sj * ss + cc, sj * cs - sc, + -sj, cj * si, cj * ci); } /**@brief Set the matrix to the identity */ void setIdentity() - { -#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined(BT_USE_NEON) - m_el[0] = v1000; - m_el[1] = v0100; - m_el[2] = v0010; + { +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) + m_el[0] = v1000; + m_el[1] = v0100; + m_el[2] = v0010; #else - setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), - btScalar(0.0), btScalar(1.0), btScalar(0.0), - btScalar(0.0), btScalar(0.0), btScalar(1.0)); + setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), + btScalar(0.0), btScalar(1.0), btScalar(0.0), + btScalar(0.0), btScalar(0.0), btScalar(1.0)); #endif } - static const btMatrix3x3& getIdentity() + static const btMatrix3x3& getIdentity() { -#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined(BT_USE_NEON) - static const btMatrix3x3 - identityMatrix(v1000, v0100, v0010); +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) + static const btMatrix3x3 + identityMatrix(v1000, v0100, v0010); #else - static const btMatrix3x3 - identityMatrix( - btScalar(1.0), btScalar(0.0), btScalar(0.0), - btScalar(0.0), btScalar(1.0), btScalar(0.0), - btScalar(0.0), btScalar(0.0), btScalar(1.0)); + static const btMatrix3x3 + identityMatrix( + btScalar(1.0), btScalar(0.0), btScalar(0.0), + btScalar(0.0), btScalar(1.0), btScalar(0.0), + btScalar(0.0), btScalar(0.0), btScalar(1.0)); #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(btScalar *m) const + void getOpenGLSubMatrix(btScalar * m) const { -#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_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, btvFFF0fMask); // x2 y2 z2 0 - - vT = _mm_unpackhi_ps(v0, v1); // z0 z1 * * - v0 = _mm_unpacklo_ps(v0, v1); // x0 x1 y0 y1 +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_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; - v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3) ); // y0 y1 y2 0 - v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3) ); // x0 x1 x2 0 - v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT))); // z0 z1 z2 0 + v2 = _mm_and_ps(v2, btvFFF0fMask); // x2 y2 z2 0 - vm[0] = v0; - vm[1] = v1; - vm[2] = v2; + vT = _mm_unpackhi_ps(v0, v1); // z0 z1 * * + v0 = _mm_unpacklo_ps(v0, v1); // x0 x1 y0 y1 + + v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3)); // y0 y1 y2 0 + v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3)); // x0 x1 x2 0 + v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT))); // z0 z1 z2 0 + + vm[0] = v0; + vm[1] = v1; + vm[2] = v2; #elif defined(BT_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) {static_cast(-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 + // 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){static_cast(-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; + vm[0] = v0; + vm[1] = v1; + vm[2] = v2; #else - m[0] = btScalar(m_el[0].x()); - m[1] = btScalar(m_el[1].x()); - m[2] = btScalar(m_el[2].x()); - m[3] = btScalar(0.0); - m[4] = btScalar(m_el[0].y()); - m[5] = btScalar(m_el[1].y()); - m[6] = btScalar(m_el[2].y()); - m[7] = btScalar(0.0); - m[8] = btScalar(m_el[0].z()); - m[9] = btScalar(m_el[1].z()); + m[0] = btScalar(m_el[0].x()); + m[1] = btScalar(m_el[1].x()); + m[2] = btScalar(m_el[2].x()); + m[3] = btScalar(0.0); + m[4] = btScalar(m_el[0].y()); + m[5] = btScalar(m_el[1].y()); + m[6] = btScalar(m_el[2].y()); + m[7] = btScalar(0.0); + m[8] = btScalar(m_el[0].z()); + m[9] = btScalar(m_el[1].z()); m[10] = btScalar(m_el[2].z()); - m[11] = btScalar(0.0); + m[11] = btScalar(0.0); #endif } /**@brief Get the matrix represented as a quaternion * @param q The quaternion which will be set */ - void getRotation(btQuaternion& q) const + void getRotation(btQuaternion & q) const { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON) - btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); - btScalar s, x; - - union { - btSimdFloat4 vec; - btScalar f[4]; - } temp; - - if (trace > btScalar(0.0)) - { - x = trace + btScalar(1.0); +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) + btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); + btScalar s, x; - temp.f[0]=m_el[2].y() - m_el[1].z(); - temp.f[1]=m_el[0].z() - m_el[2].x(); - temp.f[2]=m_el[1].x() - m_el[0].y(); - temp.f[3]=x; - //temp.f[3]= s * btScalar(0.5); - } - else - { - int i, j, k; - if(m_el[0].x() < m_el[1].y()) - { - if( m_el[1].y() < m_el[2].z() ) - { i = 2; j = 0; k = 1; } - else - { i = 1; j = 2; k = 0; } - } - else - { - if( m_el[0].x() < m_el[2].z()) - { i = 2; j = 0; k = 1; } - else - { i = 0; j = 1; k = 2; } - } + union { + btSimdFloat4 vec; + btScalar f[4]; + } temp; - x = m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0); + if (trace > btScalar(0.0)) + { + x = trace + btScalar(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 * btScalar(0.5); - } + temp.f[0] = m_el[2].y() - m_el[1].z(); + temp.f[1] = m_el[0].z() - m_el[2].x(); + temp.f[2] = m_el[1].x() - m_el[0].y(); + temp.f[3] = x; + //temp.f[3]= s * btScalar(0.5); + } + else + { + int i, j, k; + if (m_el[0].x() < m_el[1].y()) + { + if (m_el[1].y() < m_el[2].z()) + { + i = 2; + j = 0; + k = 1; + } + else + { + i = 1; + j = 2; + k = 0; + } + } + else + { + if (m_el[0].x() < m_el[2].z()) + { + i = 2; + j = 0; + k = 1; + } + else + { + i = 0; + j = 1; + k = 2; + } + } - s = btSqrt(x); - q.set128(temp.vec); - s = btScalar(0.5) / s; + x = m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0); - q *= s; -#else + 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 * btScalar(0.5); + } + + s = btSqrt(x); + q.set128(temp.vec); + s = btScalar(0.5) / s; + + q *= s; +#else btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); btScalar temp[4]; - if (trace > btScalar(0.0)) + if (trace > btScalar(0.0)) { btScalar s = btSqrt(trace + btScalar(1.0)); - temp[3]=(s * btScalar(0.5)); + temp[3] = (s * btScalar(0.5)); s = btScalar(0.5) / s; - temp[0]=((m_el[2].y() - m_el[1].z()) * s); - temp[1]=((m_el[0].z() - m_el[2].x()) * s); - temp[2]=((m_el[1].x() - m_el[0].y()) * s); - } - else + temp[0] = ((m_el[2].y() - m_el[1].z()) * s); + temp[1] = ((m_el[0].z() - m_el[2].x()) * s); + temp[2] = ((m_el[1].x() - m_el[0].y()) * s); + } + else { - int i = m_el[0].x() < m_el[1].y() ? - (m_el[1].y() < m_el[2].z() ? 2 : 1) : - (m_el[0].x() < m_el[2].z() ? 2 : 0); - int j = (i + 1) % 3; + int i = m_el[0].x() < m_el[1].y() ? (m_el[1].y() < m_el[2].z() ? 2 : 1) : (m_el[0].x() < m_el[2].z() ? 2 : 0); + int j = (i + 1) % 3; int k = (i + 2) % 3; btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0)); @@ -481,44 +492,42 @@ public: 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]); + 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(btScalar& yaw, btScalar& pitch, btScalar& roll) const + * @param roll around Z axis */ + void getEulerYPR(btScalar & yaw, btScalar & pitch, btScalar & roll) const { - // first use the normal calculus yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); pitch = btScalar(btAsin(-m_el[2].x())); roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); // on pitch = +/-HalfPI - if (btFabs(pitch)==SIMD_HALF_PI) + if (btFabs(pitch) == SIMD_HALF_PI) { - if (yaw>0) - yaw-=SIMD_PI; + if (yaw > 0) + yaw -= SIMD_PI; else - yaw+=SIMD_PI; + yaw += SIMD_PI; - if (roll>0) - roll-=SIMD_PI; + if (roll > 0) + roll -= SIMD_PI; else - roll+=SIMD_PI; + roll += SIMD_PI; } }; - /**@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*/ - void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const + * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ + void getEulerZYX(btScalar & yaw, btScalar & pitch, btScalar & roll, unsigned int solution_number = 1) const { struct Euler { @@ -528,7 +537,7 @@ public: }; Euler euler_out; - Euler euler_out2; //second solution + Euler euler_out2; //second solution //get the pointer to the raw data // Check that pitch is not at a singularity @@ -538,7 +547,7 @@ public: euler_out2.yaw = 0; // From difference of angles formula - btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); + btScalar delta = btAtan2(m_el[0].x(), m_el[0].z()); if (m_el[2].x() > 0) //gimbal locked up { euler_out.pitch = SIMD_PI / btScalar(2.0); @@ -546,7 +555,7 @@ public: euler_out.roll = euler_out.pitch + delta; euler_out2.roll = euler_out.pitch + delta; } - else // gimbal locked down + else // gimbal locked down { euler_out.pitch = -SIMD_PI / btScalar(2.0); euler_out2.pitch = -SIMD_PI / btScalar(2.0); @@ -556,29 +565,29 @@ public: } else { - euler_out.pitch = - btAsin(m_el[2].x()); + euler_out.pitch = -btAsin(m_el[2].x()); euler_out2.pitch = SIMD_PI - euler_out.pitch; - euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), - m_el[2].z()/btCos(euler_out.pitch)); - euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), - m_el[2].z()/btCos(euler_out2.pitch)); + euler_out.roll = btAtan2(m_el[2].y() / btCos(euler_out.pitch), + m_el[2].z() / btCos(euler_out.pitch)); + euler_out2.roll = btAtan2(m_el[2].y() / btCos(euler_out2.pitch), + m_el[2].z() / btCos(euler_out2.pitch)); - euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), - m_el[0].x()/btCos(euler_out.pitch)); - euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), - m_el[0].x()/btCos(euler_out2.pitch)); + euler_out.yaw = btAtan2(m_el[1].x() / btCos(euler_out.pitch), + m_el[0].x() / btCos(euler_out.pitch)); + euler_out2.yaw = btAtan2(m_el[1].x() / btCos(euler_out2.pitch), + m_el[0].x() / btCos(euler_out2.pitch)); } if (solution_number == 1) - { - yaw = euler_out.yaw; + { + yaw = euler_out.yaw; pitch = euler_out.pitch; roll = euler_out.roll; } else - { - yaw = euler_out2.yaw; + { + yaw = euler_out2.yaw; pitch = euler_out2.pitch; roll = euler_out2.roll; } @@ -589,18 +598,18 @@ public: btMatrix3x3 scaled(const btVector3& s) const { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON) +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) return btMatrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s); -#else +#else return btMatrix3x3( - m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), + m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); #endif } /**@brief Return the determinant of the matrix */ - btScalar determinant() const; + btScalar determinant() const; /**@brief Return the adjoint of the matrix */ btMatrix3x3 adjoint() const; /**@brief Return the matrix with all values non negative */ @@ -608,7 +617,7 @@ public: /**@brief Return the transpose of the matrix */ btMatrix3x3 transpose() const; /**@brief Return the inverse of the matrix */ - btMatrix3x3 inverse() const; + btMatrix3x3 inverse() const; /// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. @@ -618,9 +627,9 @@ public: btVector3 col1 = getColumn(0); btVector3 col2 = getColumn(1); btVector3 col3 = getColumn(2); - + btScalar det = btDot(col1, btCross(col2, col3)); - if (btFabs(det)>SIMD_EPSILON) + if (btFabs(det) > SIMD_EPSILON) { det = 1.0f / det; } @@ -634,67 +643,131 @@ public: btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; - SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const + SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const { return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); } - SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const + SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const { return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); } - SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const + SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const { 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 - SIMD_FORCE_INLINE void extractRotation(btQuaternion &q,btScalar tolerance = 1.0e-9, int maxIter=100) + ///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; + int iter = 0; btScalar w; - const btMatrix3x3& A=*this; - for(iter = 0; iter < maxIter; iter++) + 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); + 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) + if (w < tolerance) break; - q = btQuaternion(btVector3((btScalar(1.0) / w) * omega),w) * + q = btQuaternion(btVector3((btScalar(1.0) / w) * omega), w) * q; q.normalize(); } } - - - /**@brief diagonalizes this matrix + /**@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 maxIter The iteration stops when we hit the given tolerance or when maxIter have been executed. + * @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 tolerance = 1.0e-9, int maxIter=100) + void diagonalize(btMatrix3x3 & rot, btScalar threshold, int maxSteps) { - btQuaternion r; - r = btQuaternion::getIdentity(); - extractRotation(r,tolerance,maxIter); - rot.setRotation(r); - btMatrix3x3 rotInv = btMatrix3x3(r.inverse()); - btMatrix3x3 old = *this; - setValue(old.tdotx( rotInv[0]), old.tdoty( rotInv[0]), old.tdotz( rotInv[0]), - old.tdotx( rotInv[1]), old.tdoty( rotInv[1]), old.tdotz( rotInv[1]), - old.tdotx( rotInv[2]), old.tdoty( rotInv[2]), old.tdotz( rotInv[2])); + 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; + btScalar max = btFabs(m_el[0][1]); + btScalar v = btFabs(m_el[0][2]); + if (v > max) + { + q = 2; + r = 1; + max = v; + } + v = btFabs(m_el[1][2]); + if (v > max) + { + p = 1; + q = 2; + r = 0; + max = v; + } + + btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2])); + if (max <= t) + { + if (max <= SIMD_EPSILON * t) + { + return; + } + step = 1; + } + + // compute Jacobi rotation J which leads to a zero for element [p][q] + btScalar mpq = m_el[p][q]; + btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); + btScalar theta2 = theta * theta; + btScalar cos; + btScalar sin; + if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON)) + { + t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) + : 1 / (theta - btSqrt(1 + theta2)); + cos = 1 / btSqrt(1 + t * t); + sin = cos * t; + } + else + { + // approximation for large theta-value, i.e., a nearly diagonal matrix + t = 1 / (theta * (2 + btScalar(0.5) / theta2)); + cos = 1 - btScalar(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; + btScalar mrp = m_el[r][p]; + btScalar 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++) + { + btVector3& 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 @@ -702,304 +775,298 @@ public: * @param c1 The second column to use for calculating the cofactor * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details */ - btScalar cofac(int r1, int c1, int r2, int c2) const + btScalar cofac(int r1, int c1, int r2, int c2) const { return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; } - void serialize(struct btMatrix3x3Data& dataOut) const; + void serialize(struct btMatrix3x3Data & dataOut) const; - void serializeFloat(struct btMatrix3x3FloatData& dataOut) const; + void serializeFloat(struct btMatrix3x3FloatData & dataOut) const; - void deSerialize(const struct btMatrix3x3Data& dataIn); + void deSerialize(const struct btMatrix3x3Data& dataIn); - void deSerializeFloat(const struct btMatrix3x3FloatData& dataIn); - - void deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn); + void deSerializeFloat(const struct btMatrix3x3FloatData& dataIn); + void deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn); }; - -SIMD_FORCE_INLINE btMatrix3x3& +SIMD_FORCE_INLINE btMatrix3x3& btMatrix3x3::operator*=(const btMatrix3x3& m) { -#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - __m128 rv00, rv01, rv02; - __m128 rv10, rv11, rv12; - __m128 rv20, rv21, rv22; - __m128 mv0, mv1, mv2; +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_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; + rv02 = m_el[0].mVec128; + rv12 = m_el[1].mVec128; + rv22 = m_el[2].mVec128; - mv0 = _mm_and_ps(m[0].mVec128, btvFFF0fMask); - mv1 = _mm_and_ps(m[1].mVec128, btvFFF0fMask); - mv2 = _mm_and_ps(m[2].mVec128, btvFFF0fMask); - - // rv0 - rv00 = bt_splat_ps(rv02, 0); - rv01 = bt_splat_ps(rv02, 1); - rv02 = bt_splat_ps(rv02, 2); - - rv00 = _mm_mul_ps(rv00, mv0); - rv01 = _mm_mul_ps(rv01, mv1); - rv02 = _mm_mul_ps(rv02, mv2); - - // rv1 - rv10 = bt_splat_ps(rv12, 0); - rv11 = bt_splat_ps(rv12, 1); - rv12 = bt_splat_ps(rv12, 2); - - rv10 = _mm_mul_ps(rv10, mv0); - rv11 = _mm_mul_ps(rv11, mv1); - rv12 = _mm_mul_ps(rv12, mv2); - - // rv2 - rv20 = bt_splat_ps(rv22, 0); - rv21 = bt_splat_ps(rv22, 1); - rv22 = bt_splat_ps(rv22, 2); - - rv20 = _mm_mul_ps(rv20, mv0); - rv21 = _mm_mul_ps(rv21, mv1); - rv22 = _mm_mul_ps(rv22, mv2); + mv0 = _mm_and_ps(m[0].mVec128, btvFFF0fMask); + mv1 = _mm_and_ps(m[1].mVec128, btvFFF0fMask); + mv2 = _mm_and_ps(m[2].mVec128, btvFFF0fMask); - rv00 = _mm_add_ps(rv00, rv01); - rv10 = _mm_add_ps(rv10, rv11); - rv20 = _mm_add_ps(rv20, rv21); + // rv0 + rv00 = bt_splat_ps(rv02, 0); + rv01 = bt_splat_ps(rv02, 1); + rv02 = bt_splat_ps(rv02, 2); - 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); + rv00 = _mm_mul_ps(rv00, mv0); + rv01 = _mm_mul_ps(rv01, mv1); + rv02 = _mm_mul_ps(rv02, mv2); + + // rv1 + rv10 = bt_splat_ps(rv12, 0); + rv11 = bt_splat_ps(rv12, 1); + rv12 = bt_splat_ps(rv12, 2); + + rv10 = _mm_mul_ps(rv10, mv0); + rv11 = _mm_mul_ps(rv11, mv1); + rv12 = _mm_mul_ps(rv12, mv2); + + // rv2 + rv20 = bt_splat_ps(rv22, 0); + rv21 = bt_splat_ps(rv22, 1); + rv22 = bt_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(BT_USE_NEON) - float32x4_t rv0, rv1, rv2; - float32x4_t v0, v1, v2; - float32x4_t mv0, mv1, mv2; + 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; + 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, btvFFF0Mask); - mv1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask); - mv2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask); - - 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); + mv0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask); + mv1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask); + mv2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask); - m_el[0].mVec128 = rv0; - m_el[1].mVec128 = rv1; - m_el[2].mVec128 = rv2; -#else + 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[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; } -SIMD_FORCE_INLINE btMatrix3x3& +SIMD_FORCE_INLINE btMatrix3x3& btMatrix3x3::operator+=(const btMatrix3x3& m) { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_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; +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_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]); + 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; } SIMD_FORCE_INLINE btMatrix3x3 -operator*(const btMatrix3x3& m, const btScalar & k) +operator*(const btMatrix3x3& m, const btScalar& k) { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) - __m128 vk = bt_splat_ps(_mm_load_ss((float *)&k), 0x80); - return btMatrix3x3( - _mm_mul_ps(m[0].mVec128, vk), - _mm_mul_ps(m[1].mVec128, vk), - _mm_mul_ps(m[2].mVec128, vk)); +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) + __m128 vk = bt_splat_ps(_mm_load_ss((float*)&k), 0x80); + return btMatrix3x3( + _mm_mul_ps(m[0].mVec128, vk), + _mm_mul_ps(m[1].mVec128, vk), + _mm_mul_ps(m[2].mVec128, vk)); #elif defined(BT_USE_NEON) - return btMatrix3x3( - vmulq_n_f32(m[0].mVec128, k), - vmulq_n_f32(m[1].mVec128, k), - vmulq_n_f32(m[2].mVec128, k)); + return btMatrix3x3( + vmulq_n_f32(m[0].mVec128, k), + vmulq_n_f32(m[1].mVec128, k), + vmulq_n_f32(m[2].mVec128, k)); #else return btMatrix3x3( - m[0].x()*k,m[0].y()*k,m[0].z()*k, - m[1].x()*k,m[1].y()*k,m[1].z()*k, - m[2].x()*k,m[2].y()*k,m[2].z()*k); + m[0].x() * k, m[0].y() * k, m[0].z() * k, + m[1].x() * k, m[1].y() * k, m[1].z() * k, + m[2].x() * k, m[2].y() * k, m[2].z() * k); #endif } -SIMD_FORCE_INLINE btMatrix3x3 +SIMD_FORCE_INLINE btMatrix3x3 operator+(const btMatrix3x3& m1, const btMatrix3x3& m2) { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON) +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) return btMatrix3x3( - m1[0].mVec128 + m2[0].mVec128, - m1[1].mVec128 + m2[1].mVec128, - m1[2].mVec128 + m2[2].mVec128); + m1[0].mVec128 + m2[0].mVec128, + m1[1].mVec128 + m2[1].mVec128, + m1[2].mVec128 + m2[2].mVec128); #else return btMatrix3x3( - 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 -} + m1[0][0] + m2[0][0], + m1[0][1] + m2[0][1], + m1[0][2] + m2[0][2], -SIMD_FORCE_INLINE btMatrix3x3 -operator-(const btMatrix3x3& m1, const btMatrix3x3& m2) -{ -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON) - return btMatrix3x3( - m1[0].mVec128 - m2[0].mVec128, - m1[1].mVec128 - m2[1].mVec128, - m1[2].mVec128 - m2[2].mVec128); -#else - return btMatrix3x3( - 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]); + 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 } +SIMD_FORCE_INLINE btMatrix3x3 +operator-(const btMatrix3x3& m1, const btMatrix3x3& m2) +{ +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) + return btMatrix3x3( + m1[0].mVec128 - m2[0].mVec128, + m1[1].mVec128 - m2[1].mVec128, + m1[2].mVec128 - m2[2].mVec128); +#else + return btMatrix3x3( + m1[0][0] - m2[0][0], + m1[0][1] - m2[0][1], + m1[0][2] - m2[0][2], -SIMD_FORCE_INLINE btMatrix3x3& + 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 +} + +SIMD_FORCE_INLINE btMatrix3x3& btMatrix3x3::operator-=(const btMatrix3x3& m) { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_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; +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_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]); + 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; } - -SIMD_FORCE_INLINE btScalar +SIMD_FORCE_INLINE btScalar btMatrix3x3::determinant() const -{ +{ return btTriple((*this)[0], (*this)[1], (*this)[2]); } - -SIMD_FORCE_INLINE btMatrix3x3 +SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::absolute() const { -#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) - return btMatrix3x3( - _mm_and_ps(m_el[0].mVec128, btvAbsfMask), - _mm_and_ps(m_el[1].mVec128, btvAbsfMask), - _mm_and_ps(m_el[2].mVec128, btvAbsfMask)); -#elif defined(BT_USE_NEON) - return btMatrix3x3( - (float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, btv3AbsMask), - (float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, btv3AbsMask), - (float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask)); -#else +#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) return btMatrix3x3( - btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), - btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), - btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); -#endif -} - -SIMD_FORCE_INLINE btMatrix3x3 -btMatrix3x3::transpose() const -{ -#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_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, btvFFF0fMask); // 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, BT_SHUFFLE(2, 3, 1, 3) ); // y0 y1 y2 0 - v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3) ); // x0 x1 x2 0 - v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT))); // z0 z1 z2 0 - - - return btMatrix3x3( v0, v1, v2 ); + _mm_and_ps(m_el[0].mVec128, btvAbsfMask), + _mm_and_ps(m_el[1].mVec128, btvAbsfMask), + _mm_and_ps(m_el[2].mVec128, btvAbsfMask)); #elif defined(BT_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) {static_cast(-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 btMatrix3x3( v0, v1, v2 ); + return btMatrix3x3( + (float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, btv3AbsMask), + (float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, btv3AbsMask), + (float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask)); #else - return btMatrix3x3( m_el[0].x(), m_el[1].x(), m_el[2].x(), - m_el[0].y(), m_el[1].y(), m_el[2].y(), - m_el[0].z(), m_el[1].z(), m_el[2].z()); + return btMatrix3x3( + btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), + btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), + btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); #endif } -SIMD_FORCE_INLINE btMatrix3x3 -btMatrix3x3::adjoint() const +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::transpose() const +{ +#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_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, btvFFF0fMask); // 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, BT_SHUFFLE(2, 3, 1, 3)); // y0 y1 y2 0 + v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3)); // x0 x1 x2 0 + v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT))); // z0 z1 z2 0 + + return btMatrix3x3(v0, v1, v2); +#elif defined(BT_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){static_cast(-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 btMatrix3x3(v0, v1, v2); +#else + return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), + m_el[0].y(), m_el[1].y(), m_el[2].y(), + m_el[0].z(), m_el[1].z(), m_el[2].z()); +#endif +} + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::adjoint() const { return btMatrix3x3(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)); + 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)); } -SIMD_FORCE_INLINE btMatrix3x3 +SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::inverse() const { btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); @@ -1008,54 +1075,54 @@ btMatrix3x3::inverse() const 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, - co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); + co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, + co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); } -SIMD_FORCE_INLINE btMatrix3x3 +SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const { -#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_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, btvFFF0fMask ); - __m128 m1 = _mm_and_ps( m.getRow(1).mVec128, btvFFF0fMask); - __m128 m2 = _mm_and_ps( m.getRow(2).mVec128, btvFFF0fMask ); - __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 btMatrix3x3( r0, r1, r2 ); +#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_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, btvFFF0fMask); + __m128 m1 = _mm_and_ps(m.getRow(1).mVec128, btvFFF0fMask); + __m128 m2 = _mm_and_ps(m.getRow(2).mVec128, btvFFF0fMask); + __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 btMatrix3x3(r0, r1, r2); #elif defined BT_USE_NEON - // zeros w - static const uint32x4_t xyzMask = (const uint32x4_t){ static_cast(-1), static_cast(-1), static_cast(-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 btMatrix3x3( r0, r1, r2 ); + // zeros w + static const uint32x4_t xyzMask = (const uint32x4_t){static_cast(-1), static_cast(-1), static_cast(-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 btMatrix3x3(r0, r1, r2); #else - return btMatrix3x3( + return btMatrix3x3( m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), @@ -1068,51 +1135,51 @@ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const #endif } -SIMD_FORCE_INLINE btMatrix3x3 +SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::timesTranspose(const btMatrix3x3& m) const { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) - __m128 a0 = m_el[0].mVec128; - __m128 a1 = m_el[1].mVec128; - __m128 a2 = m_el[2].mVec128; - - btMatrix3x3 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 btMatrix3x3( r0, r1, r2); - +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) + __m128 a0 = m_el[0].mVec128; + __m128 a1 = m_el[1].mVec128; + __m128 a2 = m_el[2].mVec128; + + btMatrix3x3 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 btMatrix3x3(r0, r1, r2); + #elif defined BT_USE_NEON - float32x4_t a0 = m_el[0].mVec128; - float32x4_t a1 = m_el[1].mVec128; - float32x4_t a2 = m_el[2].mVec128; - - btMatrix3x3 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 btMatrix3x3( r0, r1, r2 ); - + float32x4_t a0 = m_el[0].mVec128; + float32x4_t a1 = m_el[1].mVec128; + float32x4_t a2 = m_el[2].mVec128; + + btMatrix3x3 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 btMatrix3x3(r0, r1, r2); + #else return btMatrix3x3( m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), @@ -1121,139 +1188,138 @@ btMatrix3x3::timesTranspose(const btMatrix3x3& m) const #endif } -SIMD_FORCE_INLINE btVector3 -operator*(const btMatrix3x3& m, const btVector3& v) +SIMD_FORCE_INLINE btVector3 +operator*(const btMatrix3x3& m, const btVector3& v) { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON) - return v.dot3(m[0], m[1], m[2]); +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) + return v.dot3(m[0], m[1], m[2]); #else return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); #endif } - SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v, const btMatrix3x3& m) { -#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) +#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) - const __m128 vv = v.mVec128; + const __m128 vv = v.mVec128; - __m128 c0 = bt_splat_ps( vv, 0); - __m128 c1 = bt_splat_ps( vv, 1); - __m128 c2 = bt_splat_ps( vv, 2); + __m128 c0 = bt_splat_ps(vv, 0); + __m128 c1 = bt_splat_ps(vv, 1); + __m128 c2 = bt_splat_ps(vv, 2); - c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, btvFFF0fMask) ); - c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, btvFFF0fMask) ); - c0 = _mm_add_ps(c0, c1); - c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, btvFFF0fMask) ); - - return btVector3(_mm_add_ps(c0, c2)); + c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, btvFFF0fMask)); + c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, btvFFF0fMask)); + c0 = _mm_add_ps(c0, c1); + c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, btvFFF0fMask)); + + return btVector3(_mm_add_ps(c0, c2)); #elif defined(BT_USE_NEON) - const float32x4_t vv = v.mVec128; - const float32x2_t vlo = vget_low_f32(vv); - const float32x2_t vhi = vget_high_f32(vv); + 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; + float32x4_t c0, c1, c2; - c0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask); - c1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask); - c2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask); + c0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask); + c1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask); + c2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask); - 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 btVector3(c0); + 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 btVector3(c0); #else return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); #endif } -SIMD_FORCE_INLINE btMatrix3x3 +SIMD_FORCE_INLINE btMatrix3x3 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) { -#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) +#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_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, btvFFF0fMask); - - __m128 c0 = bt_splat_ps( m10, 0); - __m128 c1 = bt_splat_ps( m11, 0); - __m128 c2 = bt_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, btvFFF0fMask); - - __m128 c0_1 = bt_splat_ps( m10, 1); - __m128 c1_1 = bt_splat_ps( m11, 1); - __m128 c2_1 = bt_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, btvFFF0fMask); - - c0 = _mm_add_ps(c0, c0_1); - c1 = _mm_add_ps(c1, c1_1); - c2 = _mm_add_ps(c2, c2_1); - - m10 = bt_splat_ps( m10, 2); - m11 = bt_splat_ps( m11, 2); - m12 = bt_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 btMatrix3x3(c0, c1, c2); + __m128 m10 = m1[0].mVec128; + __m128 m11 = m1[1].mVec128; + __m128 m12 = m1[2].mVec128; + + __m128 m2v = _mm_and_ps(m2[0].mVec128, btvFFF0fMask); + + __m128 c0 = bt_splat_ps(m10, 0); + __m128 c1 = bt_splat_ps(m11, 0); + __m128 c2 = bt_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, btvFFF0fMask); + + __m128 c0_1 = bt_splat_ps(m10, 1); + __m128 c1_1 = bt_splat_ps(m11, 1); + __m128 c2_1 = bt_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, btvFFF0fMask); + + c0 = _mm_add_ps(c0, c0_1); + c1 = _mm_add_ps(c1, c1_1); + c2 = _mm_add_ps(c2, c2_1); + + m10 = bt_splat_ps(m10, 2); + m11 = bt_splat_ps(m11, 2); + m12 = bt_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 btMatrix3x3(c0, c1, c2); #elif defined(BT_USE_NEON) - float32x4_t rv0, rv1, rv2; - float32x4_t v0, v1, v2; - float32x4_t mv0, mv1, mv2; + 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; + v0 = m1[0].mVec128; + v1 = m1[1].mVec128; + v2 = m1[2].mVec128; - mv0 = (float32x4_t) vandq_s32((int32x4_t)m2[0].mVec128, btvFFF0Mask); - mv1 = (float32x4_t) vandq_s32((int32x4_t)m2[1].mVec128, btvFFF0Mask); - mv2 = (float32x4_t) vandq_s32((int32x4_t)m2[2].mVec128, btvFFF0Mask); - - 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); + mv0 = (float32x4_t)vandq_s32((int32x4_t)m2[0].mVec128, btvFFF0Mask); + mv1 = (float32x4_t)vandq_s32((int32x4_t)m2[1].mVec128, btvFFF0Mask); + mv2 = (float32x4_t)vandq_s32((int32x4_t)m2[2].mVec128, btvFFF0Mask); + + 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 btMatrix3x3(rv0, rv1, rv2); - -#else + +#else return btMatrix3x3( - 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])); + 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 } @@ -1276,73 +1342,67 @@ m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); * It will test all elements are equal. */ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) { -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) - __m128 c0, c1, c2; + __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); + 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); 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] && - 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] ); + +#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 btMatrix3x3FloatData +struct btMatrix3x3FloatData { btVector3FloatData m_el[3]; }; ///for serialization -struct btMatrix3x3DoubleData +struct btMatrix3x3DoubleData { btVector3DoubleData m_el[3]; }; - - - -SIMD_FORCE_INLINE void btMatrix3x3::serialize(struct btMatrix3x3Data& dataOut) const +SIMD_FORCE_INLINE void btMatrix3x3::serialize(struct btMatrix3x3Data& dataOut) const { - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) m_el[i].serialize(dataOut.m_el[i]); } -SIMD_FORCE_INLINE void btMatrix3x3::serializeFloat(struct btMatrix3x3FloatData& dataOut) const +SIMD_FORCE_INLINE void btMatrix3x3::serializeFloat(struct btMatrix3x3FloatData& dataOut) const { - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) m_el[i].serializeFloat(dataOut.m_el[i]); } - -SIMD_FORCE_INLINE void btMatrix3x3::deSerialize(const struct btMatrix3x3Data& dataIn) +SIMD_FORCE_INLINE void btMatrix3x3::deSerialize(const struct btMatrix3x3Data& dataIn) { - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) m_el[i].deSerialize(dataIn.m_el[i]); } -SIMD_FORCE_INLINE void btMatrix3x3::deSerializeFloat(const struct btMatrix3x3FloatData& dataIn) +SIMD_FORCE_INLINE void btMatrix3x3::deSerializeFloat(const struct btMatrix3x3FloatData& dataIn) { - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) m_el[i].deSerializeFloat(dataIn.m_el[i]); } -SIMD_FORCE_INLINE void btMatrix3x3::deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn) +SIMD_FORCE_INLINE void btMatrix3x3::deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn) { - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) m_el[i].deSerializeDouble(dataIn.m_el[i]); } -#endif //BT_MATRIX3x3_H - +#endif //BT_MATRIX3x3_H diff --git a/Engine/lib/bullet/src/LinearMath/btMatrixX.h b/Engine/lib/bullet/src/LinearMath/btMatrixX.h index 42caed42e..388c57c2d 100644 --- a/Engine/lib/bullet/src/LinearMath/btMatrixX.h +++ b/Engine/lib/bullet/src/LinearMath/btMatrixX.h @@ -24,24 +24,23 @@ subject to the following restrictions: //#define BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM #include -#include // std::setw -#endif //BT_DEBUG_OSTREAM +#include // std::setw +#endif //BT_DEBUG_OSTREAM class btIntSortPredicate { - public: - bool operator() ( const int& a, const int& b ) const - { - return a < b; - } +public: + bool operator()(const int& a, const int& b) const + { + return a < b; + } }; - template struct btVectorX { - btAlignedObjectArray m_storage; - + btAlignedObjectArray m_storage; + btVectorX() { } @@ -49,7 +48,7 @@ struct btVectorX { m_storage.resize(numRows); } - + void resize(int rows) { m_storage.resize(rows); @@ -66,13 +65,13 @@ struct btVectorX { return rows(); } - + T nrm2() const { T norm = T(0); - + int nn = rows(); - + { if (nn == 1) { @@ -82,11 +81,11 @@ struct btVectorX { T scale = 0.0; T ssq = 1.0; - + /* The following loop is equivalent to this call to the LAPACK auxiliary routine: CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */ - - for (int ix=0;ix @@ -151,8 +148,7 @@ struct btVectorX } */ - -template +template struct btMatrixX { int m_rows; @@ -161,10 +157,10 @@ struct btMatrixX int m_resizeOperations; int m_setElemOperations; - btAlignedObjectArray m_storage; - mutable btAlignedObjectArray< btAlignedObjectArray > m_rowNonZeroElements1; + btAlignedObjectArray m_storage; + mutable btAlignedObjectArray > m_rowNonZeroElements1; - T* getBufferPointerWritable() + T* getBufferPointerWritable() { return m_storage.size() ? &m_storage[0] : 0; } @@ -174,21 +170,21 @@ struct btMatrixX return m_storage.size() ? &m_storage[0] : 0; } btMatrixX() - :m_rows(0), - m_cols(0), - m_operations(0), - m_resizeOperations(0), - m_setElemOperations(0) + : m_rows(0), + m_cols(0), + m_operations(0), + m_resizeOperations(0), + m_setElemOperations(0) { } - btMatrixX(int rows,int cols) - :m_rows(rows), - m_cols(cols), - m_operations(0), - m_resizeOperations(0), - m_setElemOperations(0) + btMatrixX(int rows, int cols) + : m_rows(rows), + m_cols(cols), + m_operations(0), + m_resizeOperations(0), + m_setElemOperations(0) { - resize(rows,cols); + resize(rows, cols); } void resize(int rows, int cols) { @@ -197,7 +193,7 @@ struct btMatrixX m_cols = cols; { BT_PROFILE("m_storage.resize"); - m_storage.resize(rows*cols); + m_storage.resize(rows * cols); } } int cols() const @@ -215,108 +211,102 @@ struct btMatrixX } */ - void addElem(int row,int col, T val) + void addElem(int row, int col, T val) { if (val) { - if (m_storage[col+row*m_cols]==0.f) + if (m_storage[col + row * m_cols] == 0.f) { - setElem(row,col,val); - } else + setElem(row, col, val); + } + else { - m_storage[row*m_cols+col] += val; + m_storage[row * m_cols + col] += val; } } } - - - void setElem(int row,int col, T val) + + void setElem(int row, int col, T val) { m_setElemOperations++; - m_storage[row*m_cols+col] = val; + m_storage[row * m_cols + col] = val; } - - void mulElem(int row,int col, T val) + + void mulElem(int row, int col, T val) { m_setElemOperations++; //mul doesn't change sparsity info - m_storage[row*m_cols+col] *= val; + m_storage[row * m_cols + col] *= val; } - - - - + void copyLowerToUpperTriangle() { - int count=0; - for (int row=0;row0 && numRowsOther>0 && B && C); - const btScalar *bb = B; - for ( int i = 0;i 0 && numRowsOther > 0 && B && C); + const btScalar* bb = B; + for (int i = 0; i < numRows; i++) { - const btScalar *cc = C; - for ( int j = 0;j& block) + void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX& block) { - btAssert(rowend+1-rowstart == block.rows()); - btAssert(colend+1-colstart == block.cols()); - for (int row=0;row btMatrixXf; typedef btVectorX btVectorXf; typedef btMatrixX btMatrixXd; typedef btVectorX btVectorXd; - #ifdef BT_DEBUG_OSTREAM -template -std::ostream& operator<< (std::ostream& os, const btMatrixX& mat) +template +std::ostream& operator<<(std::ostream& os, const btMatrixX& mat) +{ + os << " ["; + //printf("%s ---------------------\n",msg); + for (int i = 0; i < mat.rows(); i++) { - - os << " ["; - //printf("%s ---------------------\n",msg); - for (int i=0;i -std::ostream& operator<< (std::ostream& os, const btVectorX& mat) + os << " ]"; + //printf("\n---------------------\n"); + + return os; +} +template +std::ostream& operator<<(std::ostream& os, const btVectorX& mat) +{ + os << " ["; + //printf("%s ---------------------\n",msg); + for (int i = 0; i < mat.rows(); i++) { - - os << " ["; - //printf("%s ---------------------\n",msg); - for (int i=0;i -SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) +SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) { - return a < b ? a : b ; + return a < b ? a : b; } template -SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) +SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) { - return a > b ? a : b; + return a > b ? a : b; } template -SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub) +SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub) { - return a < lb ? lb : (ub < a ? ub : a); + return a < lb ? lb : (ub < a ? ub : a); } template -SIMD_FORCE_INLINE void btSetMin(T& a, const T& b) +SIMD_FORCE_INLINE void btSetMin(T& a, const T& b) { - if (b < a) + if (b < a) { a = b; } } template -SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) +SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) { - if (a < b) + if (a < b) { a = b; } } template -SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub) +SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub) { - if (a < lb) + if (a < lb) { - a = lb; + a = lb; } - else if (ub < a) + else if (ub < a) { a = ub; } } -#endif //BT_GEN_MINMAX_H +#endif //BT_GEN_MINMAX_H diff --git a/Engine/lib/bullet/src/LinearMath/btMotionState.h b/Engine/lib/bullet/src/LinearMath/btMotionState.h index 943181409..ae6a51611 100644 --- a/Engine/lib/bullet/src/LinearMath/btMotionState.h +++ b/Engine/lib/bullet/src/LinearMath/btMotionState.h @@ -20,21 +20,17 @@ subject to the following restrictions: ///The btMotionState interface class allows the dynamics world to synchronize and interpolate the updated world transforms with graphics ///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation) -class btMotionState +class btMotionState { - public: - - virtual ~btMotionState() - { - - } - - virtual void getWorldTransform(btTransform& worldTrans ) const =0; +public: + virtual ~btMotionState() + { + } - //Bullet only calls the update of worldtransform for active objects - virtual void setWorldTransform(const btTransform& worldTrans)=0; - - + virtual void getWorldTransform(btTransform& worldTrans) const = 0; + + //Bullet only calls the update of worldtransform for active objects + virtual void setWorldTransform(const btTransform& worldTrans) = 0; }; -#endif //BT_MOTIONSTATE_H +#endif //BT_MOTIONSTATE_H diff --git a/Engine/lib/bullet/src/LinearMath/btPolarDecomposition.cpp b/Engine/lib/bullet/src/LinearMath/btPolarDecomposition.cpp index b3664faa4..d9c72a801 100644 --- a/Engine/lib/bullet/src/LinearMath/btPolarDecomposition.cpp +++ b/Engine/lib/bullet/src/LinearMath/btPolarDecomposition.cpp @@ -3,96 +3,92 @@ namespace { - btScalar abs_column_sum(const btMatrix3x3& a, int i) - { - return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]); - } - - btScalar abs_row_sum(const btMatrix3x3& a, int i) - { - return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]); - } - - btScalar p1_norm(const btMatrix3x3& a) - { - const btScalar sum0 = abs_column_sum(a,0); - const btScalar sum1 = abs_column_sum(a,1); - const btScalar sum2 = abs_column_sum(a,2); - return btMax(btMax(sum0, sum1), sum2); - } - - btScalar pinf_norm(const btMatrix3x3& a) - { - const btScalar sum0 = abs_row_sum(a,0); - const btScalar sum1 = abs_row_sum(a,1); - const btScalar sum2 = abs_row_sum(a,2); - return btMax(btMax(sum0, sum1), sum2); - } +btScalar abs_column_sum(const btMatrix3x3& a, int i) +{ + return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]); } +btScalar abs_row_sum(const btMatrix3x3& a, int i) +{ + return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]); +} +btScalar p1_norm(const btMatrix3x3& a) +{ + const btScalar sum0 = abs_column_sum(a, 0); + const btScalar sum1 = abs_column_sum(a, 1); + const btScalar sum2 = abs_column_sum(a, 2); + return btMax(btMax(sum0, sum1), sum2); +} + +btScalar pinf_norm(const btMatrix3x3& a) +{ + const btScalar sum0 = abs_row_sum(a, 0); + const btScalar sum1 = abs_row_sum(a, 1); + const btScalar sum2 = abs_row_sum(a, 2); + return btMax(btMax(sum0, sum1), sum2); +} +} // namespace btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations) -: m_tolerance(tolerance) -, m_maxIterations(maxIterations) + : m_tolerance(tolerance), m_maxIterations(maxIterations) { } unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const { - // Use the 'u' and 'h' matrices for intermediate calculations - u = a; - h = a.inverse(); + // Use the 'u' and 'h' matrices for intermediate calculations + u = a; + h = a.inverse(); - for (unsigned int i = 0; i < m_maxIterations; ++i) - { - const btScalar h_1 = p1_norm(h); - const btScalar h_inf = pinf_norm(h); - const btScalar u_1 = p1_norm(u); - const btScalar u_inf = pinf_norm(u); + for (unsigned int i = 0; i < m_maxIterations; ++i) + { + const btScalar h_1 = p1_norm(h); + const btScalar h_inf = pinf_norm(h); + const btScalar u_1 = p1_norm(u); + const btScalar u_inf = pinf_norm(u); - const btScalar h_norm = h_1 * h_inf; - const btScalar u_norm = u_1 * u_inf; + const btScalar h_norm = h_1 * h_inf; + const btScalar u_norm = u_1 * u_inf; - // The matrix is effectively singular so we cannot invert it - if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm)) - break; + // The matrix is effectively singular so we cannot invert it + if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm)) + break; - const btScalar gamma = btPow(h_norm / u_norm, 0.25f); - const btScalar inv_gamma = btScalar(1.0) / gamma; + const btScalar gamma = btPow(h_norm / u_norm, 0.25f); + const btScalar inv_gamma = btScalar(1.0) / gamma; - // Determine the delta to 'u' - const btMatrix3x3 delta = (u * (gamma - btScalar(2.0)) + h.transpose() * inv_gamma) * btScalar(0.5); + // Determine the delta to 'u' + const btMatrix3x3 delta = (u * (gamma - btScalar(2.0)) + h.transpose() * inv_gamma) * btScalar(0.5); - // Update the matrices - u += delta; - h = u.inverse(); + // Update the matrices + u += delta; + h = u.inverse(); - // Check for convergence - if (p1_norm(delta) <= m_tolerance * u_1) - { - h = u.transpose() * a; - h = (h + h.transpose()) * 0.5; - return i; - } - } + // Check for convergence + if (p1_norm(delta) <= m_tolerance * u_1) + { + h = u.transpose() * a; + h = (h + h.transpose()) * 0.5; + return i; + } + } - // The algorithm has failed to converge to the specified tolerance, but we - // want to make sure that the matrices returned are in the right form. - h = u.transpose() * a; - h = (h + h.transpose()) * 0.5; + // The algorithm has failed to converge to the specified tolerance, but we + // want to make sure that the matrices returned are in the right form. + h = u.transpose() * a; + h = (h + h.transpose()) * 0.5; - return m_maxIterations; + return m_maxIterations; } unsigned int btPolarDecomposition::maxIterations() const { - return m_maxIterations; + return m_maxIterations; } unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) { - static btPolarDecomposition polar; - return polar.decompose(a, u, h); + static btPolarDecomposition polar; + return polar.decompose(a, u, h); } - diff --git a/Engine/lib/bullet/src/LinearMath/btPolarDecomposition.h b/Engine/lib/bullet/src/LinearMath/btPolarDecomposition.h index 1feea0f78..bf29140a1 100644 --- a/Engine/lib/bullet/src/LinearMath/btPolarDecomposition.h +++ b/Engine/lib/bullet/src/LinearMath/btPolarDecomposition.h @@ -13,10 +13,8 @@ */ class btPolarDecomposition { - public: - - - /** +public: + /** * Creates an instance with optional parameters. * * @param tolerance - the tolerance used to determine convergence of the @@ -24,10 +22,10 @@ class btPolarDecomposition * @param maxIterations - the maximum number of iterations used to achieve * convergence */ - btPolarDecomposition(btScalar tolerance = btScalar(0.0001), - unsigned int maxIterations = 16); + btPolarDecomposition(btScalar tolerance = btScalar(0.0001), + unsigned int maxIterations = 16); - /** + /** * Decomposes a matrix into orthogonal and symmetric, positive-definite * parts. If the number of iterations returned by this function is equal to * the maximum number of iterations, the algorithm has failed to converge. @@ -38,19 +36,19 @@ class btPolarDecomposition * * @return the number of iterations performed by the algorithm. */ - unsigned int decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const; + unsigned int decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const; - /** + /** * Returns the maximum number of iterations that this algorithm will perform * to achieve convergence. * * @return maximum number of iterations */ - unsigned int maxIterations() const; + unsigned int maxIterations() const; - private: - btScalar m_tolerance; - unsigned int m_maxIterations; +private: + btScalar m_tolerance; + unsigned int m_maxIterations; }; /** @@ -66,7 +64,6 @@ class btPolarDecomposition * * @return the number of iterations performed by the algorithm. */ -unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h); - -#endif // POLARDECOMPOSITION_H +unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h); +#endif // POLARDECOMPOSITION_H diff --git a/Engine/lib/bullet/src/LinearMath/btPoolAllocator.h b/Engine/lib/bullet/src/LinearMath/btPoolAllocator.h index efdeda8ff..4e7b49660 100644 --- a/Engine/lib/bullet/src/LinearMath/btPoolAllocator.h +++ b/Engine/lib/bullet/src/LinearMath/btPoolAllocator.h @@ -12,7 +12,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef _BT_POOL_ALLOCATOR_H #define _BT_POOL_ALLOCATOR_H @@ -23,38 +22,38 @@ subject to the following restrictions: ///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately. class btPoolAllocator { - int m_elemSize; - int m_maxElements; - int m_freeCount; - void* m_firstFree; - unsigned char* m_pool; - btSpinMutex m_mutex; // only used if BT_THREADSAFE + int m_elemSize; + int m_maxElements; + int m_freeCount; + void* m_firstFree; + unsigned char* m_pool; + btSpinMutex m_mutex; // only used if BT_THREADSAFE public: - btPoolAllocator(int elemSize, int maxElements) - :m_elemSize(elemSize), - m_maxElements(maxElements) + : m_elemSize(elemSize), + m_maxElements(maxElements) { - m_pool = (unsigned char*) btAlignedAlloc( static_cast(m_elemSize*m_maxElements),16); + m_pool = (unsigned char*)btAlignedAlloc(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; - } + m_firstFree = p; + m_freeCount = m_maxElements; + int count = m_maxElements; + while (--count) + { + *(void**)p = (p + m_elemSize); + p += m_elemSize; + } + *(void**)p = 0; + } ~btPoolAllocator() { - btAlignedFree( m_pool); + btAlignedFree(m_pool); } - int getFreeCount() const + int getFreeCount() const { return m_freeCount; } @@ -69,26 +68,27 @@ public: return m_maxElements; } - void* allocate(int size) + void* allocate(int size) { // release mode fix (void)size; - btMutexLock(&m_mutex); - btAssert(!size || size<=m_elemSize); + btMutexLock(&m_mutex); + btAssert(!size || size <= m_elemSize); //btAssert(m_freeCount>0); // should return null if all full - void* result = m_firstFree; - if (NULL != m_firstFree) - { - m_firstFree = *(void**)m_firstFree; - --m_freeCount; - } - btMutexUnlock(&m_mutex); - return result; + void* result = m_firstFree; + if (NULL != m_firstFree) + { + m_firstFree = *(void**)m_firstFree; + --m_freeCount; + } + btMutexUnlock(&m_mutex); + return result; } bool validPtr(void* ptr) { - if (ptr) { + if (ptr) + { if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize)) { return true; @@ -97,34 +97,34 @@ public: return false; } - void freeMemory(void* ptr) + void freeMemory(void* ptr) { - if (ptr) { - btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize); + 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); - } + btMutexLock(&m_mutex); + *(void**)ptr = m_firstFree; + m_firstFree = ptr; + ++m_freeCount; + btMutexUnlock(&m_mutex); + } } - int getElementSize() const + int getElementSize() const { return m_elemSize; } - unsigned char* getPoolAddress() + unsigned char* getPoolAddress() { return m_pool; } - const unsigned char* getPoolAddress() const + const unsigned char* getPoolAddress() const { return m_pool; } - }; -#endif //_BT_POOL_ALLOCATOR_H +#endif //_BT_POOL_ALLOCATOR_H diff --git a/Engine/lib/bullet/src/LinearMath/btQuadWord.h b/Engine/lib/bullet/src/LinearMath/btQuadWord.h index fcfb3be44..ab2d3175a 100644 --- a/Engine/lib/bullet/src/LinearMath/btQuadWord.h +++ b/Engine/lib/bullet/src/LinearMath/btQuadWord.h @@ -12,18 +12,13 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_SIMD_QUADWORD_H #define BT_SIMD_QUADWORD_H #include "btScalar.h" #include "btMinMax.h" - - - - -#if defined (__CELLOS_LV2) && defined (__SPU__) +#if defined(__CELLOS_LV2) && defined(__SPU__) #include #endif @@ -31,51 +26,53 @@ subject to the following restrictions: * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. */ #ifndef USE_LIBSPE2 -ATTRIBUTE_ALIGNED16(class) btQuadWord +ATTRIBUTE_ALIGNED16(class) +btQuadWord #else class btQuadWord #endif { protected: - -#if defined (__SPU__) && defined (__CELLOS_LV2__) +#if defined(__SPU__) && defined(__CELLOS_LV2__) union { vec_float4 mVec128; - btScalar m_floats[4]; + btScalar m_floats[4]; }; + public: - vec_float4 get128() const + vec_float4 get128() const { return mVec128; } -protected: -#else //__CELLOS_LV2__ __SPU__ -#if defined(BT_USE_SSE) || defined(BT_USE_NEON) +protected: +#else //__CELLOS_LV2__ __SPU__ + +#if defined(BT_USE_SSE) || defined(BT_USE_NEON) union { btSimdFloat4 mVec128; - btScalar m_floats[4]; + btScalar m_floats[4]; }; + public: - SIMD_FORCE_INLINE btSimdFloat4 get128() const + SIMD_FORCE_INLINE btSimdFloat4 get128() const { return mVec128; } - SIMD_FORCE_INLINE void set128(btSimdFloat4 v128) + SIMD_FORCE_INLINE void set128(btSimdFloat4 v128) { mVec128 = v128; } #else - btScalar m_floats[4]; -#endif // BT_USE_SSE + btScalar m_floats[4]; +#endif // BT_USE_SSE -#endif //__CELLOS_LV2__ __SPU__ +#endif //__CELLOS_LV2__ __SPU__ - public: - +public: #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) - // Set Vector + // Set Vector SIMD_FORCE_INLINE btQuadWord(const btSimdFloat4 vec) { mVec128 = vec; @@ -88,157 +85,154 @@ public: } // Assignment Operator - SIMD_FORCE_INLINE btQuadWord& - operator=(const btQuadWord& v) + SIMD_FORCE_INLINE btQuadWord& + operator=(const btQuadWord& v) { mVec128 = v.mVec128; - + return *this; } - + #endif - /**@brief Return the x value */ - SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } - /**@brief Return the y value */ - SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } - /**@brief Return the z value */ - SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } - /**@brief Set the x value */ - SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x;}; - /**@brief Set the y value */ - SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y;}; - /**@brief Set the z value */ - SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z;}; - /**@brief Set the w value */ - SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w;}; - /**@brief Return the x value */ - SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } - /**@brief Return the y value */ - SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } - /**@brief Return the z value */ - SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } - /**@brief Return the w value */ - SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x; }; + /**@brief Set the y value */ + SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y; }; + /**@brief Set the z value */ + SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z; }; + /**@brief Set the w value */ + SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w; }; + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } + /**@brief Return the w value */ + SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } - //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } + //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. - SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } - SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } + SIMD_FORCE_INLINE operator btScalar*() { return &m_floats[0]; } + SIMD_FORCE_INLINE operator const btScalar*() const { return &m_floats[0]; } - SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const + SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const { #ifdef BT_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])); + 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 } - SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const + SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const { return !(*this == other); } - /**@brief Set x,y,z and zero w + /**@brief Set x,y,z and zero w * @param x Value of x * @param y Value of y * @param z Value of z */ - SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z) - { - m_floats[0]=_x; - m_floats[1]=_y; - m_floats[2]=_z; - m_floats[3] = 0.f; - } + SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z) + { + m_floats[0] = _x; + m_floats[1] = _y; + m_floats[2] = _z; + m_floats[3] = 0.f; + } -/* void getValue(btScalar *m) const + /* void getValue(btScalar *m) const { m[0] = m_floats[0]; m[1] = m_floats[1]; m[2] = m_floats[2]; } */ -/**@brief Set the values + /**@brief Set the values * @param x Value of x * @param y Value of y * @param z Value of z * @param w Value of w */ - SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w) - { - m_floats[0]=_x; - m_floats[1]=_y; - m_floats[2]=_z; - m_floats[3]=_w; - } - /**@brief No initialization constructor */ - SIMD_FORCE_INLINE btQuadWord() - // :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.)) - { - } - - /**@brief Three argument constructor (zeros w) + SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w) + { + m_floats[0] = _x; + m_floats[1] = _y; + m_floats[2] = _z; + m_floats[3] = _w; + } + /**@brief No initialization constructor */ + SIMD_FORCE_INLINE btQuadWord() + // :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.)) + { + } + + /**@brief Three argument constructor (zeros w) * @param x Value of x * @param y Value of y * @param z Value of z */ - SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z) - { - m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f; - } + SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z) + { + m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f; + } -/**@brief Initializing constructor + /**@brief Initializing constructor * @param x Value of x * @param y Value of y * @param z Value of z * @param w Value of w */ - SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w) - { - m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w; - } + SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w) + { + m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w; + } - /**@brief Set each element to the max of the current values and the values of another btQuadWord + /**@brief Set each element to the max of the current values and the values of another btQuadWord * @param other The other btQuadWord to compare with */ - SIMD_FORCE_INLINE void setMax(const btQuadWord& other) - { - #ifdef BT_USE_SSE - mVec128 = _mm_max_ps(mVec128, other.mVec128); - #elif defined(BT_USE_NEON) - mVec128 = vmaxq_f32(mVec128, other.mVec128); - #else - btSetMax(m_floats[0], other.m_floats[0]); - btSetMax(m_floats[1], other.m_floats[1]); - btSetMax(m_floats[2], other.m_floats[2]); - btSetMax(m_floats[3], other.m_floats[3]); - #endif - } - /**@brief Set each element to the min of the current values and the values of another btQuadWord + SIMD_FORCE_INLINE void setMax(const btQuadWord& other) + { +#ifdef BT_USE_SSE + mVec128 = _mm_max_ps(mVec128, other.mVec128); +#elif defined(BT_USE_NEON) + mVec128 = vmaxq_f32(mVec128, other.mVec128); +#else + btSetMax(m_floats[0], other.m_floats[0]); + btSetMax(m_floats[1], other.m_floats[1]); + btSetMax(m_floats[2], other.m_floats[2]); + btSetMax(m_floats[3], other.m_floats[3]); +#endif + } + /**@brief Set each element to the min of the current values and the values of another btQuadWord * @param other The other btQuadWord to compare with */ - SIMD_FORCE_INLINE void setMin(const btQuadWord& other) - { - #ifdef BT_USE_SSE - mVec128 = _mm_min_ps(mVec128, other.mVec128); - #elif defined(BT_USE_NEON) - mVec128 = vminq_f32(mVec128, other.mVec128); - #else - btSetMin(m_floats[0], other.m_floats[0]); - btSetMin(m_floats[1], other.m_floats[1]); - btSetMin(m_floats[2], other.m_floats[2]); - btSetMin(m_floats[3], other.m_floats[3]); - #endif - } - - - + SIMD_FORCE_INLINE void setMin(const btQuadWord& other) + { +#ifdef BT_USE_SSE + mVec128 = _mm_min_ps(mVec128, other.mVec128); +#elif defined(BT_USE_NEON) + mVec128 = vminq_f32(mVec128, other.mVec128); +#else + btSetMin(m_floats[0], other.m_floats[0]); + btSetMin(m_floats[1], other.m_floats[1]); + btSetMin(m_floats[2], other.m_floats[2]); + btSetMin(m_floats[3], other.m_floats[3]); +#endif + } }; -#endif //BT_SIMD_QUADWORD_H +#endif //BT_SIMD_QUADWORD_H diff --git a/Engine/lib/bullet/src/LinearMath/btQuaternion.h b/Engine/lib/bullet/src/LinearMath/btQuaternion.h index 7bd39e6a3..53e8169b8 100644 --- a/Engine/lib/bullet/src/LinearMath/btQuaternion.h +++ b/Engine/lib/bullet/src/LinearMath/btQuaternion.h @@ -12,25 +12,19 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_SIMD__QUATERNION_H_ #define BT_SIMD__QUATERNION_H_ - #include "btVector3.h" #include "btQuadWord.h" - #ifdef BT_USE_DOUBLE_PRECISION #define btQuaternionData btQuaternionDoubleData #define btQuaternionDataName "btQuaternionDoubleData" #else #define btQuaternionData btQuaternionFloatData #define btQuaternionDataName "btQuaternionFloatData" -#endif //BT_USE_DOUBLE_PRECISION - - +#endif //BT_USE_DOUBLE_PRECISION #ifdef BT_USE_SSE @@ -39,7 +33,7 @@ subject to the following restrictions: #endif -#if defined(BT_USE_SSE) +#if defined(BT_USE_SSE) #define vQInv (_mm_set_ps(+0.0f, -0.0f, -0.0f, -0.0f)) #define vPPPM (_mm_set_ps(-0.0f, +0.0f, +0.0f, +0.0f)) @@ -52,13 +46,14 @@ const btSimdFloat4 ATTRIBUTE_ALIGNED16(vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f}; #endif /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */ -class btQuaternion : public btQuadWord { +class btQuaternion : public btQuadWord +{ public: - /**@brief No initialization constructor */ + /**@brief No initialization constructor */ btQuaternion() {} -#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))|| defined(BT_USE_NEON) - // Set Vector +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) + // Set Vector SIMD_FORCE_INLINE btQuaternion(const btSimdFloat4 vec) { mVec128 = vec; @@ -71,42 +66,43 @@ public: } // Assignment Operator - SIMD_FORCE_INLINE btQuaternion& - operator=(const btQuaternion& v) + SIMD_FORCE_INLINE btQuaternion& + operator=(const btQuaternion& v) { mVec128 = v.mVec128; - + return *this; } - + #endif // template // explicit Quaternion(const btScalar *v) : Tuple4(v) {} - /**@brief Constructor from scalars */ - btQuaternion(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w) - : btQuadWord(_x, _y, _z, _w) - {} - /**@brief Axis angle Constructor + /**@brief Constructor from scalars */ + btQuaternion(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w) + : btQuadWord(_x, _y, _z, _w) + { + } + /**@brief Axis angle Constructor * @param axis The axis which the rotation is around * @param angle The magnitude of the rotation around the angle (Radians) */ - btQuaternion(const btVector3& _axis, const btScalar& _angle) - { - setRotation(_axis, _angle); + btQuaternion(const btVector3& _axis, const btScalar& _angle) + { + setRotation(_axis, _angle); } - /**@brief Constructor from Euler angles + /**@brief Constructor from Euler angles * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */ btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) - { + { #ifndef BT_EULER_DEFAULT_ZYX - setEuler(yaw, pitch, roll); + setEuler(yaw, pitch, roll); #else - setEulerZYX(yaw, pitch, roll); -#endif + setEulerZYX(yaw, pitch, roll); +#endif } - /**@brief Set the rotation using axis angle notation + /**@brief Set the rotation using axis angle notation * @param axis The axis around which to rotate * @param angle The magnitude of the rotation in Radians */ void setRotation(const btVector3& axis, const btScalar& _angle) @@ -114,18 +110,18 @@ public: btScalar d = axis.length(); btAssert(d != btScalar(0.0)); btScalar s = btSin(_angle * btScalar(0.5)) / d; - setValue(axis.x() * s, axis.y() * s, axis.z() * s, - btCos(_angle * btScalar(0.5))); + setValue(axis.x() * s, axis.y() * s, axis.z() * s, + btCos(_angle * btScalar(0.5))); } - /**@brief Set the quaternion using Euler angles + /**@brief Set the quaternion using Euler angles * @param yaw Angle around Y * @param pitch Angle around X * @param roll Angle around Z */ void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { - btScalar halfYaw = btScalar(yaw) * btScalar(0.5); - btScalar halfPitch = btScalar(pitch) * btScalar(0.5); - btScalar halfRoll = btScalar(roll) * btScalar(0.5); + btScalar halfYaw = btScalar(yaw) * btScalar(0.5); + btScalar halfPitch = btScalar(pitch) * btScalar(0.5); + btScalar halfRoll = btScalar(roll) * btScalar(0.5); btScalar cosYaw = btCos(halfYaw); btScalar sinYaw = btSin(halfYaw); btScalar cosPitch = btCos(halfPitch); @@ -133,32 +129,32 @@ public: btScalar cosRoll = btCos(halfRoll); btScalar sinRoll = btSin(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); + 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 + /**@brief Set the quaternion using euler angles * @param yaw Angle around Z * @param pitch Angle around Y * @param roll Angle around X */ void setEulerZYX(const btScalar& yawZ, const btScalar& pitchY, const btScalar& rollX) { - btScalar halfYaw = btScalar(yawZ) * btScalar(0.5); - btScalar halfPitch = btScalar(pitchY) * btScalar(0.5); - btScalar halfRoll = btScalar(rollX) * 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); btScalar sinPitch = btSin(halfPitch); btScalar cosRoll = btCos(halfRoll); btScalar sinRoll = btSin(halfRoll); - setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x - cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y - cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z - cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx + setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x + cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y + cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z + cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx } - /**@brief Get the euler angles from this quaternion + /**@brief Get the euler angles from this quaternion * @param yaw Angle around Z * @param pitch Angle around Y * @param roll Angle around X */ @@ -173,184 +169,204 @@ public: 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); + + // If the pitch angle is PI/2 or -PI/2, we can only compute + // the sum roll + yaw. However, any combination that gives + // the right sum will produce the correct orientation, so we + // set rollX = 0 and compute yawZ. + if (sarg <= -btScalar(0.99999)) + { + pitchY = btScalar(-0.5) * SIMD_PI; + rollX = 0; + yawZ = btScalar(2) * btAtan2(m_floats[0], -m_floats[1]); + } + else if (sarg >= btScalar(0.99999)) + { + pitchY = btScalar(0.5) * SIMD_PI; + rollX = 0; + yawZ = btScalar(2) * btAtan2(-m_floats[0], m_floats[1]); + } + else + { + pitchY = btAsin(sarg); + rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz); + yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz); + } } - /**@brief Add two quaternions + /**@brief Add two quaternions * @param q The quaternion to add to this one */ - SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) + SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) mVec128 = _mm_add_ps(mVec128, q.mVec128); #elif defined(BT_USE_NEON) mVec128 = vaddq_f32(mVec128, q.mVec128); -#else - m_floats[0] += q.x(); - m_floats[1] += q.y(); - m_floats[2] += q.z(); - m_floats[3] += q.m_floats[3]; +#else + m_floats[0] += q.x(); + m_floats[1] += q.y(); + m_floats[2] += q.z(); + m_floats[3] += q.m_floats[3]; #endif return *this; } - /**@brief Subtract out a quaternion + /**@brief Subtract out a quaternion * @param q The quaternion to subtract from this one */ - btQuaternion& operator-=(const btQuaternion& q) + btQuaternion& operator-=(const btQuaternion& q) { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) mVec128 = _mm_sub_ps(mVec128, q.mVec128); #elif defined(BT_USE_NEON) mVec128 = vsubq_f32(mVec128, q.mVec128); -#else - m_floats[0] -= q.x(); - m_floats[1] -= q.y(); - m_floats[2] -= q.z(); - m_floats[3] -= q.m_floats[3]; +#else + m_floats[0] -= q.x(); + m_floats[1] -= q.y(); + m_floats[2] -= q.z(); + m_floats[3] -= q.m_floats[3]; #endif - return *this; + return *this; } - /**@brief Scale this quaternion + /**@brief Scale this quaternion * @param s The scalar to scale by */ btQuaternion& operator*=(const btScalar& s) { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - __m128 vs = _mm_load_ss(&s); // (S 0 0 0) - vs = bt_pshufd_ps(vs, 0); // (S S S S) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) + __m128 vs = _mm_load_ss(&s); // (S 0 0 0) + vs = bt_pshufd_ps(vs, 0); // (S S S S) mVec128 = _mm_mul_ps(mVec128, vs); #elif defined(BT_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; + 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 + /**@brief Multiply this quaternion by q on the right * @param q The other quaternion * Equivilant to this = this * q */ btQuaternion& operator*=(const btQuaternion& q) { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) __m128 vQ2 = q.get128(); - - __m128 A1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(0,1,2,0)); - __m128 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); - + + __m128 A1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(0, 1, 2, 0)); + __m128 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3, 3, 3, 0)); + A1 = A1 * B1; - - __m128 A2 = bt_pshufd_ps(mVec128, BT_SHUFFLE(1,2,0,1)); - __m128 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1)); - + + __m128 A2 = bt_pshufd_ps(mVec128, BT_SHUFFLE(1, 2, 0, 1)); + __m128 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2, 0, 1, 1)); + A2 = A2 * B2; - - B1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(2,0,1,2)); - B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2)); - - B1 = B1 * B2; // A3 *= B3 - - mVec128 = bt_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, vPPPM); // change sign of the last element - mVec128 = mVec128+ A1; // AB03 + AB12 -#elif defined(BT_USE_NEON) + B1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(2, 0, 1, 2)); + B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1, 2, 0, 2)); - 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]; + B1 = B1 * B2; // A3 *= B3 - 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); + mVec128 = bt_splat_ps(mVec128, 3); // A0 + mVec128 = mVec128 * vQ2; // A0 * B0 - vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1); + A1 = A1 + A2; // AB12 + mVec128 = mVec128 - B1; // AB03 = AB0 - AB3 + A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element + mVec128 = mVec128 + A1; // AB03 + AB12 - vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1); - vQ2xz = vext_f32(vQ2zx, vQ2zx, 1); +#elif defined(BT_USE_NEON) - 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 + 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; - A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1)); - B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1)); + { + float32x2x2_t tmp; + tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y} + vQ1zx = tmp.val[0]; - A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z - B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z + 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); - 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 + vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1); - A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2 - A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3 - - // change the sign of the last element - A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM); - A0 = vaddq_f32(A0, A1); // AB03 + AB12 - - mVec128 = A0; + 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 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM); + A0 = vaddq_f32(A0, A1); // AB03 + AB12 + + mVec128 = A0; #else setValue( - m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), + m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); #endif return *this; } - /**@brief Return the dot product between this quaternion and another + /**@brief Return the dot product between this quaternion and another * @param q The other quaternion */ btScalar dot(const btQuaternion& q) const { -#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - __m128 vd; - +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) + __m128 vd; + vd = _mm_mul_ps(mVec128, q.mVec128); - - __m128 t = _mm_movehl_ps(vd, vd); + + __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); + + return _mm_cvtss_f32(vd); #elif defined(BT_USE_NEON) float32x4_t vd = vmulq_f32(mVec128, q.mVec128); - float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd)); + 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.x() + - m_floats[1] * q.y() + - m_floats[2] * q.z() + - m_floats[3] * q.m_floats[3]; +#else + return m_floats[0] * q.x() + + m_floats[1] * q.y() + + m_floats[2] * q.z() + + m_floats[3] * q.m_floats[3]; #endif } - /**@brief Return the length squared of the quaternion */ + /**@brief Return the length squared of the quaternion */ btScalar length2() const { return dot(*this); } - /**@brief Return the length of the quaternion */ + /**@brief Return the length of the quaternion */ btScalar length() const { return btSqrt(length2()); @@ -358,46 +374,46 @@ public: btQuaternion& safeNormalize() { btScalar l2 = length2(); - if (l2>SIMD_EPSILON) + if (l2 > SIMD_EPSILON) { normalize(); } return *this; } - /**@brief Normalize the quaternion + /**@brief Normalize the quaternion * Such that x^2 + y^2 + z^2 +w^2 = 1 */ - btQuaternion& normalize() + btQuaternion& normalize() { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - __m128 vd; - +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) + __m128 vd; + vd = _mm_mul_ps(mVec128, mVec128); - - __m128 t = _mm_movehl_ps(vd, vd); + + __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(vOnes, vd); - vd = bt_pshufd_ps(vd, 0); // splat + vd = bt_pshufd_ps(vd, 0); // splat mVec128 = _mm_mul_ps(mVec128, vd); - + return *this; -#else +#else return *this /= length(); #endif } - /**@brief Return a scaled version of this quaternion + /**@brief Return a scaled version of this quaternion * @param s The scale factor */ SIMD_FORCE_INLINE btQuaternion operator*(const btScalar& s) const { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - __m128 vs = _mm_load_ss(&s); // (S 0 0 0) - vs = bt_pshufd_ps(vs, 0x00); // (S S S S) - +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) + __m128 vs = _mm_load_ss(&s); // (S 0 0 0) + vs = bt_pshufd_ps(vs, 0x00); // (S S S S) + return btQuaternion(_mm_mul_ps(mVec128, vs)); #elif defined(BT_USE_NEON) return btQuaternion(vmulq_n_f32(mVec128, s)); @@ -406,7 +422,7 @@ public: #endif } - /**@brief Return an inversely scaled versionof this quaternion + /**@brief Return an inversely scaled versionof this quaternion * @param s The inverse scale factor */ btQuaternion operator/(const btScalar& s) const { @@ -414,49 +430,49 @@ public: return *this * (btScalar(1.0) / s); } - /**@brief Inversely scale this quaternion + /**@brief Inversely scale this quaternion * @param s The scale factor */ - btQuaternion& operator/=(const btScalar& s) + btQuaternion& operator/=(const btScalar& s) { btAssert(s != btScalar(0.0)); return *this *= btScalar(1.0) / s; } - /**@brief Return a normalized version of this quaternion */ - btQuaternion normalized() const + /**@brief Return a normalized version of this quaternion */ + btQuaternion normalized() const { return *this / length(); - } + } /**@brief Return the ***half*** angle between this quaternion and the other * @param q The other quaternion */ - btScalar angle(const btQuaternion& q) const + btScalar angle(const btQuaternion& q) const { btScalar s = btSqrt(length2() * q.length2()); btAssert(s != btScalar(0.0)); return btAcos(dot(q) / s); } - + /**@brief Return the angle between this quaternion and the other along the shortest path * @param q The other quaternion */ - btScalar angleShortestPath(const btQuaternion& q) const + btScalar angleShortestPath(const btQuaternion& q) const { btScalar s = btSqrt(length2() * q.length2()); btAssert(s != btScalar(0.0)); - if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp return btAcos(dot(-q) / s) * btScalar(2.0); - else + else return btAcos(dot(q) / s) * btScalar(2.0); } /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */ - btScalar getAngle() const + btScalar getAngle() const { btScalar s = btScalar(2.) * btAcos(m_floats[3]); return s; } /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */ - btScalar getAngleShortestPath() const + btScalar getAngleShortestPath() const { btScalar s; if (m_floats[3] >= 0) @@ -466,120 +482,117 @@ public: return s; } - /**@brief Return the axis of the rotation represented by this quaternion */ btVector3 getAxis() const { - btScalar s_squared = 1.f-m_floats[3]*m_floats[3]; - - if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero - return btVector3(1.0, 0.0, 0.0); // Arbitrary - btScalar s = 1.f/btSqrt(s_squared); + btScalar s_squared = 1.f - m_floats[3] * m_floats[3]; + + if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero + return btVector3(1.0, 0.0, 0.0); // Arbitrary + btScalar s = 1.f / btSqrt(s_squared); return btVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s); } /**@brief Return the inverse of this quaternion */ btQuaternion inverse() const { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) return btQuaternion(_mm_xor_ps(mVec128, vQInv)); #elif defined(BT_USE_NEON) - return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv)); -#else + return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv)); +#else return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); #endif } - /**@brief Return the sum of this quaternion and the other + /**@brief Return the sum of this quaternion and the other * @param q2 The other quaternion */ SIMD_FORCE_INLINE btQuaternion operator+(const btQuaternion& q2) const { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) return btQuaternion(_mm_add_ps(mVec128, q2.mVec128)); #elif defined(BT_USE_NEON) - return btQuaternion(vaddq_f32(mVec128, q2.mVec128)); -#else + return btQuaternion(vaddq_f32(mVec128, q2.mVec128)); +#else const btQuaternion& q1 = *this; return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); #endif } - /**@brief Return the difference between this quaternion and the other + /**@brief Return the difference between this quaternion and the other * @param q2 The other quaternion */ SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q2) const { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) return btQuaternion(_mm_sub_ps(mVec128, q2.mVec128)); #elif defined(BT_USE_NEON) - return btQuaternion(vsubq_f32(mVec128, q2.mVec128)); -#else + return btQuaternion(vsubq_f32(mVec128, q2.mVec128)); +#else const btQuaternion& q1 = *this; return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); #endif } - /**@brief Return the negative of this quaternion + /**@brief Return the negative of this quaternion * This simply negates each element */ SIMD_FORCE_INLINE btQuaternion operator-() const { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) return btQuaternion(_mm_xor_ps(mVec128, btvMzeroMask)); #elif defined(BT_USE_NEON) - return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask) ); -#else + return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask)); +#else const btQuaternion& q2 = *this; - return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); + return btQuaternion(-q2.x(), -q2.y(), -q2.z(), -q2.m_floats[3]); #endif } - /**@todo document this and it's use */ - SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const + /**@todo document this and it's use */ + SIMD_FORCE_INLINE btQuaternion farthest(const btQuaternion& qd) const { - btQuaternion diff,sum; + btQuaternion diff, sum; diff = *this - qd; sum = *this + qd; - if( diff.dot(diff) > sum.dot(sum) ) + if (diff.dot(diff) > sum.dot(sum)) return qd; return (-qd); } /**@todo document this and it's use */ - SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const + SIMD_FORCE_INLINE btQuaternion nearest(const btQuaternion& qd) const { - btQuaternion diff,sum; + btQuaternion diff, sum; diff = *this - qd; sum = *this + qd; - if( diff.dot(diff) < sum.dot(sum) ) + 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 + /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion * @param q The other quaternion to interpolate with * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. * Slerp interpolates assuming constant velocity. */ btQuaternion slerp(const btQuaternion& q, const btScalar& t) const { - 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)) + + if (absproduct < btScalar(1.0 - SIMD_EPSILON)) { // 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), @@ -592,312 +605,308 @@ public: } } - static const btQuaternion& getIdentity() + static const btQuaternion& getIdentity() { - static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.)); + static const btQuaternion identityQuat(btScalar(0.), btScalar(0.), btScalar(0.), btScalar(1.)); return identityQuat; } SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; } - SIMD_FORCE_INLINE void serialize(struct btQuaternionData& dataOut) const; + 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 serializeFloat(struct btQuaternionFloatData& dataOut) const; + SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionDoubleData& dataIn); - SIMD_FORCE_INLINE void deSerializeFloat(const struct btQuaternionFloatData& dataIn); + SIMD_FORCE_INLINE void serializeFloat(struct btQuaternionFloatData& dataOut) const; - SIMD_FORCE_INLINE void serializeDouble(struct btQuaternionDoubleData& dataOut) const; + SIMD_FORCE_INLINE void deSerializeFloat(const struct btQuaternionFloatData& dataIn); - SIMD_FORCE_INLINE void deSerializeDouble(const struct btQuaternionDoubleData& dataIn); + SIMD_FORCE_INLINE void serializeDouble(struct btQuaternionDoubleData& dataOut) const; + SIMD_FORCE_INLINE void deSerializeDouble(const struct btQuaternionDoubleData& dataIn); }; - - - - /**@brief Return the product of two quaternions */ SIMD_FORCE_INLINE btQuaternion -operator*(const btQuaternion& q1, const btQuaternion& q2) +operator*(const btQuaternion& q1, const btQuaternion& q2) { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) __m128 vQ1 = q1.get128(); __m128 vQ2 = q2.get128(); __m128 A0, A1, B1, A2, B2; - - A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y z x // vtrn - B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W W X // vdup vext + + A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0, 1, 2, 0)); // X Y z x // vtrn + B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3, 3, 3, 0)); // W W W X // vdup vext A1 = A1 * B1; - - A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1)); // Y Z X Y // vext - B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1)); // z x Y Y // vtrn vdup + + A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1, 2, 0, 1)); // Y Z X Y // vext + B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2, 0, 1, 1)); // z x Y Y // vtrn vdup A2 = A2 * B2; - B1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2)); // z x Y Z // vtrn vext - B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2)); // Y Z x z // vext vtrn - - B1 = B1 * B2; // A3 *= B3 + B1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2, 0, 1, 2)); // z x Y Z // vtrn vext + B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1, 2, 0, 2)); // Y Z x z // vext vtrn - A0 = bt_splat_ps(vQ1, 3); // A0 - A0 = A0 * vQ2; // A0 * B0 + B1 = B1 * B2; // A3 *= B3 + + A0 = bt_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, vPPPM); // change sign of the last element + A0 = A0 + A1; // AB03 + AB12 - A1 = A1 + A2; // AB12 - A0 = A0 - B1; // AB03 = AB0 - AB3 - - A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element - A0 = A0 + A1; // AB03 + AB12 - return btQuaternion(A0); -#elif defined(BT_USE_NEON) +#elif defined(BT_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]; + float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz; - 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); + { + float32x2x2_t tmp; + 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); + 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); - vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1); - vQ2xz = vext_f32(vQ2zx, vQ2zx, 1); + vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 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 + 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)); + 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 + 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 + 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 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM); + A0 = vaddq_f32(A0, A1); // AB03 + AB12 - A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2 - A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3 - - // change the sign of the last element - A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM); - A0 = vaddq_f32(A0, A1); // AB03 + AB12 - return btQuaternion(A0); #else return btQuaternion( - q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), + q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(), q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(), - q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); + q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); #endif } SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q, const btVector3& w) { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) __m128 vQ1 = q.get128(); __m128 vQ2 = w.get128(); __m128 A1, B1, A2, B2, A3, B3; - - A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(3,3,3,0)); - B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(0,1,2,0)); + + A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(3, 3, 3, 0)); + B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(0, 1, 2, 0)); A1 = A1 * B1; - - A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1)); - B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1)); + + A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1, 2, 0, 1)); + B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2, 0, 1, 1)); A2 = A2 * B2; - A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2)); - B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2)); - - A3 = A3 * B3; // A3 *= B3 + A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2, 0, 1, 2)); + B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1, 2, 0, 2)); + + A3 = A3 * B3; // A3 *= B3 + + A1 = A1 + A2; // AB12 + A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element + A1 = A1 - A3; // AB123 = AB12 - AB3 - A1 = A1 + A2; // AB12 - A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element - A1 = A1 - A3; // AB123 = AB12 - AB3 - return btQuaternion(A1); - -#elif defined(BT_USE_NEON) + +#elif defined(BT_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; + float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz; - tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y} - vQ2zx = tmp.val[0]; + vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1); + { + 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]; - vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1); + tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y} + vQ1zx = tmp.val[0]; + } - vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1); - vQ2xz = vext_f32(vQ2zx, vQ2zx, 1); + vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 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 + 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)); + 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 + 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 + A3 = vmulq_f32(A3, B3); // A3 *= B3 + + A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2 + + // change the sign of the last element + A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM); + + A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3 - A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2 - - // change the sign of the last element - A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM); - - A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3 - return btQuaternion(A1); - + #else - return btQuaternion( - q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), - q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), - q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), - -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); + return btQuaternion( + q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), + q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), + q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), + -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); #endif } SIMD_FORCE_INLINE btQuaternion operator*(const btVector3& w, const btQuaternion& q) { -#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) __m128 vQ1 = w.get128(); __m128 vQ2 = q.get128(); __m128 A1, B1, A2, B2, A3, B3; - - A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y z x - B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W W X + + A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0, 1, 2, 0)); // X Y z x + B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3, 3, 3, 0)); // W W W X A1 = A1 * B1; - - A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1)); - B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1)); - A2 = A2 *B2; + A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1, 2, 0, 1)); + B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2, 0, 1, 1)); - A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2)); - B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2)); - - A3 = A3 * B3; // A3 *= B3 + A2 = A2 * B2; + + A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2, 0, 1, 2)); + B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1, 2, 0, 2)); + + A3 = A3 * B3; // A3 *= B3 + + A1 = A1 + A2; // AB12 + A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element + A1 = A1 - A3; // AB123 = AB12 - AB3 - A1 = A1 + A2; // AB12 - A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element - A1 = A1 - A3; // AB123 = AB12 - AB3 - return btQuaternion(A1); -#elif defined(BT_USE_NEON) +#elif defined(BT_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]; + float32x4_t A1, B1, A2, B2, A3, B3; + float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz; - 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); + { + float32x2x2_t tmp; - vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1); + tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y} + vQ1zx = tmp.val[0]; - vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1); - vQ2xz = vext_f32(vQ2zx, vQ2zx, 1); + 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); - 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 + 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)); + 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 + 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 + A3 = vmulq_f32(A3, B3); // A3 *= B3 + + A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2 + + // change the sign of the last element + A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM); + + A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3 - A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2 - - // change the sign of the last element - A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM); - - A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3 - return btQuaternion(A1); - + #else - return btQuaternion( - +w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), + return btQuaternion( + +w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), +w.y() * q.w() + w.z() * q.x() - w.x() * q.z(), +w.z() * q.w() + w.x() * q.y() - w.y() * q.x(), - -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); + -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); #endif } /**@brief Calculate the dot product between two quaternions */ -SIMD_FORCE_INLINE btScalar -dot(const btQuaternion& q1, const btQuaternion& q2) -{ - return q1.dot(q2); +SIMD_FORCE_INLINE btScalar +dot(const btQuaternion& q1, const btQuaternion& q2) +{ + return q1.dot(q2); } - /**@brief Return the length of a quaternion */ SIMD_FORCE_INLINE btScalar -length(const btQuaternion& q) -{ - return q.length(); +length(const btQuaternion& q) +{ + return q.length(); } /**@brief Return the angle between two quaternions*/ SIMD_FORCE_INLINE btScalar -btAngle(const btQuaternion& q1, const btQuaternion& q2) -{ - return q1.angle(q2); +btAngle(const btQuaternion& q1, const btQuaternion& q2) +{ + return q1.angle(q2); } /**@brief Return the inverse of a quaternion*/ SIMD_FORCE_INLINE btQuaternion -inverse(const btQuaternion& q) +inverse(const btQuaternion& q) { return q.inverse(); } @@ -908,109 +917,105 @@ inverse(const btQuaternion& q) * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 * Slerp assumes constant velocity between positions. */ SIMD_FORCE_INLINE btQuaternion -slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) +slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) { return q1.slerp(q2, t); } -SIMD_FORCE_INLINE btVector3 -quatRotate(const btQuaternion& rotation, const btVector3& v) +SIMD_FORCE_INLINE btVector3 +quatRotate(const btQuaternion& rotation, const btVector3& v) { btQuaternion q = rotation * v; q *= rotation.inverse(); -#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) return btVector3(_mm_and_ps(q.get128(), btvFFF0fMask)); #elif defined(BT_USE_NEON) - return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask)); -#else - return btVector3(q.getX(),q.getY(),q.getZ()); + return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask)); +#else + return btVector3(q.getX(), q.getY(), q.getZ()); #endif } -SIMD_FORCE_INLINE btQuaternion -shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized +SIMD_FORCE_INLINE btQuaternion +shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized { btVector3 c = v0.cross(v1); - btScalar d = v0.dot(v1); + btScalar d = v0.dot(v1); if (d < -1.0 + SIMD_EPSILON) { - btVector3 n,unused; - btPlaneSpace1(v0,n,unused); - return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0 + btVector3 n, unused; + btPlaneSpace1(v0, n, unused); + return btQuaternion(n.x(), n.y(), n.z(), 0.0f); // just pick any vector that is orthogonal to v0 } - btScalar s = btSqrt((1.0f + d) * 2.0f); + btScalar s = btSqrt((1.0f + d) * 2.0f); btScalar rs = 1.0f / s; - return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); + return btQuaternion(c.getX() * rs, c.getY() * rs, c.getZ() * rs, s * 0.5f); } -SIMD_FORCE_INLINE btQuaternion -shortestArcQuatNormalize2(btVector3& v0,btVector3& v1) +SIMD_FORCE_INLINE btQuaternion +shortestArcQuatNormalize2(btVector3& v0, btVector3& v1) { v0.normalize(); v1.normalize(); - return shortestArcQuat(v0,v1); + return shortestArcQuat(v0, v1); } - - - -struct btQuaternionFloatData +struct btQuaternionFloatData { - float m_floats[4]; + float m_floats[4]; }; -struct btQuaternionDoubleData +struct btQuaternionDoubleData { - double m_floats[4]; - + double m_floats[4]; }; -SIMD_FORCE_INLINE void btQuaternion::serializeFloat(struct btQuaternionFloatData& dataOut) const +SIMD_FORCE_INLINE void btQuaternion::serializeFloat(struct btQuaternionFloatData& dataOut) const { ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) dataOut.m_floats[i] = float(m_floats[i]); } -SIMD_FORCE_INLINE void btQuaternion::deSerializeFloat(const struct btQuaternionFloatData& dataIn) +SIMD_FORCE_INLINE void btQuaternion::deSerializeFloat(const struct btQuaternionFloatData& dataIn) { - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) m_floats[i] = btScalar(dataIn.m_floats[i]); } - -SIMD_FORCE_INLINE void btQuaternion::serializeDouble(struct btQuaternionDoubleData& dataOut) const +SIMD_FORCE_INLINE void btQuaternion::serializeDouble(struct btQuaternionDoubleData& dataOut) const { ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) dataOut.m_floats[i] = double(m_floats[i]); } -SIMD_FORCE_INLINE void btQuaternion::deSerializeDouble(const struct btQuaternionDoubleData& dataIn) +SIMD_FORCE_INLINE void btQuaternion::deSerializeDouble(const struct btQuaternionDoubleData& dataIn) { - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) m_floats[i] = btScalar(dataIn.m_floats[i]); } - -SIMD_FORCE_INLINE void btQuaternion::serialize(struct btQuaternionData& dataOut) const +SIMD_FORCE_INLINE void btQuaternion::serialize(struct btQuaternionData& dataOut) const { ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) 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] = dataIn.m_floats[i]; + 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] = (btScalar)dataIn.m_floats[i]; +} -#endif //BT_SIMD__QUATERNION_H_ - - - +#endif //BT_SIMD__QUATERNION_H_ diff --git a/Engine/lib/bullet/src/LinearMath/btQuickprof.cpp b/Engine/lib/bullet/src/LinearMath/btQuickprof.cpp index aed3104a6..86fd1d781 100644 --- a/Engine/lib/bullet/src/LinearMath/btQuickprof.cpp +++ b/Engine/lib/bullet/src/LinearMath/btQuickprof.cpp @@ -16,16 +16,13 @@ #include "btQuickprof.h" #include "btThreads.h" - - - #ifdef __CELLOS_LV2__ #include #include #include #endif -#if defined (SUNOS) || defined (__SUNOS__) +#if defined(SUNOS) || defined(__SUNOS__) #include #endif #ifdef __APPLE__ @@ -42,49 +39,46 @@ #define NOIME #ifdef _XBOX - #include -#else //_XBOX - #include +#include +#else //_XBOX +#include -#if WINVER <0x0602 +#if WINVER < 0x0602 #define GetTickCount64 GetTickCount #endif -#endif //_XBOX +#endif //_XBOX #include - -#else //_WIN32 +#else //_WIN32 #include #ifdef BT_LINUX_REALTIME //required linking against rt (librt) #include -#endif //BT_LINUX_REALTIME +#endif //BT_LINUX_REALTIME -#endif //_WIN32 +#endif //_WIN32 -#define mymin(a,b) (a > b ? a : b) +#define mymin(a, b) (a > b ? a : b) struct btClockData { - #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER mClockFrequency; LONGLONG mStartTick; LARGE_INTEGER mStartTime; #else #ifdef __CELLOS_LV2__ - uint64_t mStartTime; + uint64_t mStartTime; #else #ifdef __APPLE__ - uint64_t mStartTimeNano; + uint64_t mStartTimeNano; #endif struct timeval mStartTime; #endif -#endif //__CELLOS_LV2__ - +#endif //__CELLOS_LV2__ }; ///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling. @@ -114,8 +108,7 @@ btClock& btClock::operator=(const btClock& other) return *this; } - - /// Resets the initial reference time. +/// Resets the initial reference time. void btClock::reset() { #ifdef BT_USE_WINDOWS_TIMERS @@ -124,14 +117,14 @@ void btClock::reset() #else #ifdef __CELLOS_LV2__ - typedef uint64_t ClockSize; + typedef uint64_t ClockSize; ClockSize newTime; //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); - SYS_TIMEBASE_GET( newTime ); + SYS_TIMEBASE_GET(newTime); m_data->mStartTime = newTime; #else #ifdef __APPLE__ - m_data->mStartTimeNano = mach_absolute_time(); + m_data->mStartTimeNano = mach_absolute_time(); #endif gettimeofday(&m_data->mStartTime, 0); #endif @@ -146,66 +139,66 @@ unsigned long long int btClock::getTimeMilliseconds() LARGE_INTEGER currentTime; QueryPerformanceCounter(¤tTime); LONGLONG elapsedTime = currentTime.QuadPart - - m_data->mStartTime.QuadPart; - // Compute the number of millisecond ticks elapsed. + m_data->mStartTime.QuadPart; + // Compute the number of millisecond ticks elapsed. unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / - m_data->mClockFrequency.QuadPart); + m_data->mClockFrequency.QuadPart); - return msecTicks; + return msecTicks; #else #ifdef __CELLOS_LV2__ - uint64_t freq=sys_time_get_timebase_frequency(); - double dFreq=((double) freq) / 1000.0; - typedef uint64_t ClockSize; - ClockSize newTime; - SYS_TIMEBASE_GET( newTime ); - //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + uint64_t freq = sys_time_get_timebase_frequency(); + double dFreq = ((double)freq) / 1000.0; + typedef uint64_t ClockSize; + ClockSize newTime; + SYS_TIMEBASE_GET(newTime); + //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); - return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq); + return (unsigned long int)((double(newTime - m_data->mStartTime)) / dFreq); #else - struct timeval currentTime; - gettimeofday(¤tTime, 0); - return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 + - (currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000; -#endif //__CELLOS_LV2__ + struct timeval currentTime; + gettimeofday(¤tTime, 0); + return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 + + (currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000; +#endif //__CELLOS_LV2__ #endif } - /// Returns the time in us since the last call to reset or since - /// the Clock was created. +/// Returns the time in us since the last call to reset or since +/// the Clock was created. unsigned long long int btClock::getTimeMicroseconds() { #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 *= 1000000; - elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart; + //see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx + LARGE_INTEGER currentTime, elapsedTime; - return (unsigned long long) elapsedTime.QuadPart; + QueryPerformanceCounter(¤tTime); + elapsedTime.QuadPart = currentTime.QuadPart - + m_data->mStartTime.QuadPart; + elapsedTime.QuadPart *= 1000000; + 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)/ 1000000.0; - typedef uint64_t ClockSize; - ClockSize newTime; - //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); - SYS_TIMEBASE_GET( newTime ); + uint64_t freq = sys_time_get_timebase_frequency(); + double dFreq = ((double)freq) / 1000000.0; + 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); + return (unsigned long int)((double(newTime - m_data->mStartTime)) / dFreq); #else - struct timeval currentTime; - gettimeofday(¤tTime, 0); - return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 + - (currentTime.tv_usec - m_data->mStartTime.tv_usec); -#endif//__CELLOS_LV2__ + struct timeval currentTime; + gettimeofday(¤tTime, 0); + return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 + + (currentTime.tv_usec - m_data->mStartTime.tv_usec); +#endif //__CELLOS_LV2__ #endif } @@ -213,65 +206,63 @@ 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; + LARGE_INTEGER currentTime, elapsedTime; - return (unsigned long long) elapsedTime.QuadPart; + 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 ); + 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); + 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); + 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__ - -#else//__APPLE__ - #ifdef BT_LINUX_REALTIME - timespec ts; - clock_gettime(CLOCK_REALTIME,&ts); - return 1000000000*ts.tv_sec + ts.tv_nsec; + 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 + 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 +#endif //__APPLE__ +#endif //__CELLOS_LV2__ +#endif } - -/// Returns the time in s since the last call to reset or since +/// Returns the time in s since the last call to reset or since /// the Clock was created. btScalar btClock::getTimeSeconds() { @@ -281,23 +272,19 @@ btScalar btClock::getTimeSeconds() #ifndef BT_NO_PROFILE - static btClock gProfileClock; - -inline void Profile_Get_Ticks(unsigned long int * ticks) +inline void Profile_Get_Ticks(unsigned long int* ticks) { *ticks = (unsigned long int)gProfileClock.getTimeMicroseconds(); } inline float Profile_Get_Tick_Rate(void) { -// return 1000000.f; + // return 1000000.f; return 1000.f; - } - /*************************************************************************************************** ** ** CProfileNode @@ -313,35 +300,32 @@ inline float Profile_Get_Tick_Rate(void) * The name is assumed to be a static pointer, only the pointer is stored and compared for * * efficiency reasons. * *=============================================================================================*/ -CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) : - Name( name ), - TotalCalls( 0 ), - TotalTime( 0 ), - StartTime( 0 ), - RecursionCounter( 0 ), - Parent( parent ), - Child( NULL ), - Sibling( NULL ), - m_userPtr(0) +CProfileNode::CProfileNode(const char* name, CProfileNode* parent) : Name(name), + TotalCalls(0), + TotalTime(0), + StartTime(0), + RecursionCounter(0), + Parent(parent), + Child(NULL), + Sibling(NULL), + m_userPtr(0) { Reset(); } - -void CProfileNode::CleanupMemory() +void CProfileNode::CleanupMemory() { - delete ( Child); + delete (Child); Child = NULL; - delete ( Sibling); + delete (Sibling); Sibling = NULL; } -CProfileNode::~CProfileNode( void ) +CProfileNode::~CProfileNode(void) { CleanupMemory(); } - /*********************************************************************************************** * INPUT: * * name - static string pointer to the name of the node we are searching for * @@ -350,12 +334,14 @@ CProfileNode::~CProfileNode( void ) * All profile names are assumed to be static strings so this function uses pointer compares * * to find the named node. * *=============================================================================================*/ -CProfileNode * CProfileNode::Get_Sub_Node( const char * name ) +CProfileNode* CProfileNode::Get_Sub_Node(const char* name) { // Try to find this sub node - CProfileNode * child = Child; - while ( child ) { - if ( child->Name == name ) { + CProfileNode* child = Child; + while (child) + { + if (child->Name == name) + { return child; } child = child->Sibling; @@ -363,176 +349,212 @@ CProfileNode * CProfileNode::Get_Sub_Node( const char * name ) // We didn't find it, so add it - CProfileNode * node = new CProfileNode( name, this ); + CProfileNode* node = new CProfileNode(name, this); node->Sibling = Child; Child = node; return node; } - -void CProfileNode::Reset( void ) +void CProfileNode::Reset(void) { TotalCalls = 0; TotalTime = 0.0f; - - if ( Child ) { + if (Child) + { Child->Reset(); } - if ( Sibling ) { + if (Sibling) + { Sibling->Reset(); } } - -void CProfileNode::Call( void ) +void CProfileNode::Call(void) { TotalCalls++; - if (RecursionCounter++ == 0) { + if (RecursionCounter++ == 0) + { Profile_Get_Ticks(&StartTime); } } - -bool CProfileNode::Return( void ) +bool CProfileNode::Return(void) { - if ( --RecursionCounter == 0 && TotalCalls != 0 ) { + if (--RecursionCounter == 0 && TotalCalls != 0) + { unsigned long int time; Profile_Get_Ticks(&time); - time-=StartTime; + time -= StartTime; TotalTime += (float)time / Profile_Get_Tick_Rate(); } - return ( RecursionCounter == 0 ); + return (RecursionCounter == 0); } - /*************************************************************************************************** ** ** CProfileIterator ** ***************************************************************************************************/ -CProfileIterator::CProfileIterator( CProfileNode * start ) +CProfileIterator::CProfileIterator(CProfileNode* start) { CurrentParent = start; CurrentChild = CurrentParent->Get_Child(); } - -void CProfileIterator::First(void) +void CProfileIterator::First(void) { CurrentChild = CurrentParent->Get_Child(); } - -void CProfileIterator::Next(void) +void CProfileIterator::Next(void) { CurrentChild = CurrentChild->Get_Sibling(); } - -bool CProfileIterator::Is_Done(void) +bool CProfileIterator::Is_Done(void) { return CurrentChild == NULL; } - -void CProfileIterator::Enter_Child( int index ) +void CProfileIterator::Enter_Child(int index) { CurrentChild = CurrentParent->Get_Child(); - while ( (CurrentChild != NULL) && (index != 0) ) { + while ((CurrentChild != NULL) && (index != 0)) + { index--; CurrentChild = CurrentChild->Get_Sibling(); } - if ( CurrentChild != NULL ) { + if (CurrentChild != NULL) + { CurrentParent = CurrentChild; CurrentChild = CurrentParent->Get_Child(); } } - -void CProfileIterator::Enter_Parent( void ) +void CProfileIterator::Enter_Parent(void) { - if ( CurrentParent->Get_Parent() != NULL ) { + if (CurrentParent->Get_Parent() != NULL) + { CurrentParent = CurrentParent->Get_Parent(); } CurrentChild = CurrentParent->Get_Child(); } - /*************************************************************************************************** ** ** CProfileManager ** ***************************************************************************************************/ +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 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; -CProfileNode* gCurrentNodes[BT_QUICKPROF_MAX_THREAD_COUNT]= +CProfileIterator* CProfileManager::Get_Iterator(void) { - &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 threadIndex = btQuickprofGetCurrentThreadIndex2(); + if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT) + return 0; - -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]); + return new CProfileIterator(&gRoots[threadIndex]); } -void CProfileManager::CleanupMemory(void) +void CProfileManager::CleanupMemory(void) { - for (int i=0;i= BT_QUICKPROF_MAX_THREAD_COUNT) + if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT) return; - if (name != gCurrentNodes[threadIndex]->Get_Name()) { - gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Sub_Node( name ); + if (name != gCurrentNodes[threadIndex]->Get_Name()) + { + gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Sub_Node(name); } gCurrentNodes[threadIndex]->Call(); } - /*********************************************************************************************** * CProfileManager::Stop_Profile -- Stop timing and record the results. * *=============================================================================================*/ -void CProfileManager::Stop_Profile( void ) +void CProfileManager::Stop_Profile(void) { int threadIndex = btQuickprofGetCurrentThreadIndex2(); - if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT) + 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 (gCurrentNodes[threadIndex]->Return()) { + if (gCurrentNodes[threadIndex]->Return()) + { gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Parent(); } } - - - - - /*********************************************************************************************** * CProfileManager::Reset -- Reset the contents of the profiling system * * * * This resets everything except for the tree structure. All of the timing data is reset. * *=============================================================================================*/ -void CProfileManager::Reset( void ) +void CProfileManager::Reset(void) { gProfileClock.reset(); int threadIndex = btQuickprofGetCurrentThreadIndex2(); - if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT) + if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT) return; gRoots[threadIndex].Reset(); gRoots[threadIndex].Call(); @@ -598,20 +616,18 @@ void CProfileManager::Reset( void ) Profile_Get_Ticks(&ResetTime); } - /*********************************************************************************************** * CProfileManager::Increment_Frame_Counter -- Increment the frame counter * *=============================================================================================*/ -void CProfileManager::Increment_Frame_Counter( void ) +void CProfileManager::Increment_Frame_Counter(void) { FrameCounter++; } - /*********************************************************************************************** * CProfileManager::Get_Time_Since_Reset -- returns the elapsed time since last reset * *=============================================================================================*/ -float CProfileManager::Get_Time_Since_Reset( void ) +float CProfileManager::Get_Time_Since_Reset(void) { unsigned long int time; Profile_Get_Ticks(&time); @@ -621,34 +637,34 @@ float CProfileManager::Get_Time_Since_Reset( void ) #include -void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing) +void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing) { profileIterator->First(); if (profileIterator->Is_Done()) return; - float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time(); + float accumulated_time = 0, parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time(); int i; int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); - for (i=0;iGet_Current_Parent_Name(), parent_time ); + for (i = 0; i < spacing; i++) printf("."); + printf("Profiling: %s (total running time: %.3f ms) ---\n", profileIterator->Get_Current_Parent_Name(), parent_time); float totalTime = 0.f; - int numChildren = 0; - for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next()) + for (i = 0; !profileIterator->Is_Done(); i++, profileIterator->Next()) { numChildren++; float current_total_time = profileIterator->Get_Current_Total_Time(); accumulated_time += current_total_time; float fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f; { - int i; for (i=0;iGet_Current_Name(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls()); + printf("%d -- %s (%.2f %%) :: %.3f ms / frame (%d calls)\n", i, profileIterator->Get_Current_Name(), fraction, (current_total_time / (double)frames_since_reset), profileIterator->Get_Current_Total_Calls()); totalTime += current_total_time; //recurse into children } @@ -657,95 +673,94 @@ void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spaci { //printf("what's wrong\n"); } - for (i=0;i SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); + for (i = 0; i < spacing; i++) printf("."); + printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:", parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); - for (i=0;iEnter_Child(i); - dumpRecursive(profileIterator,spacing+3); + dumpRecursive(profileIterator, spacing + 3); profileIterator->Enter_Parent(); } } - - -void CProfileManager::dumpAll() +void CProfileManager::dumpAll() { CProfileIterator* profileIterator = 0; profileIterator = CProfileManager::Get_Iterator(); - dumpRecursive(profileIterator,0); + dumpRecursive(profileIterator, 0); CProfileManager::Release_Iterator(profileIterator); } +void btEnterProfileZoneDefault(const char* name) +{ +} +void btLeaveProfileZoneDefault() +{ +} +#else +void btEnterProfileZoneDefault(const char* name) +{ +} +void btLeaveProfileZoneDefault() +{ +} +#endif //BT_NO_PROFILE + + +// 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() { -#if BT_THREADSAFE - return btGetCurrentThreadIndex(); -#else // #if BT_THREADSAFE const unsigned int kNullIndex = ~0U; -#ifdef _WIN32 - #if defined(__MINGW32__) || defined(__MINGW64__) - static __thread unsigned int sThreadIndex = kNullIndex; - #else - __declspec( thread ) static unsigned int sThreadIndex = kNullIndex; - #endif + +#if BT_THREADSAFE + return btGetCurrentThreadIndex(); #else -#ifdef __APPLE__ - #if TARGET_OS_IPHONE - unsigned int sThreadIndex = 0; - return -1; - #else - static __thread unsigned int sThreadIndex = kNullIndex; - #endif -#else//__APPLE__ -#if __linux__ +#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 -#endif//__APPLE__ - -#endif - static int gThreadCounter=0; - if ( sThreadIndex == kNullIndex ) + static int gThreadCounter = 0; + + if (sThreadIndex == kNullIndex) { sThreadIndex = gThreadCounter++; } return sThreadIndex; -#endif // #else // #if BT_THREADSAFE +#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; @@ -760,14 +775,13 @@ void btLeaveProfileZone() btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc() { - return bts_enterFunc ; + return bts_enterFunc; } btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc() { return bts_leaveFunc; } - void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc) { bts_enterFunc = enterFunc; @@ -777,13 +791,12 @@ void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc) bts_leaveFunc = leaveFunc; } -CProfileSample::CProfileSample( const char * name ) -{ +CProfileSample::CProfileSample(const char* name) +{ btEnterProfileZone(name); } -CProfileSample::~CProfileSample( void ) -{ +CProfileSample::~CProfileSample(void) +{ btLeaveProfileZone(); } - diff --git a/Engine/lib/bullet/src/LinearMath/btQuickprof.h b/Engine/lib/bullet/src/LinearMath/btQuickprof.h index 7b38d71b9..990d401d5 100644 --- a/Engine/lib/bullet/src/LinearMath/btQuickprof.h +++ b/Engine/lib/bullet/src/LinearMath/btQuickprof.h @@ -7,11 +7,9 @@ ** ***************************************************************************************************/ -// Credits: The Clock class was inspired by the Timer classes in +// Credits: The Clock class was inspired by the Timer classes in // Ogre (www.ogre3d.org). - - #ifndef BT_QUICK_PROF_H #define BT_QUICK_PROF_H @@ -34,96 +32,88 @@ public: /// Resets the initial reference time. void reset(); - /// Returns the time in ms since the last call to reset or since + /// Returns the time in ms since the last call to reset or since /// the btClock was created. unsigned long long int getTimeMilliseconds(); - /// Returns the time in us since the last call to reset or since + /// Returns the time in us since the last call to reset or since /// the Clock was created. unsigned long long int getTimeMicroseconds(); - + unsigned long long int getTimeNanoseconds(); - /// Returns the time in s since the last call to reset or since + /// Returns the time in s since the last call to reset or since /// the Clock was created. btScalar getTimeSeconds(); - + private: struct btClockData* m_data; }; -#endif //USE_BT_CLOCK +#endif //USE_BT_CLOCK -typedef void (btEnterProfileZoneFunc)(const char* msg); -typedef void (btLeaveProfileZoneFunc)(); +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_ENABLE_PROFILE +#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 +//btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined, +//otherwise returns thread index in range [0..maxThreads] +unsigned int btQuickprofGetCurrentThreadIndex2(); + +#ifndef BT_NO_PROFILE + + +#include //@todo remove this, backwards compatibility #include "btAlignedAllocator.h" #include - - - - - - - - ///A node in the Profile Hierarchy Tree -class CProfileNode { - +class CProfileNode +{ public: - CProfileNode( const char * name, CProfileNode * parent ); - ~CProfileNode( void ); + CProfileNode(const char* name, CProfileNode* parent); + ~CProfileNode(void); - CProfileNode * Get_Sub_Node( const char * name ); + CProfileNode* Get_Sub_Node(const char* name); - CProfileNode * Get_Parent( void ) { return Parent; } - CProfileNode * Get_Sibling( void ) { return Sibling; } - CProfileNode * Get_Child( void ) { return Child; } + CProfileNode* Get_Parent(void) { return Parent; } + CProfileNode* Get_Sibling(void) { return Sibling; } + CProfileNode* Get_Child(void) { return Child; } - void CleanupMemory(); - void Reset( void ); - void Call( void ); - bool Return( void ); + void CleanupMemory(); + void Reset(void); + void Call(void); + bool Return(void); + + const char* Get_Name(void) { return Name; } + int Get_Total_Calls(void) { return TotalCalls; } + float Get_Total_Time(void) { return TotalTime; } + void* GetUserPointer() const { return m_userPtr; } + void SetUserPointer(void* ptr) { m_userPtr = ptr; } - const char * Get_Name( void ) { return Name; } - int Get_Total_Calls( void ) { return TotalCalls; } - float Get_Total_Time( void ) { return TotalTime; } - void* GetUserPointer() const {return m_userPtr;} - void SetUserPointer(void* ptr) { m_userPtr = ptr;} protected: + const char* Name; + int TotalCalls; + float TotalTime; + unsigned long int StartTime; + int RecursionCounter; - const char * Name; - int TotalCalls; - float TotalTime; - unsigned long int StartTime; - int RecursionCounter; - - CProfileNode * Parent; - CProfileNode * Child; - CProfileNode * Sibling; - void* m_userPtr; + CProfileNode* Parent; + CProfileNode* Child; + CProfileNode* Sibling; + void* m_userPtr; }; ///An iterator to navigate through the tree @@ -131,91 +121,80 @@ class CProfileIterator { public: // Access all the children of the current parent - void First(void); - void Next(void); - bool Is_Done(void); - bool Is_Root(void) { return (CurrentParent->Get_Parent() == 0); } + void First(void); + void Next(void); + bool Is_Done(void); + bool Is_Root(void) { return (CurrentParent->Get_Parent() == 0); } - void Enter_Child( int index ); // Make the given child the new parent - void Enter_Largest_Child( void ); // Make the largest child the new parent - void Enter_Parent( void ); // Make the current parent's parent the new parent + void Enter_Child(int index); // Make the given child the new parent + void Enter_Largest_Child(void); // Make the largest child the new parent + void Enter_Parent(void); // Make the current parent's parent the new parent // Access the current child - const char * Get_Current_Name( void ) { return CurrentChild->Get_Name(); } - int Get_Current_Total_Calls( void ) { return CurrentChild->Get_Total_Calls(); } - float Get_Current_Total_Time( void ) { return CurrentChild->Get_Total_Time(); } + const char* Get_Current_Name(void) { return CurrentChild->Get_Name(); } + int Get_Current_Total_Calls(void) { return CurrentChild->Get_Total_Calls(); } + float Get_Current_Total_Time(void) { return CurrentChild->Get_Total_Time(); } - void* Get_Current_UserPointer( void ) { return CurrentChild->GetUserPointer(); } - void Set_Current_UserPointer(void* ptr) {CurrentChild->SetUserPointer(ptr);} + void* Get_Current_UserPointer(void) { return CurrentChild->GetUserPointer(); } + void Set_Current_UserPointer(void* ptr) { CurrentChild->SetUserPointer(ptr); } // Access the current parent - const char * Get_Current_Parent_Name( void ) { return CurrentParent->Get_Name(); } - int Get_Current_Parent_Total_Calls( void ) { return CurrentParent->Get_Total_Calls(); } - float Get_Current_Parent_Total_Time( void ) { return CurrentParent->Get_Total_Time(); } - - + const char* Get_Current_Parent_Name(void) { return CurrentParent->Get_Name(); } + int Get_Current_Parent_Total_Calls(void) { return CurrentParent->Get_Total_Calls(); } + float Get_Current_Parent_Total_Time(void) { return CurrentParent->Get_Total_Time(); } protected: + CProfileNode* CurrentParent; + CProfileNode* CurrentChild; - CProfileNode * CurrentParent; - CProfileNode * CurrentChild; - - - CProfileIterator( CProfileNode * start ); - friend class CProfileManager; + CProfileIterator(CProfileNode* start); + friend class CProfileManager; }; - ///The Manager for the Profile system -class CProfileManager { +class CProfileManager +{ public: - static void Start_Profile( const char * name ); - static void Stop_Profile( void ); + 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 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 void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); } + static CProfileIterator* Get_Iterator(void); + // { + // + // return new CProfileIterator( &Root ); + // } + static void Release_Iterator(CProfileIterator* iterator) { delete (iterator); } - static void dumpRecursive(CProfileIterator* profileIterator, int spacing); + static void dumpRecursive(CProfileIterator* profileIterator, int spacing); - static void dumpAll(); + static void dumpAll(); private: - - static int FrameCounter; - static unsigned long int ResetTime; + static int FrameCounter; + static unsigned long int ResetTime; }; - - - -#endif //#ifndef BT_NO_PROFILE +#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 { +class CProfileSample +{ public: - CProfileSample( const char * name ); + CProfileSample(const char* name); - ~CProfileSample( void ); + ~CProfileSample(void); }; -#define BT_PROFILE( name ) CProfileSample __profile( name ) - - - -#endif //BT_QUICK_PROF_H - +#define BT_PROFILE(name) CProfileSample __profile(name) +#endif //BT_QUICK_PROF_H diff --git a/Engine/lib/bullet/src/LinearMath/btRandom.h b/Engine/lib/bullet/src/LinearMath/btRandom.h index 4cbfc6bfe..e659af860 100644 --- a/Engine/lib/bullet/src/LinearMath/btRandom.h +++ b/Engine/lib/bullet/src/LinearMath/btRandom.h @@ -12,8 +12,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_GEN_RANDOM_H #define BT_GEN_RANDOM_H @@ -24,8 +22,8 @@ subject to the following restrictions: #define GEN_RAND_MAX UINT_MAX -SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { init_genrand(seed); } -SIMD_FORCE_INLINE unsigned int GEN_rand() { return genrand_int32(); } +SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { init_genrand(seed); } +SIMD_FORCE_INLINE unsigned int GEN_rand() { return genrand_int32(); } #else @@ -33,10 +31,9 @@ SIMD_FORCE_INLINE unsigned int GEN_rand() { return genrand_int #define GEN_RAND_MAX RAND_MAX -SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { srand(seed); } -SIMD_FORCE_INLINE unsigned int GEN_rand() { return rand(); } +SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { srand(seed); } +SIMD_FORCE_INLINE unsigned int GEN_rand() { return rand(); } #endif -#endif //BT_GEN_RANDOM_H - +#endif //BT_GEN_RANDOM_H diff --git a/Engine/lib/bullet/src/LinearMath/btScalar.h b/Engine/lib/bullet/src/LinearMath/btScalar.h index bffb2ce27..ba49d6700 100644 --- a/Engine/lib/bullet/src/LinearMath/btScalar.h +++ b/Engine/lib/bullet/src/LinearMath/btScalar.h @@ -25,14 +25,13 @@ subject to the following restrictions: #include /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 287 +#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 // @@ -44,16 +43,19 @@ inline int btGetVersion() // // 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__); } +#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 +#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) @@ -122,7 +124,7 @@ inline int btGetVersion() #ifdef BT_DEBUG #ifdef _MSC_VER #include - #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak(); }} + #define btAssert(x) { if(!(x)){printf("Assert " __FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak(); }} #else//_MSC_VER #include #define btAssert assert @@ -150,7 +152,7 @@ inline int btGetVersion() #ifdef __SPU__ #include #define printf spu_printf - #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} + #define btAssert(x) {if(!(x)){printf("Assert " __FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} #else #define btAssert assert #endif diff --git a/Engine/lib/bullet/src/LinearMath/btSerializer.cpp b/Engine/lib/bullet/src/LinearMath/btSerializer.cpp index fcd2255ad..18683c8fa 100644 --- a/Engine/lib/bullet/src/LinearMath/btSerializer.cpp +++ b/Engine/lib/bullet/src/LinearMath/btSerializer.cpp @@ -1,5 +1,6 @@ +// clang-format off char sBulletDNAstr[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-124),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), +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), @@ -72,528 +73,620 @@ char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),cha 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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(-32),char(1),char(8),char(1),char(-104),char(0),char(88),char(0),char(-72),char(0), -char(104),char(0),char(-16),char(1),char(-80),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(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(-76),char(1),char(-16),char(2),char(-120),char(1),char(-64),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(27),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(8),char(0),char(117),char(0),char(8),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(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(0),char(0),char(37),char(0),char(50),char(0),char(27),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(7),char(0),char(117),char(0),char(7),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(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0), -char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(8),char(0),char(126),char(0),char(8),char(0),char(127),char(0),char(8),char(0),char(111),char(0), -char(8),char(0),char(-128),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(-127),char(0),char(8),char(0),char(-126),char(0),char(8),char(0),char(-125),char(0), -char(8),char(0),char(-124),char(0),char(8),char(0),char(-123),char(0),char(8),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(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0), -char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(52),char(0),char(22),char(0), -char(7),char(0),char(126),char(0),char(7),char(0),char(127),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-128),char(0),char(7),char(0),char(115),char(0), -char(7),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(7),char(0),char(-125),char(0),char(7),char(0),char(-124),char(0),char(7),char(0),char(-123),char(0), -char(7),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(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0),char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0), -char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(53),char(0),char(2),char(0),char(51),char(0),char(-111),char(0),char(14),char(0),char(-110),char(0), -char(54),char(0),char(2),char(0),char(52),char(0),char(-111),char(0),char(13),char(0),char(-110),char(0),char(55),char(0),char(21),char(0),char(50),char(0),char(-109),char(0), -char(17),char(0),char(-108),char(0),char(13),char(0),char(-107),char(0),char(13),char(0),char(-106),char(0),char(13),char(0),char(-105),char(0),char(13),char(0),char(-104),char(0), -char(13),char(0),char(-110),char(0),char(13),char(0),char(-103),char(0),char(13),char(0),char(-102),char(0),char(13),char(0),char(-101),char(0),char(13),char(0),char(-100),char(0), -char(7),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),char(7),char(0),char(-97),char(0),char(7),char(0),char(-96),char(0),char(7),char(0),char(-95),char(0), -char(7),char(0),char(-94),char(0),char(7),char(0),char(-93),char(0),char(7),char(0),char(-92),char(0),char(7),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0), -char(56),char(0),char(22),char(0),char(49),char(0),char(-109),char(0),char(18),char(0),char(-108),char(0),char(14),char(0),char(-107),char(0),char(14),char(0),char(-106),char(0), -char(14),char(0),char(-105),char(0),char(14),char(0),char(-104),char(0),char(14),char(0),char(-110),char(0),char(14),char(0),char(-103),char(0),char(14),char(0),char(-102),char(0), -char(14),char(0),char(-101),char(0),char(14),char(0),char(-100),char(0),char(8),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(8),char(0),char(-97),char(0), -char(8),char(0),char(-96),char(0),char(8),char(0),char(-95),char(0),char(8),char(0),char(-94),char(0),char(8),char(0),char(-93),char(0),char(8),char(0),char(-92),char(0), -char(8),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(0),char(0),char(37),char(0),char(57),char(0),char(2),char(0),char(4),char(0),char(-89),char(0), -char(4),char(0),char(-88),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-87),char(0),char(55),char(0),char(-86),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0), -char(7),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0), -char(59),char(0),char(13),char(0),char(60),char(0),char(-87),char(0),char(60),char(0),char(-86),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0), -char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0), -char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(61),char(0),char(14),char(0), -char(56),char(0),char(-87),char(0),char(56),char(0),char(-86),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0), -char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),char(8),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0), -char(4),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(0),char(0),char(-75),char(0),char(62),char(0),char(3),char(0), -char(59),char(0),char(-74),char(0),char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(63),char(0),char(3),char(0),char(61),char(0),char(-74),char(0), -char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(64),char(0),char(3),char(0),char(59),char(0),char(-74),char(0),char(14),char(0),char(-73),char(0), -char(14),char(0),char(-72),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), -char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(7),char(0),char(-66),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(66),char(0),char(13),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0), -char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(7),char(0),char(-66),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(67),char(0),char(14),char(0), -char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0), -char(4),char(0),char(-67),char(0),char(8),char(0),char(-66),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(0),char(0),char(-59),char(0),char(68),char(0),char(10),char(0), -char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),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(-62),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(127),char(0), -char(69),char(0),char(11),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),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(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0), -char(7),char(0),char(127),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0), -char(19),char(0),char(-70),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(13),char(0),char(-53),char(0),char(13),char(0),char(-52),char(0), -char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(71),char(0),char(9),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0), -char(20),char(0),char(-70),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),char(14),char(0),char(-53),char(0),char(14),char(0),char(-52),char(0), -char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(72),char(0),char(5),char(0),char(70),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0), -char(7),char(0),char(-47),char(0),char(7),char(0),char(-46),char(0),char(7),char(0),char(-45),char(0),char(73),char(0),char(5),char(0),char(71),char(0),char(-49),char(0), -char(4),char(0),char(-48),char(0),char(8),char(0),char(-47),char(0),char(8),char(0),char(-46),char(0),char(8),char(0),char(-45),char(0),char(74),char(0),char(41),char(0), -char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),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(13),char(0),char(-37),char(0),char(13),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0), -char(13),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0),char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0), -char(0),char(0),char(-29),char(0),char(0),char(0),char(-59),char(0),char(13),char(0),char(-53),char(0),char(13),char(0),char(-52),char(0),char(13),char(0),char(-28),char(0), -char(13),char(0),char(-27),char(0),char(13),char(0),char(-26),char(0),char(13),char(0),char(-25),char(0),char(13),char(0),char(-24),char(0),char(13),char(0),char(-23),char(0), -char(13),char(0),char(-22),char(0),char(13),char(0),char(-21),char(0),char(13),char(0),char(-20),char(0),char(13),char(0),char(-19),char(0),char(13),char(0),char(-18),char(0), -char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0),char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0), -char(4),char(0),char(-12),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), -char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),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(14),char(0),char(-37),char(0), -char(14),char(0),char(-36),char(0),char(14),char(0),char(-35),char(0),char(14),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0), -char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0),char(0),char(0),char(-29),char(0),char(0),char(0),char(-59),char(0),char(14),char(0),char(-53),char(0), -char(14),char(0),char(-52),char(0),char(14),char(0),char(-28),char(0),char(14),char(0),char(-27),char(0),char(14),char(0),char(-26),char(0),char(14),char(0),char(-25),char(0), -char(14),char(0),char(-24),char(0),char(14),char(0),char(-23),char(0),char(14),char(0),char(-22),char(0),char(14),char(0),char(-21),char(0),char(14),char(0),char(-20),char(0), -char(14),char(0),char(-19),char(0),char(14),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0), -char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(76),char(0),char(9),char(0),char(59),char(0),char(-74),char(0), -char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),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(4),char(0),char(-50),char(0),char(77),char(0),char(9),char(0),char(61),char(0),char(-74),char(0), -char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),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(4),char(0),char(-50),char(0),char(78),char(0),char(5),char(0),char(58),char(0),char(-74),char(0), -char(13),char(0),char(-11),char(0),char(13),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0), -char(61),char(0),char(-74),char(0),char(14),char(0),char(-11),char(0),char(14),char(0),char(-10),char(0),char(8),char(0),char(-9),char(0),char(80),char(0),char(4),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(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0), -char(80),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(7),char(0),char(-99),char(0),char(7),char(0),char(1),char(1),char(4),char(0),char(2),char(1),char(4),char(0),char(53),char(0), -char(82),char(0),char(4),char(0),char(80),char(0),char(-5),char(0),char(4),char(0),char(3),char(1),char(7),char(0),char(4),char(1),char(4),char(0),char(5),char(1), -char(83),char(0),char(4),char(0),char(13),char(0),char(0),char(1),char(80),char(0),char(-5),char(0),char(4),char(0),char(6),char(1),char(7),char(0),char(7),char(1), -char(84),char(0),char(7),char(0),char(13),char(0),char(8),char(1),char(80),char(0),char(-5),char(0),char(4),char(0),char(9),char(1),char(7),char(0),char(10),char(1), -char(7),char(0),char(11),char(1),char(7),char(0),char(12),char(1),char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(13),char(1), -char(13),char(0),char(11),char(1),char(13),char(0),char(14),char(1),char(60),char(0),char(15),char(1),char(4),char(0),char(16),char(1),char(7),char(0),char(12),char(1), -char(86),char(0),char(26),char(0),char(4),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(127),char(0),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(7),char(0),char(35),char(1),char(7),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(39),char(1), -char(4),char(0),char(40),char(1),char(4),char(0),char(120),char(0),char(87),char(0),char(12),char(0),char(17),char(0),char(41),char(1),char(17),char(0),char(42),char(1), -char(17),char(0),char(43),char(1),char(13),char(0),char(44),char(1),char(13),char(0),char(45),char(1),char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1), -char(4),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1),char(7),char(0),char(10),char(1),char(4),char(0),char(53),char(0), -char(88),char(0),char(27),char(0),char(19),char(0),char(51),char(1),char(17),char(0),char(52),char(1),char(17),char(0),char(53),char(1),char(13),char(0),char(44),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(13),char(0),char(57),char(1),char(13),char(0),char(58),char(1), -char(4),char(0),char(59),char(1),char(7),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1), -char(7),char(0),char(64),char(1),char(7),char(0),char(65),char(1),char(4),char(0),char(66),char(1),char(4),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(4),char(0),char(74),char(1),char(4),char(0),char(75),char(1),char(4),char(0),char(76),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(77),char(1), -char(9),char(0),char(78),char(1),char(13),char(0),char(79),char(1),char(7),char(0),char(80),char(1),char(7),char(0),char(-125),char(0),char(7),char(0),char(81),char(1), -char(4),char(0),char(82),char(1),char(13),char(0),char(83),char(1),char(4),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(53),char(0),char(90),char(0),char(19),char(0),char(50),char(0),char(-109),char(0),char(87),char(0),char(87),char(1),char(80),char(0),char(88),char(1), -char(81),char(0),char(89),char(1),char(82),char(0),char(90),char(1),char(83),char(0),char(91),char(1),char(84),char(0),char(92),char(1),char(85),char(0),char(93),char(1), -char(88),char(0),char(94),char(1),char(89),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(62),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(4),char(0),char(101),char(1),char(4),char(0),char(102),char(1), -char(86),char(0),char(103),char(1),char(91),char(0),char(24),char(0),char(16),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1), -char(14),char(0),char(107),char(1),char(14),char(0),char(108),char(1),char(14),char(0),char(109),char(1),char(8),char(0),char(110),char(1),char(4),char(0),char(111),char(1), -char(4),char(0),char(86),char(1),char(4),char(0),char(112),char(1),char(4),char(0),char(113),char(1),char(8),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(8),char(0),char(118),char(1),char(8),char(0),char(119),char(1),char(8),char(0),char(120),char(1), -char(8),char(0),char(121),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(123),char(1),char(0),char(0),char(124),char(1),char(49),char(0),char(125),char(1), -char(0),char(0),char(126),char(1),char(92),char(0),char(24),char(0),char(15),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1), -char(13),char(0),char(107),char(1),char(13),char(0),char(108),char(1),char(13),char(0),char(109),char(1),char(4),char(0),char(112),char(1),char(7),char(0),char(110),char(1), -char(4),char(0),char(111),char(1),char(4),char(0),char(86),char(1),char(7),char(0),char(114),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1), -char(4),char(0),char(113),char(1),char(7),char(0),char(117),char(1),char(7),char(0),char(118),char(1),char(7),char(0),char(119),char(1),char(7),char(0),char(120),char(1), -char(7),char(0),char(121),char(1),char(7),char(0),char(122),char(1),char(0),char(0),char(123),char(1),char(0),char(0),char(124),char(1),char(50),char(0),char(125),char(1), -char(0),char(0),char(126),char(1),char(93),char(0),char(9),char(0),char(20),char(0),char(127),char(1),char(14),char(0),char(-128),char(1),char(8),char(0),char(-127),char(1), -char(0),char(0),char(-126),char(1),char(91),char(0),char(90),char(1),char(49),char(0),char(-125),char(1),char(0),char(0),char(126),char(1),char(4),char(0),char(97),char(1), -char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),char(0),char(0),char(-126),char(1),char(92),char(0),char(90),char(1),char(50),char(0),char(-125),char(1), -char(19),char(0),char(127),char(1),char(13),char(0),char(-128),char(1),char(7),char(0),char(-127),char(1),char(4),char(0),char(97),char(1),}; +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); + +// clang-format on diff --git a/Engine/lib/bullet/src/LinearMath/btSerializer.h b/Engine/lib/bullet/src/LinearMath/btSerializer.h index 89b4d7468..ba3444161 100644 --- a/Engine/lib/bullet/src/LinearMath/btSerializer.h +++ b/Engine/lib/bullet/src/LinearMath/btSerializer.h @@ -16,126 +16,117 @@ subject to the following restrictions: #ifndef BT_SERIALIZER_H #define BT_SERIALIZER_H -#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE +#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE #include "btHashMap.h" -#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__) #include #endif #include - - - extern char sBulletDNAstr[]; extern int sBulletDNAlen; extern char sBulletDNAstr64[]; extern int sBulletDNAlen64; -SIMD_FORCE_INLINE int btStrLen(const char* str) +SIMD_FORCE_INLINE int btStrLen(const char* str) { - if (!str) - return(0); + if (!str) + return (0); int len = 0; while (*str != 0) { - str++; - len++; - } + str++; + len++; + } - return len; + return len; } - class btChunk { public: - int m_chunkCode; - int m_length; - void *m_oldPtr; - int m_dna_nr; - int m_number; + int m_chunkCode; + int m_length; + void* m_oldPtr; + int m_dna_nr; + int m_number; }; -enum btSerializationFlags +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 +class btSerializer { - public: - virtual ~btSerializer() {} - virtual const unsigned char* getBufferPointer() const = 0; + virtual const unsigned char* getBufferPointer() const = 0; - virtual int getCurrentBufferSize() const = 0; + virtual int getCurrentBufferSize() const = 0; - virtual btChunk* allocate(size_t size, int numElements) = 0; + virtual btChunk* allocate(size_t size, int numElements) = 0; - virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr)= 0; + virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode, void* oldPtr) = 0; - virtual void* findPointer(void* oldPtr) = 0; + virtual void* findPointer(void* oldPtr) = 0; - virtual void* getUniquePointer(void*oldPtr) = 0; + virtual void* getUniquePointer(void* oldPtr) = 0; - virtual void startSerialization() = 0; + virtual void startSerialization() = 0; - virtual void finishSerialization() = 0; + virtual void finishSerialization() = 0; - virtual const char* findNameForPointer(const void* ptr) const = 0; + virtual const char* findNameForPointer(const void* ptr) const = 0; - virtual void registerNameForPointer(const void* ptr, const char* name) = 0; + virtual void registerNameForPointer(const void* ptr, const char* name) = 0; - virtual void serializeName(const char* ptr) = 0; + virtual void serializeName(const char* ptr) = 0; - virtual int getSerializationFlags() const = 0; + virtual int getSerializationFlags() const = 0; - virtual void setSerializationFlags(int flags) = 0; + virtual void setSerializationFlags(int flags) = 0; virtual int getNumChunks() const = 0; virtual const btChunk* getChunk(int chunkIndex) const = 0; - }; - - #define BT_HEADER_LENGTH 12 -#if defined(__sgi) || defined (__sparc) || defined (__sparc__) || defined (__PPC__) || defined (__ppc__) || defined (__BIG_ENDIAN__) -# define BT_MAKE_ID(a,b,c,d) ( (int)(a)<<24 | (int)(b)<<16 | (c)<<8 | (d) ) +#if defined(__sgi) || defined(__sparc) || defined(__sparc__) || defined(__PPC__) || defined(__ppc__) || defined(__BIG_ENDIAN__) +#define BT_MAKE_ID(a, b, c, d) ((int)(a) << 24 | (int)(b) << 16 | (c) << 8 | (d)) #else -# define BT_MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) ) +#define BT_MAKE_ID(a, b, c, d) ((int)(d) << 24 | (int)(c) << 16 | (b) << 8 | (a)) #endif +#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') +#define BT_CONSTRAINT_CODE BT_MAKE_ID('C', 'O', 'N', 'S') +#define BT_BOXSHAPE_CODE BT_MAKE_ID('B', 'O', 'X', 'S') +#define BT_QUANTIZED_BVH_CODE BT_MAKE_ID('Q', 'B', 'V', 'H') +#define BT_TRIANLGE_INFO_MAP BT_MAKE_ID('T', 'M', 'A', 'P') +#define BT_SHAPE_CODE BT_MAKE_ID('S', 'H', 'A', 'P') +#define BT_ARRAY_CODE BT_MAKE_ID('A', 'R', 'A', 'Y') +#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') -#define BT_MULTIBODY_CODE BT_MAKE_ID('M','B','D','Y') -#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') -#define BT_CONSTRAINT_CODE BT_MAKE_ID('C','O','N','S') -#define BT_BOXSHAPE_CODE BT_MAKE_ID('B','O','X','S') -#define BT_QUANTIZED_BVH_CODE BT_MAKE_ID('Q','B','V','H') -#define BT_TRIANLGE_INFO_MAP BT_MAKE_ID('T','M','A','P') -#define BT_SHAPE_CODE BT_MAKE_ID('S','H','A','P') -#define BT_ARRAY_CODE BT_MAKE_ID('A','R','A','Y') -#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_DNA_CODE BT_MAKE_ID('D','N','A','1') - - -struct btPointerUid +struct btPointerUid { - union - { - void* m_ptr; - int m_uniqueIds[2]; + union { + void* m_ptr; + int m_uniqueIds[2]; }; }; @@ -144,8 +135,8 @@ struct btBulletSerializedArrays btBulletSerializedArrays() { } - btAlignedObjectArray m_bvhsDouble; - btAlignedObjectArray m_bvhsFloat; + btAlignedObjectArray m_bvhsDouble; + btAlignedObjectArray m_bvhsFloat; btAlignedObjectArray m_colShapeData; btAlignedObjectArray m_dynamicWorldInfoDataDouble; btAlignedObjectArray m_dynamicWorldInfoDataFloat; @@ -155,51 +146,42 @@ struct btBulletSerializedArrays btAlignedObjectArray m_collisionObjectDataFloat; btAlignedObjectArray m_constraintDataFloat; btAlignedObjectArray m_constraintDataDouble; - btAlignedObjectArray m_constraintData;//for backwards compatibility + btAlignedObjectArray m_constraintData; //for backwards compatibility btAlignedObjectArray m_softBodyFloatData; btAlignedObjectArray m_softBodyDoubleData; - }; - ///The btDefaultSerializer is the main Bullet serialization class. ///The constructor takes an optional argument for backwards compatibility, it is recommended to leave this empty/zero. -class btDefaultSerializer : public btSerializer +class btDefaultSerializer : public btSerializer { +protected: + btAlignedObjectArray mTypes; + btAlignedObjectArray mStructs; + btAlignedObjectArray mTlens; + btHashMap mStructReverse; + btHashMap mTypeLookup; + + btHashMap m_chunkP; + + btHashMap m_nameMap; + + btHashMap m_uniquePointers; + int m_uniqueIdGenerator; + + int m_totalSize; + unsigned char* m_buffer; + bool m_ownsBuffer; + int m_currentSize; + void* m_dna; + int m_dnaLength; + + int m_serializationFlags; + + btAlignedObjectArray m_chunkPtrs; protected: - - btAlignedObjectArray mTypes; - btAlignedObjectArray mStructs; - btAlignedObjectArray mTlens; - btHashMap mStructReverse; - btHashMap mTypeLookup; - - - - btHashMap m_chunkP; - - btHashMap m_nameMap; - - btHashMap m_uniquePointers; - int m_uniqueIdGenerator; - - int m_totalSize; - unsigned char* m_buffer; - bool m_ownsBuffer; - int m_currentSize; - void* m_dna; - int m_dnaLength; - - int m_serializationFlags; - - - btAlignedObjectArray m_chunkPtrs; - -protected: - - - virtual void* findPointer(void* oldPtr) + virtual void* findPointer(void* oldPtr) { void** ptr = m_chunkP.find(oldPtr); if (ptr && *ptr) @@ -207,48 +189,43 @@ protected: return 0; } + virtual void writeDNA() + { + btChunk* dnaChunk = allocate(m_dnaLength, 1); + memcpy(dnaChunk->m_oldPtr, m_dna, m_dnaLength); + finalizeChunk(dnaChunk, "DNA1", BT_DNA_CODE, m_dna); + } + int getReverseType(const char* type) const + { + btHashString 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; - virtual void writeDNA() - { - btChunk* dnaChunk = allocate(m_dnaLength,1); - memcpy(dnaChunk->m_oldPtr,m_dna,m_dnaLength); - finalizeChunk(dnaChunk,"DNA1",BT_DNA_CODE, m_dna); - } + int littleEndian = 1; + littleEndian = ((char*)&littleEndian)[0]; - int getReverseType(const char *type) const - { + m_dna = btAlignedAlloc(dnalen, 16); + memcpy(m_dna, bdnaOrg, dnalen); + m_dnaLength = dnalen; - btHashString key(type); - const int* valuePtr = mTypeLookup.find(key); - if (valuePtr) - return *valuePtr; + int* intPtr = 0; + short* shtPtr = 0; + char* cp = 0; + int dataLen = 0; + intPtr = (int*)m_dna; - 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 = btAlignedAlloc(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) @@ -256,81 +233,81 @@ protected: */ - if (strncmp((const char*)m_dna, "SDNA", 4)==0) - { - // skip ++ NAME - intPtr++; intPtr++; - } - - // Parse names - if (!littleEndian) - *intPtr = btSwapEndian(*intPtr); - - dataLen = *intPtr; - + if (strncmp((const char*)m_dna, "SDNA", 4) == 0) + { + // skip ++ NAME intPtr++; + intPtr++; + } - cp = (char*)intPtr; - int i; - for ( i=0; i amount of types (int) */ - intPtr = (int*)cp; - btAssert(strncmp(cp, "TYPE", 4)==0); intPtr++; + intPtr = (int*)cp; + btAssert(strncmp(cp, "TYPE", 4) == 0); + intPtr++; - if (!littleEndian) - *intPtr = btSwapEndian(*intPtr); + if (!littleEndian) + *intPtr = btSwapEndian(*intPtr); - dataLen = *intPtr; - intPtr++; + dataLen = *intPtr; + intPtr++; + cp = (char*)intPtr; + for (i = 0; i < dataLen; i++) + { + mTypes.push_back(cp); + while (*cp) cp++; + cp++; + } - cp = (char*)intPtr; - for (i=0; i (short) the lengths of types */ - // Parse type lens - intPtr = (int*)cp; - btAssert(strncmp(cp, "TLEN", 4)==0); intPtr++; + // Parse type lens + intPtr = (int*)cp; + btAssert(strncmp(cp, "TLEN", 4) == 0); + intPtr++; - dataLen = (int)mTypes.size(); + dataLen = (int)mTypes.size(); - shtPtr = (short*)intPtr; - for (i=0; i amount of structs (int) @@ -341,384 +318,372 @@ protected: */ - intPtr = (int*)shtPtr; - cp = (char*)intPtr; - btAssert(strncmp(cp, "STRC", 4)==0); intPtr++; + intPtr = (int*)shtPtr; + cp = (char*)intPtr; + btAssert(strncmp(cp, "STRC", 4) == 0); + intPtr++; + + if (!littleEndian) + *intPtr = btSwapEndian(*intPtr); + dataLen = *intPtr; + intPtr++; + + shtPtr = (short*)intPtr; + for (i = 0; i < dataLen; i++) + { + mStructs.push_back(shtPtr); if (!littleEndian) - *intPtr = btSwapEndian(*intPtr); - dataLen = *intPtr ; - intPtr++; - - - shtPtr = (short*)intPtr; - for (i=0; i m_skipPointers; - btHashMap m_skipPointers; - - - btDefaultSerializer(int totalSize=0, unsigned char* buffer=0) - :m_uniqueIdGenerator(0), - m_totalSize(totalSize), - m_currentSize(0), - m_dna(0), - m_dnaLength(0), - m_serializationFlags(0) + btDefaultSerializer(int totalSize = 0, unsigned char* buffer = 0) + : m_uniqueIdGenerator(0), + m_totalSize(totalSize), + m_currentSize(0), + m_dna(0), + m_dnaLength(0), + m_serializationFlags(0) + { + if (buffer == 0) { - if (buffer==0) - { - m_buffer = m_totalSize?(unsigned char*)btAlignedAlloc(totalSize,16):0; - m_ownsBuffer = true; - } else - { - m_buffer = buffer; - m_ownsBuffer = false; - } + m_buffer = m_totalSize ? (unsigned char*)btAlignedAlloc(totalSize, 16) : 0; + m_ownsBuffer = true; + } + else + { + m_buffer = buffer; + m_ownsBuffer = false; + } - const bool VOID_IS_8 = ((sizeof(void*)==8)); + const bool VOID_IS_8 = ((sizeof(void*) == 8)); #ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES - if (VOID_IS_8) - { + if (VOID_IS_8) + { #if _WIN64 - initDNA((const char*)sBulletDNAstr64,sBulletDNAlen64); + initDNA((const char*)sBulletDNAstr64, sBulletDNAlen64); #else - btAssert(0); + btAssert(0); #endif - } else - { + } + else + { #ifndef _WIN64 - initDNA((const char*)sBulletDNAstr,sBulletDNAlen); + initDNA((const char*)sBulletDNAstr, sBulletDNAlen); #else - btAssert(0); + btAssert(0); #endif - } - -#else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES - if (VOID_IS_8) - { - initDNA((const char*)sBulletDNAstr64,sBulletDNAlen64); - } else - { - initDNA((const char*)sBulletDNAstr,sBulletDNAlen); - } -#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES - } - virtual ~btDefaultSerializer() +#else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + if (VOID_IS_8) { - if (m_buffer && m_ownsBuffer) - btAlignedFree(m_buffer); - if (m_dna) - btAlignedFree(m_dna); + initDNA((const char*)sBulletDNAstr64, sBulletDNAlen64); } - - static int getMemoryDnaSizeInBytes() + else { - const bool VOID_IS_8 = ((sizeof(void*) == 8)); - - if (VOID_IS_8) - { - return sBulletDNAlen64; - } - return sBulletDNAlen; + initDNA((const char*)sBulletDNAstr, sBulletDNAlen); } - static const char* getMemoryDna() +#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + } + + virtual ~btDefaultSerializer() + { + if (m_buffer && m_ownsBuffer) + btAlignedFree(m_buffer); + if (m_dna) + btAlignedFree(m_dna); + } + + static int getMemoryDnaSizeInBytes() + { + const bool VOID_IS_8 = ((sizeof(void*) == 8)); + + if (VOID_IS_8) { - const bool VOID_IS_8 = ((sizeof(void*) == 8)); - if (VOID_IS_8) - { - return (const char*)sBulletDNAstr64; - } - return (const char*)sBulletDNAstr; + return sBulletDNAlen64; } - - void insertHeader() + return sBulletDNAlen; + } + static const char* getMemoryDna() + { + const bool VOID_IS_8 = ((sizeof(void*) == 8)); + if (VOID_IS_8) { - writeHeader(m_buffer); - m_currentSize += BT_HEADER_LENGTH; + return (const char*)sBulletDNAstr64; } + return (const char*)sBulletDNAstr; + } - void writeHeader(unsigned char* buffer) const - { + void insertHeader() + { + writeHeader(m_buffer); + m_currentSize += BT_HEADER_LENGTH; + } - -#ifdef BT_USE_DOUBLE_PRECISION - memcpy(buffer, "BULLETd", 7); + void writeHeader(unsigned char* buffer) const + { +#ifdef BT_USE_DOUBLE_PRECISION + memcpy(buffer, "BULLETd", 7); #else - memcpy(buffer, "BULLETf", 7); -#endif //BT_USE_DOUBLE_PRECISION + memcpy(buffer, "BULLETf", 7); +#endif //BT_USE_DOUBLE_PRECISION - int littleEndian= 1; - littleEndian= ((char*)&littleEndian)[0]; + int littleEndian = 1; + littleEndian = ((char*)&littleEndian)[0]; - if (sizeof(void*)==8) + if (sizeof(void*) == 8) + { + buffer[7] = '-'; + } + else + { + buffer[7] = '_'; + } + + if (littleEndian) + { + buffer[8] = 'v'; + } + else + { + buffer[8] = 'V'; + } + + buffer[9] = '2'; + buffer[10] = '8'; + buffer[11] = '8'; + } + + virtual void startSerialization() + { + m_uniqueIdGenerator = 1; + if (m_totalSize) + { + unsigned char* buffer = internalAlloc(BT_HEADER_LENGTH); + writeHeader(buffer); + } + } + + virtual void finishSerialization() + { + writeDNA(); + + //if we didn't pre-allocate a buffer, we need to create a contiguous buffer now + int mysize = 0; + if (!m_totalSize) + { + if (m_buffer) + btAlignedFree(m_buffer); + + m_currentSize += BT_HEADER_LENGTH; + m_buffer = (unsigned char*)btAlignedAlloc(m_currentSize, 16); + + unsigned char* currentPtr = m_buffer; + writeHeader(m_buffer); + currentPtr += BT_HEADER_LENGTH; + mysize += BT_HEADER_LENGTH; + for (int i = 0; i < m_chunkPtrs.size(); i++) { - buffer[7] = '-'; - } else - { - buffer[7] = '_'; + int curLength = sizeof(btChunk) + m_chunkPtrs[i]->m_length; + memcpy(currentPtr, m_chunkPtrs[i], curLength); + btAlignedFree(m_chunkPtrs[i]); + currentPtr += curLength; + mysize += curLength; } - - if (littleEndian) - { - buffer[8]='v'; - } else - { - buffer[8]='V'; - } - - - buffer[9] = '2'; - buffer[10] = '8'; - buffer[11] = '7'; - } - virtual void startSerialization() - { - m_uniqueIdGenerator= 1; - if (m_totalSize) - { - unsigned char* buffer = internalAlloc(BT_HEADER_LENGTH); - writeHeader(buffer); - } + mTypes.clear(); + mStructs.clear(); + mTlens.clear(); + mStructReverse.clear(); + mTypeLookup.clear(); + m_skipPointers.clear(); + m_chunkP.clear(); + m_nameMap.clear(); + m_uniquePointers.clear(); + m_chunkPtrs.clear(); + } - } - - virtual void finishSerialization() - { - writeDNA(); - - //if we didn't pre-allocate a buffer, we need to create a contiguous buffer now - int mysize = 0; - if (!m_totalSize) - { - if (m_buffer) - btAlignedFree(m_buffer); - - m_currentSize += BT_HEADER_LENGTH; - m_buffer = (unsigned char*)btAlignedAlloc(m_currentSize,16); - - unsigned char* currentPtr = m_buffer; - writeHeader(m_buffer); - currentPtr += BT_HEADER_LENGTH; - mysize+=BT_HEADER_LENGTH; - for (int i=0;i< m_chunkPtrs.size();i++) - { - int curLength = sizeof(btChunk)+m_chunkPtrs[i]->m_length; - memcpy(currentPtr,m_chunkPtrs[i], curLength); - btAlignedFree(m_chunkPtrs[i]); - currentPtr+=curLength; - mysize+=curLength; - } - } - - mTypes.clear(); - mStructs.clear(); - mTlens.clear(); - mStructReverse.clear(); - mTypeLookup.clear(); - m_skipPointers.clear(); - m_chunkP.clear(); - m_nameMap.clear(); - m_uniquePointers.clear(); - m_chunkPtrs.clear(); - } - - virtual void* getUniquePointer(void*oldPtr) - { - btAssert(m_uniqueIdGenerator >= 0); - if (!oldPtr) - return 0; - - btPointerUid* uptr = (btPointerUid*)m_uniquePointers.find(oldPtr); - if (uptr) - { - return uptr->m_ptr; - } - - void** ptr2 = m_skipPointers[oldPtr]; - if (ptr2) - { - return 0; - } - - m_uniqueIdGenerator++; - - btPointerUid 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(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr) - { - if (!(m_serializationFlags&BT_SERIALIZE_NO_DUPLICATE_ASSERT)) - { - btAssert(!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); - btAssert(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; + virtual void* getUniquePointer(void* oldPtr) + { + btAssert(m_uniqueIdGenerator >= 0); + if (!oldPtr) return 0; + btPointerUid* uptr = (btPointerUid*)m_uniquePointers.find(oldPtr); + if (uptr) + { + return uptr->m_ptr; } - virtual void registerNameForPointer(const void* ptr, const char* name) + void** ptr2 = m_skipPointers[oldPtr]; + if (ptr2) { - m_nameMap.insert(ptr,name); + return 0; } - virtual void serializeName(const char* name) + m_uniqueIdGenerator++; + + btPointerUid 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(btChunk* chunk, const char* structType, int chunkCode, void* oldPtr) + { + if (!(m_serializationFlags & BT_SERIALIZE_NO_DUPLICATE_ASSERT)) { - if (name) + btAssert(!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); + btAssert(m_currentSize < m_totalSize); + } + else + { + ptr = (unsigned char*)btAlignedAlloc(size, 16); + m_currentSize += int(size); + } + return ptr; + } + + virtual btChunk* allocate(size_t size, int numElements) + { + unsigned char* ptr = internalAlloc(int(size) * numElements + sizeof(btChunk)); + + unsigned char* data = ptr + sizeof(btChunk); + + btChunk* chunk = (btChunk*)ptr; + chunk->m_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 = btStrLen(name); + if (len) { - //don't serialize name twice - if (findPointer((void*)name)) - return; + int newLen = len + 1; + int padding = ((newLen + 3) & ~3) - newLen; + newLen += padding; - int len = btStrLen(name); - if (len) + //serialize name string now + btChunk* chunk = allocate(sizeof(char), newLen); + char* destinationName = (char*)chunk->m_oldPtr; + for (int i = 0; i < len; i++) { - - int newLen = len+1; - int padding = ((newLen+3)&~3)-newLen; - newLen += padding; - - //serialize name string now - btChunk* chunk = allocate(sizeof(char),newLen); - char* destinationName = (char*)chunk->m_oldPtr; - for (int i=0;i m_uid2ChunkPtr; - btHashMap m_orgPtr2UniqueDataPtr; - btHashMap m_names2Ptr; - + btHashMap m_uid2ChunkPtr; + btHashMap m_orgPtr2UniqueDataPtr; + btHashMap m_names2Ptr; - btBulletSerializedArrays m_arrays; + btBulletSerializedArrays m_arrays; - btInMemorySerializer(int totalSize=0, unsigned char* buffer=0) - :btDefaultSerializer(totalSize,buffer) - { - - } + btInMemorySerializer(int totalSize = 0, unsigned char* buffer = 0) + : btDefaultSerializer(totalSize, buffer) + { + } - virtual void startSerialization() - { - m_uid2ChunkPtr.clear(); - //todo: m_arrays.clear(); - btDefaultSerializer::startSerialization(); - } + virtual void startSerialization() + { + m_uid2ChunkPtr.clear(); + //todo: m_arrays.clear(); + btDefaultSerializer::startSerialization(); + } - + btChunk* findChunkFromUniquePointer(void* uniquePointer) + { + btChunk** chkPtr = m_uid2ChunkPtr[uniquePointer]; + if (chkPtr) + { + return *chkPtr; + } + return 0; + } - btChunk* findChunkFromUniquePointer(void* uniquePointer) - { - btChunk** chkPtr = m_uid2ChunkPtr[uniquePointer]; - if (chkPtr) - { - return *chkPtr; - } - return 0; - } + virtual void registerNameForPointer(const void* ptr, const char* name) + { + btDefaultSerializer::registerNameForPointer(ptr, name); + m_names2Ptr.insert(name, ptr); + } - virtual void registerNameForPointer(const void* ptr, const char* name) - { - btDefaultSerializer::registerNameForPointer(ptr,name); - m_names2Ptr.insert(name,ptr); - } + virtual void finishSerialization() + { + } - virtual void finishSerialization() - { - } + virtual void* getUniquePointer(void* oldPtr) + { + if (oldPtr == 0) + return 0; - virtual void* getUniquePointer(void*oldPtr) - { - if (oldPtr==0) - return 0; - - // void* uniquePtr = getUniquePointer(oldPtr); - btChunk* chunk = findChunkFromUniquePointer(oldPtr); - if (chunk) - { - return chunk->m_oldPtr; - } else - { - const char* n = (const char*) oldPtr; - const void** ptr = m_names2Ptr[n]; - if (ptr) - { - return oldPtr; - } else - { - void** ptr2 = m_skipPointers[oldPtr]; - if (ptr2) - { - return 0; - } else - { - //If this assert hit, serialization happened in the wrong order - // 'getUniquePointer' - btAssert(0); - } - - } - return 0; - } - return oldPtr; - } - - virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr) - { - if (!(m_serializationFlags&BT_SERIALIZE_NO_DUPLICATE_ASSERT)) - { - btAssert(!findPointer(oldPtr)); - } - - chunk->m_dna_nr = getReverseType(structType); - chunk->m_chunkCode = chunkCode; - //void* uniquePtr = getUniquePointer(oldPtr); - m_chunkP.insert(oldPtr,oldPtr);//chunk->m_oldPtr); - // chunk->m_oldPtr = uniquePtr;//oldPtr; - - void* uid = findPointer(oldPtr); - m_uid2ChunkPtr.insert(uid,chunk); - - switch (chunk->m_chunkCode) + // void* uniquePtr = getUniquePointer(oldPtr); + btChunk* chunk = findChunkFromUniquePointer(oldPtr); + if (chunk) + { + return chunk->m_oldPtr; + } + else + { + const char* n = (const char*)oldPtr; + const void** ptr = m_names2Ptr[n]; + if (ptr) { + return oldPtr; + } + else + { + void** ptr2 = m_skipPointers[oldPtr]; + if (ptr2) + { + return 0; + } + else + { + //If this assert hit, serialization happened in the wrong order + // 'getUniquePointer' + btAssert(0); + } + } + return 0; + } + return oldPtr; + } + + virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode, void* oldPtr) + { + if (!(m_serializationFlags & BT_SERIALIZE_NO_DUPLICATE_ASSERT)) + { + btAssert(!findPointer(oldPtr)); + } + + chunk->m_dna_nr = getReverseType(structType); + chunk->m_chunkCode = chunkCode; + //void* uniquePtr = getUniquePointer(oldPtr); + m_chunkP.insert(oldPtr, oldPtr); //chunk->m_oldPtr); + // chunk->m_oldPtr = uniquePtr;//oldPtr; + + void* uid = findPointer(oldPtr); + m_uid2ChunkPtr.insert(uid, chunk); + + switch (chunk->m_chunkCode) + { case BT_SOFTBODY_CODE: { - #ifdef BT_USE_DOUBLE_PRECISION - m_arrays.m_softBodyDoubleData.push_back((btSoftBodyDoubleData*) chunk->m_oldPtr); - #else - m_arrays.m_softBodyFloatData.push_back((btSoftBodyFloatData*) chunk->m_oldPtr); - #endif - break; - } +#ifdef BT_USE_DOUBLE_PRECISION + m_arrays.m_softBodyDoubleData.push_back((btSoftBodyDoubleData*)chunk->m_oldPtr); +#else + m_arrays.m_softBodyFloatData.push_back((btSoftBodyFloatData*)chunk->m_oldPtr); +#endif + break; + } case BT_COLLISIONOBJECT_CODE: - { - #ifdef BT_USE_DOUBLE_PRECISION - m_arrays.m_collisionObjectDataDouble.push_back((btCollisionObjectDoubleData*)chunk->m_oldPtr); - #else//BT_USE_DOUBLE_PRECISION - m_arrays.m_collisionObjectDataFloat.push_back((btCollisionObjectFloatData*)chunk->m_oldPtr); - #endif //BT_USE_DOUBLE_PRECISION - break; - } + { +#ifdef BT_USE_DOUBLE_PRECISION + m_arrays.m_collisionObjectDataDouble.push_back((btCollisionObjectDoubleData*)chunk->m_oldPtr); +#else //BT_USE_DOUBLE_PRECISION + m_arrays.m_collisionObjectDataFloat.push_back((btCollisionObjectFloatData*)chunk->m_oldPtr); +#endif //BT_USE_DOUBLE_PRECISION + break; + } case BT_RIGIDBODY_CODE: - { - #ifdef BT_USE_DOUBLE_PRECISION - m_arrays.m_rigidBodyDataDouble.push_back((btRigidBodyDoubleData*)chunk->m_oldPtr); - #else - m_arrays.m_rigidBodyDataFloat.push_back((btRigidBodyFloatData*)chunk->m_oldPtr); - #endif//BT_USE_DOUBLE_PRECISION - break; - }; + { +#ifdef BT_USE_DOUBLE_PRECISION + m_arrays.m_rigidBodyDataDouble.push_back((btRigidBodyDoubleData*)chunk->m_oldPtr); +#else + m_arrays.m_rigidBodyDataFloat.push_back((btRigidBodyFloatData*)chunk->m_oldPtr); +#endif //BT_USE_DOUBLE_PRECISION + break; + }; case BT_CONSTRAINT_CODE: - { - #ifdef BT_USE_DOUBLE_PRECISION - m_arrays.m_constraintDataDouble.push_back((btTypedConstraintDoubleData*)chunk->m_oldPtr); - #else - m_arrays.m_constraintDataFloat.push_back((btTypedConstraintFloatData*)chunk->m_oldPtr); - #endif - break; - } + { +#ifdef BT_USE_DOUBLE_PRECISION + m_arrays.m_constraintDataDouble.push_back((btTypedConstraintDoubleData*)chunk->m_oldPtr); +#else + m_arrays.m_constraintDataFloat.push_back((btTypedConstraintFloatData*)chunk->m_oldPtr); +#endif + break; + } case BT_QUANTIZED_BVH_CODE: - { - #ifdef BT_USE_DOUBLE_PRECISION - m_arrays.m_bvhsDouble.push_back((btQuantizedBvhDoubleData*) chunk->m_oldPtr); - #else - m_arrays.m_bvhsFloat.push_back((btQuantizedBvhFloatData*) chunk->m_oldPtr); - #endif - break; - } + { +#ifdef BT_USE_DOUBLE_PRECISION + m_arrays.m_bvhsDouble.push_back((btQuantizedBvhDoubleData*)chunk->m_oldPtr); +#else + m_arrays.m_bvhsFloat.push_back((btQuantizedBvhFloatData*)chunk->m_oldPtr); +#endif + break; + } case BT_SHAPE_CODE: - { - btCollisionShapeData* shapeData = (btCollisionShapeData*) chunk->m_oldPtr; - m_arrays.m_colShapeData.push_back(shapeData); - break; - } + { + btCollisionShapeData* shapeData = (btCollisionShapeData*)chunk->m_oldPtr; + m_arrays.m_colShapeData.push_back(shapeData); + break; + } case BT_TRIANLGE_INFO_MAP: case BT_ARRAY_CODE: case BT_SBMATERIAL_CODE: case BT_SBNODE_CODE: case BT_DYNAMICSWORLD_CODE: case BT_DNA_CODE: - { - break; - } + { + break; + } default: - { - } - }; - } + { + } + }; + } - int getNumChunks() const - { - return m_uid2ChunkPtr.size(); - } - - const btChunk* getChunk(int chunkIndex) const - { - return *m_uid2ChunkPtr.getAtIndex(chunkIndex); - } + int getNumChunks() const + { + return m_uid2ChunkPtr.size(); + } + const btChunk* getChunk(int chunkIndex) const + { + return *m_uid2ChunkPtr.getAtIndex(chunkIndex); + } }; -#endif //ENABLE_INMEMORY_SERIALIZER - -#endif //BT_SERIALIZER_H +#endif //ENABLE_INMEMORY_SERIALIZER +#endif //BT_SERIALIZER_H diff --git a/Engine/lib/bullet/src/LinearMath/btSerializer64.cpp b/Engine/lib/bullet/src/LinearMath/btSerializer64.cpp index 05f59202d..cf281cdb3 100644 --- a/Engine/lib/bullet/src/LinearMath/btSerializer64.cpp +++ b/Engine/lib/bullet/src/LinearMath/btSerializer64.cpp @@ -1,5 +1,6 @@ +// clang-format off char sBulletDNAstr64[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-124),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), +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), @@ -72,528 +73,619 @@ char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),cha 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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(-16),char(1),char(24),char(1),char(-104),char(0),char(88),char(0),char(-72),char(0), -char(104),char(0),char(0),char(2),char(-64),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(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(-24),char(1),char(0),char(3),char(-104),char(1),char(-48),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(27),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(8),char(0),char(117),char(0),char(8),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(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(0),char(0),char(37),char(0),char(50),char(0),char(27),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(7),char(0),char(117),char(0),char(7),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(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0), -char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(8),char(0),char(126),char(0),char(8),char(0),char(127),char(0),char(8),char(0),char(111),char(0), -char(8),char(0),char(-128),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(-127),char(0),char(8),char(0),char(-126),char(0),char(8),char(0),char(-125),char(0), -char(8),char(0),char(-124),char(0),char(8),char(0),char(-123),char(0),char(8),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(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0), -char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(52),char(0),char(22),char(0), -char(7),char(0),char(126),char(0),char(7),char(0),char(127),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-128),char(0),char(7),char(0),char(115),char(0), -char(7),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(7),char(0),char(-125),char(0),char(7),char(0),char(-124),char(0),char(7),char(0),char(-123),char(0), -char(7),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(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0),char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0), -char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(53),char(0),char(2),char(0),char(51),char(0),char(-111),char(0),char(14),char(0),char(-110),char(0), -char(54),char(0),char(2),char(0),char(52),char(0),char(-111),char(0),char(13),char(0),char(-110),char(0),char(55),char(0),char(21),char(0),char(50),char(0),char(-109),char(0), -char(17),char(0),char(-108),char(0),char(13),char(0),char(-107),char(0),char(13),char(0),char(-106),char(0),char(13),char(0),char(-105),char(0),char(13),char(0),char(-104),char(0), -char(13),char(0),char(-110),char(0),char(13),char(0),char(-103),char(0),char(13),char(0),char(-102),char(0),char(13),char(0),char(-101),char(0),char(13),char(0),char(-100),char(0), -char(7),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),char(7),char(0),char(-97),char(0),char(7),char(0),char(-96),char(0),char(7),char(0),char(-95),char(0), -char(7),char(0),char(-94),char(0),char(7),char(0),char(-93),char(0),char(7),char(0),char(-92),char(0),char(7),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0), -char(56),char(0),char(22),char(0),char(49),char(0),char(-109),char(0),char(18),char(0),char(-108),char(0),char(14),char(0),char(-107),char(0),char(14),char(0),char(-106),char(0), -char(14),char(0),char(-105),char(0),char(14),char(0),char(-104),char(0),char(14),char(0),char(-110),char(0),char(14),char(0),char(-103),char(0),char(14),char(0),char(-102),char(0), -char(14),char(0),char(-101),char(0),char(14),char(0),char(-100),char(0),char(8),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(8),char(0),char(-97),char(0), -char(8),char(0),char(-96),char(0),char(8),char(0),char(-95),char(0),char(8),char(0),char(-94),char(0),char(8),char(0),char(-93),char(0),char(8),char(0),char(-92),char(0), -char(8),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(0),char(0),char(37),char(0),char(57),char(0),char(2),char(0),char(4),char(0),char(-89),char(0), -char(4),char(0),char(-88),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-87),char(0),char(55),char(0),char(-86),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0), -char(7),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0), -char(59),char(0),char(13),char(0),char(60),char(0),char(-87),char(0),char(60),char(0),char(-86),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0), -char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0), -char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(61),char(0),char(14),char(0), -char(56),char(0),char(-87),char(0),char(56),char(0),char(-86),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0), -char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),char(8),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0), -char(4),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(0),char(0),char(-75),char(0),char(62),char(0),char(3),char(0), -char(59),char(0),char(-74),char(0),char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(63),char(0),char(3),char(0),char(61),char(0),char(-74),char(0), -char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(64),char(0),char(3),char(0),char(59),char(0),char(-74),char(0),char(14),char(0),char(-73),char(0), -char(14),char(0),char(-72),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), -char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(7),char(0),char(-66),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(66),char(0),char(13),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0), -char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(7),char(0),char(-66),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(67),char(0),char(14),char(0), -char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0), -char(4),char(0),char(-67),char(0),char(8),char(0),char(-66),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(0),char(0),char(-59),char(0),char(68),char(0),char(10),char(0), -char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),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(-62),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(127),char(0), -char(69),char(0),char(11),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),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(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0), -char(7),char(0),char(127),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0), -char(19),char(0),char(-70),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(13),char(0),char(-53),char(0),char(13),char(0),char(-52),char(0), -char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(71),char(0),char(9),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0), -char(20),char(0),char(-70),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),char(14),char(0),char(-53),char(0),char(14),char(0),char(-52),char(0), -char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(72),char(0),char(5),char(0),char(70),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0), -char(7),char(0),char(-47),char(0),char(7),char(0),char(-46),char(0),char(7),char(0),char(-45),char(0),char(73),char(0),char(5),char(0),char(71),char(0),char(-49),char(0), -char(4),char(0),char(-48),char(0),char(8),char(0),char(-47),char(0),char(8),char(0),char(-46),char(0),char(8),char(0),char(-45),char(0),char(74),char(0),char(41),char(0), -char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),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(13),char(0),char(-37),char(0),char(13),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0), -char(13),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0),char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0), -char(0),char(0),char(-29),char(0),char(0),char(0),char(-59),char(0),char(13),char(0),char(-53),char(0),char(13),char(0),char(-52),char(0),char(13),char(0),char(-28),char(0), -char(13),char(0),char(-27),char(0),char(13),char(0),char(-26),char(0),char(13),char(0),char(-25),char(0),char(13),char(0),char(-24),char(0),char(13),char(0),char(-23),char(0), -char(13),char(0),char(-22),char(0),char(13),char(0),char(-21),char(0),char(13),char(0),char(-20),char(0),char(13),char(0),char(-19),char(0),char(13),char(0),char(-18),char(0), -char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0),char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0), -char(4),char(0),char(-12),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), -char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),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(14),char(0),char(-37),char(0), -char(14),char(0),char(-36),char(0),char(14),char(0),char(-35),char(0),char(14),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0), -char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0),char(0),char(0),char(-29),char(0),char(0),char(0),char(-59),char(0),char(14),char(0),char(-53),char(0), -char(14),char(0),char(-52),char(0),char(14),char(0),char(-28),char(0),char(14),char(0),char(-27),char(0),char(14),char(0),char(-26),char(0),char(14),char(0),char(-25),char(0), -char(14),char(0),char(-24),char(0),char(14),char(0),char(-23),char(0),char(14),char(0),char(-22),char(0),char(14),char(0),char(-21),char(0),char(14),char(0),char(-20),char(0), -char(14),char(0),char(-19),char(0),char(14),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0), -char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(76),char(0),char(9),char(0),char(59),char(0),char(-74),char(0), -char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),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(4),char(0),char(-50),char(0),char(77),char(0),char(9),char(0),char(61),char(0),char(-74),char(0), -char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),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(4),char(0),char(-50),char(0),char(78),char(0),char(5),char(0),char(58),char(0),char(-74),char(0), -char(13),char(0),char(-11),char(0),char(13),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0), -char(61),char(0),char(-74),char(0),char(14),char(0),char(-11),char(0),char(14),char(0),char(-10),char(0),char(8),char(0),char(-9),char(0),char(80),char(0),char(4),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(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0), -char(80),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(7),char(0),char(-99),char(0),char(7),char(0),char(1),char(1),char(4),char(0),char(2),char(1),char(4),char(0),char(53),char(0), -char(82),char(0),char(4),char(0),char(80),char(0),char(-5),char(0),char(4),char(0),char(3),char(1),char(7),char(0),char(4),char(1),char(4),char(0),char(5),char(1), -char(83),char(0),char(4),char(0),char(13),char(0),char(0),char(1),char(80),char(0),char(-5),char(0),char(4),char(0),char(6),char(1),char(7),char(0),char(7),char(1), -char(84),char(0),char(7),char(0),char(13),char(0),char(8),char(1),char(80),char(0),char(-5),char(0),char(4),char(0),char(9),char(1),char(7),char(0),char(10),char(1), -char(7),char(0),char(11),char(1),char(7),char(0),char(12),char(1),char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(13),char(1), -char(13),char(0),char(11),char(1),char(13),char(0),char(14),char(1),char(60),char(0),char(15),char(1),char(4),char(0),char(16),char(1),char(7),char(0),char(12),char(1), -char(86),char(0),char(26),char(0),char(4),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(127),char(0),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(7),char(0),char(35),char(1),char(7),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(39),char(1), -char(4),char(0),char(40),char(1),char(4),char(0),char(120),char(0),char(87),char(0),char(12),char(0),char(17),char(0),char(41),char(1),char(17),char(0),char(42),char(1), -char(17),char(0),char(43),char(1),char(13),char(0),char(44),char(1),char(13),char(0),char(45),char(1),char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1), -char(4),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1),char(7),char(0),char(10),char(1),char(4),char(0),char(53),char(0), -char(88),char(0),char(27),char(0),char(19),char(0),char(51),char(1),char(17),char(0),char(52),char(1),char(17),char(0),char(53),char(1),char(13),char(0),char(44),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(13),char(0),char(57),char(1),char(13),char(0),char(58),char(1), -char(4),char(0),char(59),char(1),char(7),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1), -char(7),char(0),char(64),char(1),char(7),char(0),char(65),char(1),char(4),char(0),char(66),char(1),char(4),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(4),char(0),char(74),char(1),char(4),char(0),char(75),char(1),char(4),char(0),char(76),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(77),char(1), -char(9),char(0),char(78),char(1),char(13),char(0),char(79),char(1),char(7),char(0),char(80),char(1),char(7),char(0),char(-125),char(0),char(7),char(0),char(81),char(1), -char(4),char(0),char(82),char(1),char(13),char(0),char(83),char(1),char(4),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(53),char(0),char(90),char(0),char(19),char(0),char(50),char(0),char(-109),char(0),char(87),char(0),char(87),char(1),char(80),char(0),char(88),char(1), -char(81),char(0),char(89),char(1),char(82),char(0),char(90),char(1),char(83),char(0),char(91),char(1),char(84),char(0),char(92),char(1),char(85),char(0),char(93),char(1), -char(88),char(0),char(94),char(1),char(89),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(62),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(4),char(0),char(101),char(1),char(4),char(0),char(102),char(1), -char(86),char(0),char(103),char(1),char(91),char(0),char(24),char(0),char(16),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1), -char(14),char(0),char(107),char(1),char(14),char(0),char(108),char(1),char(14),char(0),char(109),char(1),char(8),char(0),char(110),char(1),char(4),char(0),char(111),char(1), -char(4),char(0),char(86),char(1),char(4),char(0),char(112),char(1),char(4),char(0),char(113),char(1),char(8),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(8),char(0),char(118),char(1),char(8),char(0),char(119),char(1),char(8),char(0),char(120),char(1), -char(8),char(0),char(121),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(123),char(1),char(0),char(0),char(124),char(1),char(49),char(0),char(125),char(1), -char(0),char(0),char(126),char(1),char(92),char(0),char(24),char(0),char(15),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1), -char(13),char(0),char(107),char(1),char(13),char(0),char(108),char(1),char(13),char(0),char(109),char(1),char(4),char(0),char(112),char(1),char(7),char(0),char(110),char(1), -char(4),char(0),char(111),char(1),char(4),char(0),char(86),char(1),char(7),char(0),char(114),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1), -char(4),char(0),char(113),char(1),char(7),char(0),char(117),char(1),char(7),char(0),char(118),char(1),char(7),char(0),char(119),char(1),char(7),char(0),char(120),char(1), -char(7),char(0),char(121),char(1),char(7),char(0),char(122),char(1),char(0),char(0),char(123),char(1),char(0),char(0),char(124),char(1),char(50),char(0),char(125),char(1), -char(0),char(0),char(126),char(1),char(93),char(0),char(9),char(0),char(20),char(0),char(127),char(1),char(14),char(0),char(-128),char(1),char(8),char(0),char(-127),char(1), -char(0),char(0),char(-126),char(1),char(91),char(0),char(90),char(1),char(49),char(0),char(-125),char(1),char(0),char(0),char(126),char(1),char(4),char(0),char(97),char(1), -char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),char(0),char(0),char(-126),char(1),char(92),char(0),char(90),char(1),char(50),char(0),char(-125),char(1), -char(19),char(0),char(127),char(1),char(13),char(0),char(-128),char(1),char(7),char(0),char(-127),char(1),char(4),char(0),char(97),char(1),}; +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); +// clang-format on diff --git a/Engine/lib/bullet/src/LinearMath/btSpatialAlgebra.h b/Engine/lib/bullet/src/LinearMath/btSpatialAlgebra.h index 8e59658bc..6ad67a108 100644 --- a/Engine/lib/bullet/src/LinearMath/btSpatialAlgebra.h +++ b/Engine/lib/bullet/src/LinearMath/btSpatialAlgebra.h @@ -12,18 +12,17 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -///These spatial algebra classes are used for btMultiBody, +///These spatial algebra classes are used for btMultiBody, ///see BulletDynamics/Featherstone #ifndef BT_SPATIAL_ALGEBRA_H #define BT_SPATIAL_ALGEBRA_H - #include "btMatrix3x3.h" struct btSpatialForceVector -{ - btVector3 m_topVec, m_bottomVec; +{ + btVector3 m_topVec, m_bottomVec; // btSpatialForceVector() { setZero(); } btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {} @@ -32,21 +31,34 @@ struct btSpatialForceVector setValue(ax, ay, az, lx, ly, lz); } // - void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; } + void setVector(const btVector3 &angular, const btVector3 &linear) + { + m_topVec = linear; + m_bottomVec = angular; + } void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) { - m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz); + m_bottomVec.setValue(ax, ay, az); + m_topVec.setValue(lx, ly, lz); } // - void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } + void addVector(const btVector3 &angular, const btVector3 &linear) + { + m_topVec += linear; + m_bottomVec += angular; + } void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) { - m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az; - m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz; + m_bottomVec[0] += ax; + m_bottomVec[1] += ay; + m_bottomVec[2] += az; + m_topVec[0] += lx; + m_topVec[1] += ly; + m_topVec[2] += lz; } // - const btVector3 & getLinear() const { return m_topVec; } - const btVector3 & getAngular() const { return m_bottomVec; } + const btVector3 &getLinear() const { return m_topVec; } + const btVector3 &getAngular() const { return m_bottomVec; } // void setLinear(const btVector3 &linear) { m_topVec = linear; } void setAngular(const btVector3 &angular) { m_bottomVec = angular; } @@ -54,14 +66,28 @@ struct btSpatialForceVector void addAngular(const btVector3 &angular) { m_bottomVec += angular; } void addLinear(const btVector3 &linear) { m_topVec += linear; } // - void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } + void setZero() + { + m_topVec.setZero(); + m_bottomVec.setZero(); + } // - btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } - btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } - btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); } - btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); } - btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); } - btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); } + btSpatialForceVector &operator+=(const btSpatialForceVector &vec) + { + m_topVec += vec.m_topVec; + m_bottomVec += vec.m_bottomVec; + return *this; + } + btSpatialForceVector &operator-=(const btSpatialForceVector &vec) + { + m_topVec -= vec.m_topVec; + m_bottomVec -= vec.m_bottomVec; + return *this; + } + btSpatialForceVector operator-(const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); } + btSpatialForceVector operator+(const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); } + btSpatialForceVector operator-() const { return btSpatialForceVector(-m_bottomVec, -m_topVec); } + btSpatialForceVector operator*(const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); } //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; } }; @@ -70,23 +96,36 @@ struct btSpatialMotionVector btVector3 m_topVec, m_bottomVec; // btSpatialMotionVector() { setZero(); } - btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {} + btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {} // - void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; } + void setVector(const btVector3 &angular, const btVector3 &linear) + { + m_topVec = angular; + m_bottomVec = linear; + } void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) { - m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz); + m_topVec.setValue(ax, ay, az); + m_bottomVec.setValue(lx, ly, lz); } // - void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } + void addVector(const btVector3 &angular, const btVector3 &linear) + { + m_topVec += linear; + m_bottomVec += angular; + } void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) { - m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az; - m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz; + m_topVec[0] += ax; + m_topVec[1] += ay; + m_topVec[2] += az; + m_bottomVec[0] += lx; + m_bottomVec[1] += ly; + m_bottomVec[2] += lz; } - // - const btVector3 & getAngular() const { return m_topVec; } - const btVector3 & getLinear() const { return m_bottomVec; } + // + const btVector3 &getAngular() const { return m_topVec; } + const btVector3 &getLinear() const { return m_bottomVec; } // void setAngular(const btVector3 &angular) { m_topVec = angular; } void setLinear(const btVector3 &linear) { m_bottomVec = linear; } @@ -94,20 +133,24 @@ struct btSpatialMotionVector void addAngular(const btVector3 &angular) { m_topVec += angular; } void addLinear(const btVector3 &linear) { m_bottomVec += linear; } // - void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } + void setZero() + { + m_topVec.setZero(); + m_bottomVec.setZero(); + } // btScalar dot(const btSpatialForceVector &b) const { return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec); } // - template + template void cross(const SpatialVectorType &b, SpatialVectorType &out) const { out.m_topVec = m_topVec.cross(b.m_topVec); out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); } - template + template SpatialVectorType cross(const SpatialVectorType &b) const { SpatialVectorType out; @@ -116,21 +159,36 @@ struct btSpatialMotionVector return out; } // - btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } - btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } - btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; } - btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); } - btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); } - btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } - btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } + btSpatialMotionVector &operator+=(const btSpatialMotionVector &vec) + { + m_topVec += vec.m_topVec; + m_bottomVec += vec.m_bottomVec; + return *this; + } + btSpatialMotionVector &operator-=(const btSpatialMotionVector &vec) + { + m_topVec -= vec.m_topVec; + m_bottomVec -= vec.m_bottomVec; + return *this; + } + btSpatialMotionVector &operator*=(const btScalar &s) + { + m_topVec *= s; + m_bottomVec *= s; + return *this; + } + btSpatialMotionVector operator-(const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); } + btSpatialMotionVector operator+(const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); } + btSpatialMotionVector operator-() const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } + btSpatialMotionVector operator*(const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } }; struct btSymmetricSpatialDyad { btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat; - // + // btSymmetricSpatialDyad() { setIdentity(); } - btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); } + btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); } // void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { @@ -146,17 +204,22 @@ struct btSymmetricSpatialDyad m_bottomLeftMat += bottomLeftMat; } // - void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); } + void setIdentity() + { + m_topLeftMat.setIdentity(); + m_topRightMat.setIdentity(); + m_bottomLeftMat.setIdentity(); + } // - btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat) + btSymmetricSpatialDyad &operator-=(const btSymmetricSpatialDyad &mat) { m_topLeftMat -= mat.m_topLeftMat; m_topRightMat -= mat.m_topRightMat; m_bottomLeftMat -= mat.m_bottomLeftMat; - return *this; + return *this; } // - btSpatialForceVector operator * (const btSpatialMotionVector &vec) + btSpatialForceVector operator*(const btSpatialMotionVector &vec) { return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec); } @@ -164,7 +227,7 @@ struct btSymmetricSpatialDyad struct btSpatialTransformationMatrix { - btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; + btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; btVector3 m_trnVec; // enum eOutputOperation @@ -174,128 +237,124 @@ struct btSpatialTransformationMatrix Subtract = 2 }; // - template - void transform( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) + template + void transform(const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) { - if(outOp == None) + if (outOp == None) { outVec.m_topVec = m_rotMat * inVec.m_topVec; outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; } - else if(outOp == Add) + else if (outOp == Add) { outVec.m_topVec += m_rotMat * inVec.m_topVec; outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; } - else if(outOp == Subtract) + else if (outOp == Subtract) { outVec.m_topVec -= m_rotMat * inVec.m_topVec; outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; } - } - template - void transformRotationOnly( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) + template + void transformRotationOnly(const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) { - if(outOp == None) + if (outOp == None) { outVec.m_topVec = m_rotMat * inVec.m_topVec; outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec; } - else if(outOp == Add) + else if (outOp == Add) { outVec.m_topVec += m_rotMat * inVec.m_topVec; outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec; } - else if(outOp == Subtract) + else if (outOp == Subtract) { outVec.m_topVec -= m_rotMat * inVec.m_topVec; outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec; } - } - template - void transformInverse( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) + template + void transformInverse(const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) { - if(outOp == None) + if (outOp == None) { outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); } - else if(outOp == Add) + else if (outOp == Add) { outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); } - else if(outOp == Subtract) + else if (outOp == Subtract) { outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); - } + } } - template - void transformInverseRotationOnly( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) + template + void transformInverseRotationOnly(const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) { - if(outOp == None) + if (outOp == None) { outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec; } - else if(outOp == Add) + else if (outOp == Add) { outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec; } - else if(outOp == Subtract) + else if (outOp == Subtract) { outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec; } - } - void transformInverse( const btSymmetricSpatialDyad &inMat, - btSymmetricSpatialDyad &outMat, - eOutputOperation outOp = None) + void transformInverse(const btSymmetricSpatialDyad &inMat, + btSymmetricSpatialDyad &outMat, + eOutputOperation outOp = None) { - const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1], - m_trnVec[2], 0, -m_trnVec[0], - -m_trnVec[1], m_trnVec[0], 0); + const btMatrix3x3 r_cross(0, -m_trnVec[2], m_trnVec[1], + m_trnVec[2], 0, -m_trnVec[0], + -m_trnVec[1], m_trnVec[0], 0); - - if(outOp == None) + if (outOp == None) { - outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topLeftMat = m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat; outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; } - else if(outOp == Add) + else if (outOp == Add) { - outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topLeftMat += m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat; outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; } - else if(outOp == Subtract) + else if (outOp == Subtract) { - outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topLeftMat -= m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat; outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; } } - template - SpatialVectorType operator * (const SpatialVectorType &vec) + template + SpatialVectorType operator*(const SpatialVectorType &vec) { SpatialVectorType out; transform(vec, out); @@ -303,7 +362,7 @@ struct btSpatialTransformationMatrix } }; -template +template void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out) { //output op maybe? @@ -314,7 +373,7 @@ void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVecto //maybe simple a*spatTranspose(a) would be nicer? } -template +template btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b) { btSymmetricSpatialDyad out; @@ -327,5 +386,4 @@ btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, //maybe simple a*spatTranspose(a) would be nicer? } -#endif //BT_SPATIAL_ALGEBRA_H - +#endif //BT_SPATIAL_ALGEBRA_H diff --git a/Engine/lib/bullet/src/LinearMath/btStackAlloc.h b/Engine/lib/bullet/src/LinearMath/btStackAlloc.h index 397b08487..3fc208497 100644 --- a/Engine/lib/bullet/src/LinearMath/btStackAlloc.h +++ b/Engine/lib/bullet/src/LinearMath/btStackAlloc.h @@ -20,97 +20,99 @@ Nov.2006 #ifndef BT_STACK_ALLOC #define BT_STACK_ALLOC -#include "btScalar.h" //for btAssert +#include "btScalar.h" //for btAssert #include "btAlignedAllocator.h" ///The btBlock class is an internal structure for the btStackAlloc memory allocator. struct btBlock { - btBlock* previous; - unsigned char* address; + btBlock* previous; + unsigned char* address; }; ///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out) class btStackAlloc { public: + btStackAlloc(unsigned int size) + { + ctor(); + create(size); + } + ~btStackAlloc() { destroy(); } - btStackAlloc(unsigned int size) { ctor();create(size); } - ~btStackAlloc() { destroy(); } - - inline void create(unsigned int size) + inline void create(unsigned int size) { destroy(); - data = (unsigned char*) btAlignedAlloc(size,16); - totalsize = size; + data = (unsigned char*)btAlignedAlloc(size, 16); + totalsize = size; } - inline void destroy() + inline void destroy() { - btAssert(usedsize==0); + btAssert(usedsize == 0); //Raise(L"StackAlloc is still in use"); - if(usedsize==0) + if (usedsize == 0) { - if(!ischild && data) + if (!ischild && data) btAlignedFree(data); - data = 0; - usedsize = 0; + data = 0; + usedsize = 0; } - } - int getAvailableMemory() const + int getAvailableMemory() const { return static_cast(totalsize - usedsize); } - unsigned char* allocate(unsigned int size) + unsigned char* allocate(unsigned int size) { - const unsigned int nus(usedsize+size); - if(nusprevious = current; - pb->address = data+usedsize; - current = pb; - return(pb); + btBlock* pb = (btBlock*)allocate(sizeof(btBlock)); + pb->previous = current; + pb->address = data + usedsize; + current = pb; + return (pb); } - SIMD_FORCE_INLINE void endBlock(btBlock* block) + SIMD_FORCE_INLINE void endBlock(btBlock* block) { - btAssert(block==current); + btAssert(block == current); //Raise(L"Unmatched blocks"); - if(block==current) + if (block == current) { - current = block->previous; - usedsize = (unsigned int)((block->address-data)-sizeof(btBlock)); + current = block->previous; + usedsize = (unsigned int)((block->address - data) - sizeof(btBlock)); } } private: - void ctor() + void ctor() { - data = 0; - totalsize = 0; - usedsize = 0; - current = 0; - ischild = false; + data = 0; + totalsize = 0; + usedsize = 0; + current = 0; + ischild = false; } - unsigned char* data; - unsigned int totalsize; - unsigned int usedsize; - btBlock* current; - bool ischild; + unsigned char* data; + unsigned int totalsize; + unsigned int usedsize; + btBlock* current; + bool ischild; }; -#endif //BT_STACK_ALLOC +#endif //BT_STACK_ALLOC diff --git a/Engine/lib/bullet/src/LinearMath/btThreads.cpp b/Engine/lib/bullet/src/LinearMath/btThreads.cpp index 59a7ea36e..69a86799f 100644 --- a/Engine/lib/bullet/src/LinearMath/btThreads.cpp +++ b/Engine/lib/bullet/src/LinearMath/btThreads.cpp @@ -12,18 +12,15 @@ subject to the following restrictions: 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 - +#endif // #if BT_USE_OPENMP && BT_THREADSAFE #if BT_USE_PPL && BT_THREADSAFE @@ -32,8 +29,7 @@ subject to the following restrictions: // Visual Studio 2010 and later should come with it #include // for GetProcessorCount() -#endif // #if BT_USE_PPL && BT_THREADSAFE - +#endif // #if BT_USE_PPL && BT_THREADSAFE #if BT_USE_TBB && BT_THREADSAFE @@ -44,8 +40,7 @@ subject to the following restrictions: #include #include -#endif // #if BT_USE_TBB && BT_THREADSAFE - +#endif // #if BT_USE_TBB && BT_THREADSAFE #if BT_THREADSAFE // @@ -53,7 +48,7 @@ subject to the following restrictions: // 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 @@ -61,25 +56,24 @@ subject to the following restrictions: // on GCC or Clang you need to compile with -std=c++11 #define USE_CPP11_ATOMICS 1 -#elif defined( _MSC_VER ) +#elif defined(_MSC_VER) // on MSVC, use intrinsics instead #define USE_MSVC_INTRINSICS 1 -#elif defined( __GNUC__ ) && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7)) +#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) +#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 @@ -89,27 +83,26 @@ subject to the following restrictions: 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 ); + 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 - } + // 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 ); + 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 @@ -117,148 +110,142 @@ void btSpinMutex::unlock() #include #include -#define THREAD_LOCAL_STATIC __declspec( thread ) static - +#define THREAD_LOCAL_STATIC __declspec(thread) static bool btSpinMutex::tryLock() { - volatile long* aDest = reinterpret_cast(&mLock); - return ( 0 == _InterlockedCompareExchange( aDest, 1, 0) ); + 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 - } + // note: this lock does not sleep the thread + while (!tryLock()) + { + // spin + } } void btSpinMutex::unlock() { - volatile long* aDest = reinterpret_cast( &mLock ); - _InterlockedExchange( aDest, 0 ); + 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); + 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 - } + // note: this lock does not sleep the thread + while (!tryLock()) + { + // spin + } } void btSpinMutex::unlock() { - __atomic_store_n(&mLock, int(0), __ATOMIC_RELEASE); + __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)); + 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 - } + // note: this lock does not sleep the thread + while (!tryLock()) + { + // spin + } } void btSpinMutex::unlock() { - // write 0 - __sync_fetch_and_and(&mLock, int(0)); + // write 0 + __sync_fetch_and_and(&mLock, int(0)); } -#else //#elif USE_MSVC_INTRINSICS +#else //#elif USE_MSVC_INTRINSICS #error "no threading primitives defined -- unknown platform" #endif //#else //#elif USE_MSVC_INTRINSICS -#else //#if BT_THREADSAFE +#else //#if BT_THREADSAFE // These should not be called ever void btSpinMutex::lock() { - btAssert( !"unimplemented btSpinMutex::lock() called" ); + btAssert(!"unimplemented btSpinMutex::lock() called"); } void btSpinMutex::unlock() { - btAssert( !"unimplemented btSpinMutex::unlock() called" ); + btAssert(!"unimplemented btSpinMutex::unlock() called"); } bool btSpinMutex::tryLock() { - btAssert( !"unimplemented btSpinMutex::tryLock() called" ); - return true; + btAssert(!"unimplemented btSpinMutex::tryLock() called"); + return true; } #define THREAD_LOCAL_STATIC static -#endif // #else //#if BT_THREADSAFE - +#endif // #else //#if BT_THREADSAFE struct ThreadsafeCounter { - unsigned int mCounter; - btSpinMutex mMutex; + unsigned int mCounter; + btSpinMutex mMutex; - ThreadsafeCounter() - { - mCounter = 0; - --mCounter; // first count should come back 0 - } + 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; - } + 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 btITaskScheduler* gBtTaskScheduler=0; 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. // @@ -276,7 +263,7 @@ static ThreadsafeCounter gThreadCounter; // 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 +// 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 @@ -290,169 +277,191 @@ static ThreadsafeCounter gThreadCounter; typedef DWORD ThreadId_t; const static ThreadId_t kInvalidThreadId = 0; -ThreadId_t gDebugThreadIds[ BT_MAX_THREAD_COUNT ]; +ThreadId_t gDebugThreadIds[BT_MAX_THREAD_COUNT]; static ThreadId_t getDebugThreadId() { - return GetCurrentThreadId(); + return GetCurrentThreadId(); } -#endif // #if BT_DETECT_BAD_THREAD_INDEX - +#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 ); - } + 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; + 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; + return btGetCurrentThreadIndex() == 0; } void btResetThreadIndexCounter() { - // for when all current worker threads are destroyed - btAssert( btIsMainThread() ); - gThreadCounter.mCounter = 0; + // for when all current worker threads are destroyed + btAssert(btIsMainThread()); + gThreadCounter.mCounter = 0; } -btITaskScheduler::btITaskScheduler( const char* name ) +btITaskScheduler::btITaskScheduler(const char* name) { - m_name = name; - m_savedThreadCounter = 0; - m_isActive = false; + 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; - } + // 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; - } + if (m_isActive) + { + m_savedThreadCounter = gThreadCounter.mCounter; // save thread counter + m_isActive = false; + } } void btPushThreadsAreRunning() { - gThreadsRunningCounterMutex.lock(); - gThreadsRunningCounter++; - gThreadsRunningCounterMutex.unlock(); + gThreadsRunningCounterMutex.lock(); + gThreadsRunningCounter++; + gThreadsRunningCounterMutex.unlock(); } void btPopThreadsAreRunning() { - gThreadsRunningCounterMutex.lock(); - gThreadsRunningCounter--; - gThreadsRunningCounterMutex.unlock(); + gThreadsRunningCounterMutex.lock(); + gThreadsRunningCounter--; + gThreadsRunningCounterMutex.unlock(); } bool btThreadsAreRunning() { - return gThreadsRunningCounter != 0; + return gThreadsRunningCounter != 0; } - -void btSetTaskScheduler( btITaskScheduler* ts ) +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(); - } + 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; + return gBtTaskScheduler; } - -void btParallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) +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 + 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 ); + btAssert(gBtTaskScheduler != NULL); // call btSetTaskScheduler() with a valid task scheduler first! + gBtTaskScheduler->parallelFor(iBegin, iEnd, grainSize, body); -#else // #if BT_THREADSAFE +#else // #if BT_THREADSAFE - // non-parallel version of btParallelFor - btAssert( !"called btParallelFor in non-threadsafe build. enable BT_THREADSAFE" ); - body.forLoop( iBegin, iEnd ); + // non-parallel version of btParallelFor + btAssert(!"called btParallelFor in non-threadsafe build. enable BT_THREADSAFE"); + body.forLoop(iBegin, iEnd); -#endif// #if BT_THREADSAFE +#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 @@ -461,67 +470,86 @@ void btParallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBod 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 ); - } + 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_job" ); - body.forLoop( i, ( std::min )( i + grainSize, iEnd ) ); - } - btPopThreadsAreRunning(); - } -}; -#endif // #if BT_USE_OPENMP && BT_THREADSAFE + 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 /// @@ -529,74 +557,94 @@ public: /// class btTaskSchedulerTBB : public btITaskScheduler { - int m_numThreads; - tbb::task_scheduler_init* m_tbbSchedulerInit; + 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; - } - } + 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 BodyAdapter - { - const btIParallelForBody* mBody; + 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; - void operator()( const tbb::blocked_range& range ) const - { - BT_PROFILE( "TBB_job" ); - mBody->forLoop( range.begin(), range.end() ); - } - }; - virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) BT_OVERRIDE - { - BT_PROFILE( "parallelFor_TBB" ); - // TBB dispatch - BodyAdapter tbbBody; - tbbBody.mBody = &body; - btPushThreadsAreRunning(); - tbb::parallel_for( tbb::blocked_range( iBegin, iEnd, grainSize ), - tbbBody, - tbb::simple_partitioner() - ); - btPopThreadsAreRunning(); - } + 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 - +#endif // #if BT_USE_TBB && BT_THREADSAFE #if BT_USE_PPL && BT_THREADSAFE /// @@ -604,119 +652,141 @@ public: /// class btTaskSchedulerPPL : public btITaskScheduler { - int m_numThreads; + 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 BodyAdapter - { - const btIParallelForBody* mBody; - int mGrainSize; - int mIndexEnd; + 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; - void operator()( int i ) const - { - BT_PROFILE( "PPL_job" ); - 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 - BodyAdapter pplBody; - pplBody.mBody = &body; - pplBody.mGrainSize = grainSize; - pplBody.mIndexEnd = iEnd; - btPushThreadsAreRunning(); - // note: MSVC 2010 doesn't support partitioner args, so avoid them - concurrency::parallel_for( iBegin, - iEnd, - grainSize, - pplBody - ); - btPopThreadsAreRunning(); - } + 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 - +#endif // #if BT_USE_PPL && BT_THREADSAFE // create a non-threaded task scheduler (always available) btITaskScheduler* btGetSequentialTaskScheduler() { - static btTaskSchedulerSequential sTaskScheduler; - return &sTaskScheduler; + 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; + static btTaskSchedulerOpenMP sTaskScheduler; + return &sTaskScheduler; #else - return NULL; + 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; + static btTaskSchedulerTBB sTaskScheduler; + return &sTaskScheduler; #else - return NULL; + 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; + static btTaskSchedulerPPL sTaskScheduler; + return &sTaskScheduler; #else - return NULL; + return NULL; #endif } - diff --git a/Engine/lib/bullet/src/LinearMath/btThreads.h b/Engine/lib/bullet/src/LinearMath/btThreads.h index 05fd15ec8..b2227e172 100644 --- a/Engine/lib/bullet/src/LinearMath/btThreads.h +++ b/Engine/lib/bullet/src/LinearMath/btThreads.h @@ -12,14 +12,12 @@ subject to the following restrictions: 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 +#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE -#if defined (_MSC_VER) && _MSC_VER >= 1600 +#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 @@ -28,13 +26,15 @@ subject to the following restrictions: #define BT_OVERRIDE #endif +// Don't set this to larger than 64, without modifying btThreadSupportPosix +// and btThreadSupportWin32. They use UINT64 bit-masks. 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 +void btResetThreadIndexCounter(); // notify that all worker threads have been destroyed /// /// btSpinMutex -- lightweight spin-mutex implemented with atomic ops, never puts @@ -44,19 +44,18 @@ void btResetThreadIndexCounter(); // notify that all worker threads have been de /// class btSpinMutex { - int mLock; + int mLock; public: - btSpinMutex() - { - mLock = 0; - } - void lock(); - void unlock(); - bool tryLock(); + btSpinMutex() + { + mLock = 0; + } + void lock(); + void unlock(); + bool tryLock(); }; - // // NOTE: btMutex* is for internal Bullet use only // @@ -68,29 +67,33 @@ public: // 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 ) +SIMD_FORCE_INLINE void btMutexLock(btSpinMutex* mutex) { #if BT_THREADSAFE - mutex->lock(); -#endif // #if BT_THREADSAFE -} - -SIMD_FORCE_INLINE void btMutexUnlock( btSpinMutex* mutex ) -{ -#if BT_THREADSAFE - mutex->unlock(); -#endif // #if BT_THREADSAFE -} - -SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* mutex ) -{ -#if BT_THREADSAFE - return mutex->tryLock(); + mutex->lock(); #else - return true; -#endif // #if BT_THREADSAFE + (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 @@ -98,8 +101,19 @@ SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* mutex ) class btIParallelForBody { public: - virtual ~btIParallelForBody() {} - virtual void forLoop( int iBegin, int iEnd ) const = 0; + 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; }; // @@ -109,28 +123,30 @@ public: class btITaskScheduler { public: - btITaskScheduler( const char* name ); - virtual ~btITaskScheduler() {} - const char* getName() const { return m_name; } + 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 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(); + // internal use only + virtual void activate(); + virtual void deactivate(); protected: - const char* m_name; - unsigned int m_savedThreadCounter; - bool m_isActive; + 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 ); +void btSetTaskScheduler(btITaskScheduler* ts); // get the current task scheduler btITaskScheduler* btGetTaskScheduler(); @@ -138,6 +154,9 @@ 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(); @@ -149,7 +168,10 @@ 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 ); +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/Engine/lib/bullet/src/LinearMath/btTransform.h b/Engine/lib/bullet/src/LinearMath/btTransform.h index d4f939a5d..6f2f99818 100644 --- a/Engine/lib/bullet/src/LinearMath/btTransform.h +++ b/Engine/lib/bullet/src/LinearMath/btTransform.h @@ -12,12 +12,9 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_TRANSFORM_H #define BT_TRANSFORM_H - #include "btMatrix3x3.h" #ifdef BT_USE_DOUBLE_PRECISION @@ -26,46 +23,45 @@ subject to the following restrictions: #define btTransformData btTransformFloatData #endif - - - /**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear. *It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */ -ATTRIBUTE_ALIGNED16(class) btTransform { - - ///Storage for the rotation +ATTRIBUTE_ALIGNED16(class) +btTransform +{ + ///Storage for the rotation btMatrix3x3 m_basis; - ///Storage for the translation - btVector3 m_origin; + ///Storage for the translation + btVector3 m_origin; public: - - /**@brief No initialization constructor */ + /**@brief No initialization constructor */ btTransform() {} - /**@brief Constructor from btQuaternion (optional btVector3 ) + /**@brief Constructor from btQuaternion (optional btVector3 ) * @param q Rotation from quaternion * @param c Translation from Vector (default 0,0,0) */ - explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q, - const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) + explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q, + const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) : m_basis(q), - m_origin(c) - {} - - /**@brief Constructor from btMatrix3x3 (optional btVector3) - * @param b Rotation from Matrix - * @param c Translation from Vector default (0,0,0)*/ - explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b, - const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) - : m_basis(b), - m_origin(c) - {} - /**@brief Copy constructor */ - SIMD_FORCE_INLINE btTransform (const btTransform& other) - : m_basis(other.m_basis), - m_origin(other.m_origin) + m_origin(c) { } - /**@brief Assignment Operator */ + + /**@brief Constructor from btMatrix3x3 (optional btVector3) + * @param b Rotation from Matrix + * @param c Translation from Vector default (0,0,0)*/ + explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b, + const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) + : m_basis(b), + m_origin(c) + { + } + /**@brief Copy constructor */ + SIMD_FORCE_INLINE btTransform(const btTransform& other) + : m_basis(other.m_basis), + m_origin(other.m_origin) + { + } + /**@brief Assignment Operator */ SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other) { m_basis = other.m_basis; @@ -73,70 +69,70 @@ public: return *this; } - - /**@brief Set the current transform as the value of the product of two transforms + /**@brief Set the current transform as the value of the product of two transforms * @param t1 Transform 1 * @param t2 Transform 2 * This = Transform1 * Transform2 */ - SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) { - m_basis = t1.m_basis * t2.m_basis; - m_origin = t1(t2.m_origin); - } + SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) + { + m_basis = t1.m_basis * t2.m_basis; + m_origin = t1(t2.m_origin); + } -/* void multInverseLeft(const btTransform& t1, const btTransform& t2) { + /* void multInverseLeft(const btTransform& t1, const btTransform& t2) { btVector3 v = t2.m_origin - t1.m_origin; m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis); m_origin = v * t1.m_basis; } */ -/**@brief Return the transform of the vector */ + /**@brief Return the transform of the vector */ SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const { - return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin; + return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin; } - /**@brief Return the transform of the vector */ + /**@brief Return the transform of the vector */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const { return (*this)(x); } - /**@brief Return the transform of the btQuaternion */ + /**@brief Return the transform of the btQuaternion */ SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q) const { return getRotation() * q; } - /**@brief Return the basis matrix for the rotation */ - SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; } - /**@brief Return the basis matrix for the rotation */ - SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; } + /**@brief Return the basis matrix for the rotation */ + SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; } + /**@brief Return the basis matrix for the rotation */ + SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; } - /**@brief Return the origin vector translation */ - SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; } - /**@brief Return the origin vector translation */ - SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; } + /**@brief Return the origin vector translation */ + SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; } + /**@brief Return the origin vector translation */ + SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; } - /**@brief Return a quaternion representing the rotation */ - btQuaternion getRotation() const { + /**@brief Return a quaternion representing the rotation */ + btQuaternion getRotation() const + { btQuaternion q; m_basis.getRotation(q); return q; } - - - /**@brief Set from an array + + /**@brief Set from an array * @param m A pointer to a 16 element array (12 rotation(row major padded on the right by 1), and 3 translation */ - void setFromOpenGLMatrix(const btScalar *m) + void setFromOpenGLMatrix(const btScalar* m) { m_basis.setFromOpenGLSubMatrix(m); - m_origin.setValue(m[12],m[13],m[14]); + m_origin.setValue(m[12], m[13], m[14]); } - /**@brief Fill an array representation + /**@brief Fill an array representation * @param m A pointer to a 16 element array (12 rotation(row major padded on the right by 1), and 3 translation */ - void getOpenGLMatrix(btScalar *m) const + void getOpenGLMatrix(btScalar * m) const { m_basis.getOpenGLSubMatrix(m); m[12] = m_origin.x(); @@ -145,80 +141,76 @@ public: m[15] = btScalar(1.0); } - /**@brief Set the translational element + /**@brief Set the translational element * @param origin The vector to set the translation to */ - SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) - { + SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) + { m_origin = origin; } SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const; - - /**@brief Set the rotational element by btMatrix3x3 */ + /**@brief Set the rotational element by btMatrix3x3 */ SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis) - { + { m_basis = basis; } - /**@brief Set the rotational element by btQuaternion */ + /**@brief Set the rotational element by btQuaternion */ SIMD_FORCE_INLINE void setRotation(const btQuaternion& q) { m_basis.setRotation(q); } - - /**@brief Set this transformation to the identity */ + /**@brief Set this transformation to the identity */ void setIdentity() { m_basis.setIdentity(); m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); } - /**@brief Multiply this Transform by another(this = this * another) + /**@brief Multiply this Transform by another(this = this * another) * @param t The other transform */ - btTransform& operator*=(const btTransform& t) + btTransform& operator*=(const btTransform& t) { m_origin += m_basis * t.m_origin; m_basis *= t.m_basis; return *this; } - /**@brief Return the inverse of this transform */ + /**@brief Return the inverse of this transform */ btTransform inverse() const - { + { btMatrix3x3 inv = m_basis.transpose(); return btTransform(inv, inv * -m_origin); } - /**@brief Return the inverse of this transform times the other transform + /**@brief Return the inverse of this transform times the other transform * @param t The other transform * return this.inverse() * the other */ - btTransform inverseTimes(const btTransform& t) const; + btTransform inverseTimes(const btTransform& t) const; - /**@brief Return the product of this transform and the other */ + /**@brief Return the product of this transform and the other */ btTransform operator*(const btTransform& t) const; - /**@brief Return an identity transform */ - static const btTransform& getIdentity() + /**@brief Return an identity transform */ + static const btTransform& getIdentity() { static const btTransform identityTransform(btMatrix3x3::getIdentity()); return identityTransform; } - void serialize(struct btTransformData& dataOut) const; + void serialize(struct btTransformData & dataOut) const; - void serializeFloat(struct btTransformFloatData& dataOut) const; + void serializeFloat(struct btTransformFloatData & dataOut) const; - void deSerialize(const struct btTransformData& dataIn); + void deSerialize(const struct btTransformData& dataIn); - void deSerializeDouble(const struct btTransformDoubleData& dataIn); - - void deSerializeFloat(const struct btTransformFloatData& dataIn); + void deSerializeDouble(const struct btTransformDoubleData& dataIn); + void deSerializeFloat(const struct btTransformFloatData& dataIn); }; - SIMD_FORCE_INLINE btVector3 btTransform::invXform(const btVector3& inVec) const { @@ -226,80 +218,69 @@ btTransform::invXform(const btVector3& inVec) const return (m_basis.transpose() * v); } -SIMD_FORCE_INLINE btTransform -btTransform::inverseTimes(const btTransform& t) const +SIMD_FORCE_INLINE btTransform +btTransform::inverseTimes(const btTransform& t) const { btVector3 v = t.getOrigin() - m_origin; - return btTransform(m_basis.transposeTimes(t.m_basis), - v * m_basis); + return btTransform(m_basis.transposeTimes(t.m_basis), + v * m_basis); } -SIMD_FORCE_INLINE btTransform -btTransform::operator*(const btTransform& t) const +SIMD_FORCE_INLINE btTransform + btTransform::operator*(const btTransform& t) const { - return btTransform(m_basis * t.m_basis, - (*this)(t.m_origin)); + return btTransform(m_basis * t.m_basis, + (*this)(t.m_origin)); } /**@brief Test if two transforms have all elements equal */ SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2) { - return ( t1.getBasis() == t2.getBasis() && - t1.getOrigin() == t2.getOrigin() ); + return (t1.getBasis() == t2.getBasis() && + t1.getOrigin() == t2.getOrigin()); } - ///for serialization -struct btTransformFloatData +struct btTransformFloatData { - btMatrix3x3FloatData m_basis; - btVector3FloatData m_origin; + btMatrix3x3FloatData m_basis; + btVector3FloatData m_origin; }; -struct btTransformDoubleData +struct btTransformDoubleData { - btMatrix3x3DoubleData m_basis; - btVector3DoubleData m_origin; + btMatrix3x3DoubleData m_basis; + btVector3DoubleData m_origin; }; - - -SIMD_FORCE_INLINE void btTransform::serialize(btTransformData& dataOut) const +SIMD_FORCE_INLINE void btTransform::serialize(btTransformData& dataOut) const { m_basis.serialize(dataOut.m_basis); m_origin.serialize(dataOut.m_origin); } -SIMD_FORCE_INLINE void btTransform::serializeFloat(btTransformFloatData& dataOut) const +SIMD_FORCE_INLINE void btTransform::serializeFloat(btTransformFloatData& dataOut) const { m_basis.serializeFloat(dataOut.m_basis); m_origin.serializeFloat(dataOut.m_origin); } - -SIMD_FORCE_INLINE void btTransform::deSerialize(const btTransformData& dataIn) +SIMD_FORCE_INLINE void btTransform::deSerialize(const btTransformData& dataIn) { m_basis.deSerialize(dataIn.m_basis); m_origin.deSerialize(dataIn.m_origin); } -SIMD_FORCE_INLINE void btTransform::deSerializeFloat(const btTransformFloatData& dataIn) +SIMD_FORCE_INLINE void btTransform::deSerializeFloat(const btTransformFloatData& dataIn) { m_basis.deSerializeFloat(dataIn.m_basis); m_origin.deSerializeFloat(dataIn.m_origin); } -SIMD_FORCE_INLINE void btTransform::deSerializeDouble(const btTransformDoubleData& dataIn) +SIMD_FORCE_INLINE void btTransform::deSerializeDouble(const btTransformDoubleData& dataIn) { m_basis.deSerializeDouble(dataIn.m_basis); m_origin.deSerializeDouble(dataIn.m_origin); } - -#endif //BT_TRANSFORM_H - - - - - - +#endif //BT_TRANSFORM_H diff --git a/Engine/lib/bullet/src/LinearMath/btTransformUtil.h b/Engine/lib/bullet/src/LinearMath/btTransformUtil.h index 182cc43fa..b874dd680 100644 --- a/Engine/lib/bullet/src/LinearMath/btTransformUtil.h +++ b/Engine/lib/bullet/src/LinearMath/btTransformUtil.h @@ -12,77 +12,66 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_TRANSFORM_UTIL_H #define BT_TRANSFORM_UTIL_H #include "btTransform.h" -#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI +#define ANGULAR_MOTION_THRESHOLD btScalar(0.5) * SIMD_HALF_PI - - - -SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) +SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents, const btVector3& supportDir) { return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), - supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), - supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); + supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), + supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); } - - - - - /// Utils related to temporal transforms class btTransformUtil { - public: - - static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform) + static void integrateTransform(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btTransform& predictedTransform) { predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep); -// #define QUATERNION_DERIVATIVE - #ifdef QUATERNION_DERIVATIVE + // #define QUATERNION_DERIVATIVE +#ifdef QUATERNION_DERIVATIVE btQuaternion predictedOrn = curTrans.getRotation(); predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); predictedOrn.safeNormalize(); - #else +#else //Exponential map //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia btVector3 axis; - btScalar fAngle2 = angvel.length2(); - btScalar fAngle = 0; - if (fAngle2>SIMD_EPSILON) - { - fAngle = btSqrt(fAngle2); - } + btScalar fAngle2 = angvel.length2(); + btScalar fAngle = 0; + if (fAngle2 > SIMD_EPSILON) + { + fAngle = btSqrt(fAngle2); + } //limit the angular motion - if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD) + if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD) { fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; } - if ( fAngle < btScalar(0.001) ) + if (fAngle < btScalar(0.001)) { // use Taylor's expansions of sync function - axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle ); + axis = angvel * (btScalar(0.5) * timeStep - (timeStep * timeStep * timeStep) * (btScalar(0.020833333333)) * fAngle * fAngle); } else { // sync(fAngle) = sin(c*fAngle)/t - axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle ); + axis = angvel * (btSin(btScalar(0.5) * fAngle * timeStep) / fAngle); } - btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) )); + btQuaternion dorn(axis.x(), axis.y(), axis.z(), btCos(fAngle * timeStep * btScalar(0.5))); btQuaternion orn0 = curTrans.getRotation(); btQuaternion predictedOrn = dorn * orn0; predictedOrn.safeNormalize(); - #endif - if (predictedOrn.length2()>SIMD_EPSILON) +#endif + if (predictedOrn.length2() > SIMD_EPSILON) { predictedTransform.setRotation(predictedOrn); } @@ -92,137 +81,133 @@ public: } } - static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel) + static void calculateVelocityQuaternion(const btVector3& pos0, const btVector3& pos1, const btQuaternion& orn0, const btQuaternion& orn1, btScalar timeStep, btVector3& linVel, btVector3& angVel) { linVel = (pos1 - pos0) / timeStep; btVector3 axis; - btScalar angle; + btScalar angle; if (orn0 != orn1) { - calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle); + calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle); angVel = axis * angle / timeStep; - } else + } + else { - angVel.setValue(0,0,0); + angVel.setValue(0, 0, 0); } } - static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle) + static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0, const btQuaternion& orn1a, btVector3& axis, btScalar& angle) { btQuaternion orn1 = orn0.nearest(orn1a); btQuaternion dorn = orn1 * orn0.inverse(); angle = dorn.getAngle(); - axis = btVector3(dorn.x(),dorn.y(),dorn.z()); + axis = btVector3(dorn.x(), dorn.y(), dorn.z()); axis[3] = btScalar(0.); //check for axis length btScalar len = axis.length2(); - if (len < SIMD_EPSILON*SIMD_EPSILON) - axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); + if (len < SIMD_EPSILON * SIMD_EPSILON) + axis = btVector3(btScalar(1.), btScalar(0.), btScalar(0.)); else axis /= btSqrt(len); } - static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel) + static void calculateVelocity(const btTransform& transform0, const btTransform& transform1, btScalar timeStep, btVector3& linVel, btVector3& angVel) { linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; btVector3 axis; - btScalar angle; - calculateDiffAxisAngle(transform0,transform1,axis,angle); + btScalar angle; + calculateDiffAxisAngle(transform0, transform1, axis, angle); angVel = axis * angle / timeStep; } - static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle) + static void calculateDiffAxisAngle(const btTransform& transform0, const btTransform& transform1, btVector3& axis, btScalar& angle) { btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse(); btQuaternion dorn; dmat.getRotation(dorn); - ///floating point inaccuracy can lead to w component > 1..., which breaks + ///floating point inaccuracy can lead to w component > 1..., which breaks dorn.normalize(); - + angle = dorn.getAngle(); - axis = btVector3(dorn.x(),dorn.y(),dorn.z()); + axis = btVector3(dorn.x(), dorn.y(), dorn.z()); axis[3] = btScalar(0.); //check for axis length btScalar len = axis.length2(); - if (len < SIMD_EPSILON*SIMD_EPSILON) - axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); + if (len < SIMD_EPSILON * SIMD_EPSILON) + axis = btVector3(btScalar(1.), btScalar(0.), btScalar(0.)); else axis /= btSqrt(len); } - }; - -///The btConvexSeparatingDistanceUtil can help speed up convex collision detection +///The btConvexSeparatingDistanceUtil can help speed up convex collision detection ///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance -class btConvexSeparatingDistanceUtil +class btConvexSeparatingDistanceUtil { - btQuaternion m_ornA; - btQuaternion m_ornB; - btVector3 m_posA; - btVector3 m_posB; - - btVector3 m_separatingNormal; + btQuaternion m_ornA; + btQuaternion m_ornB; + btVector3 m_posA; + btVector3 m_posB; - btScalar m_boundingRadiusA; - btScalar m_boundingRadiusB; - btScalar m_separatingDistance; + btVector3 m_separatingNormal; + + btScalar m_boundingRadiusA; + btScalar m_boundingRadiusB; + btScalar m_separatingDistance; public: - - btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB) - :m_boundingRadiusA(boundingRadiusA), - m_boundingRadiusB(boundingRadiusB), - m_separatingDistance(0.f) + btConvexSeparatingDistanceUtil(btScalar boundingRadiusA, btScalar boundingRadiusB) + : m_boundingRadiusA(boundingRadiusA), + m_boundingRadiusB(boundingRadiusB), + m_separatingDistance(0.f) { } - btScalar getConservativeSeparatingDistance() + btScalar getConservativeSeparatingDistance() { return m_separatingDistance; } - void updateSeparatingDistance(const btTransform& transA,const btTransform& transB) + void updateSeparatingDistance(const btTransform& transA, const btTransform& transB) { const btVector3& toPosA = transA.getOrigin(); const btVector3& toPosB = transB.getOrigin(); btQuaternion toOrnA = transA.getRotation(); btQuaternion toOrnB = transB.getRotation(); - if (m_separatingDistance>0.f) + if (m_separatingDistance > 0.f) { - - - btVector3 linVelA,angVelA,linVelB,angVelB; - btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA); - btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB); + btVector3 linVelA, angVelA, linVelB, angVelB; + btTransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, btScalar(1.), linVelA, angVelA); + btTransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, btScalar(1.), linVelB, angVelB); btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB; - btVector3 relLinVel = (linVelB-linVelA); + btVector3 relLinVel = (linVelB - linVelA); btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal); - if (relLinVelocLength<0.f) + if (relLinVelocLength < 0.f) { relLinVelocLength = 0.f; } - - btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength; + + btScalar projectedMotion = maxAngularProjectedVelocity + relLinVelocLength; m_separatingDistance -= projectedMotion; } - + m_posA = toPosA; m_posB = toPosB; m_ornA = toOrnA; m_ornB = toOrnB; } - void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB) + void initSeparatingDistance(const btVector3& separatingVector, btScalar separatingDistance, const btTransform& transA, const btTransform& transB) { m_separatingDistance = separatingDistance; - if (m_separatingDistance>0.f) + if (m_separatingDistance > 0.f) { m_separatingNormal = separatingVector; - + const btVector3& toPosA = transA.getOrigin(); const btVector3& toPosB = transB.getOrigin(); btQuaternion toOrnA = transA.getRotation(); @@ -233,9 +218,6 @@ public: m_ornB = toOrnB; } } - }; - -#endif //BT_TRANSFORM_UTIL_H - +#endif //BT_TRANSFORM_UTIL_H diff --git a/Engine/lib/bullet/src/LinearMath/btVector3.cpp b/Engine/lib/bullet/src/LinearMath/btVector3.cpp index e05bdccd6..13111157a 100644 --- a/Engine/lib/bullet/src/LinearMath/btVector3.cpp +++ b/Engine/lib/bullet/src/LinearMath/btVector3.cpp @@ -15,282 +15,285 @@ This source version has been altered. */ -#if defined (_WIN32) || defined (__i386__) +#if defined(_WIN32) || defined(__i386__) #define BT_USE_SSE_IN_API #endif - #include "btVector3.h" - - #if defined BT_USE_SIMD_VECTOR3 #if DEBUG -#include //for memset +#include //for memset #endif - #ifdef __APPLE__ #include -typedef float float4 __attribute__ ((vector_size(16))); +typedef float float4 __attribute__((vector_size(16))); #else #define float4 __m128 #endif //typedef uint32_t uint4 __attribute__ ((vector_size(16))); - #if defined BT_USE_SSE || defined _WIN32 -#define LOG2_ARRAY_SIZE 6 -#define STACK_ARRAY_COUNT (1UL << LOG2_ARRAY_SIZE) +#define LOG2_ARRAY_SIZE 6 +#define STACK_ARRAY_COUNT (1UL << LOG2_ARRAY_SIZE) #include -long _maxdot_large( const float *vv, const float *vec, unsigned long count, float *dotResult ); -long _maxdot_large( const float *vv, const float *vec, unsigned long count, float *dotResult ) +long _maxdot_large(const float *vv, const float *vec, unsigned long count, float *dotResult); +long _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 = btAssign128( -BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY ); - float4 vvec = _mm_loadu_ps( vec ); - float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa )); /// zzzz - float4 vLo = _mm_movelh_ps( vvec, vvec ); /// xyxy - - long maxIndex = -1L; - - size_t segment = 0; - float4 stack_array[ STACK_ARRAY_COUNT ]; - + 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 = btAssign128(-BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY); + float4 vvec = _mm_loadu_ps(vec); + float4 vHi = btCastiTo128f(_mm_shuffle_epi32(btCastfTo128i(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]) ); + //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( btUnlikely( 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 ) - { + + 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 (btUnlikely(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\ + 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\ @@ -316,368 +319,374 @@ long _maxdot_large( const float *vv, const float *vec, unsigned long count, floa 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; + : [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 ) - { - - + + size_t index; + float4 min; + // Faster loop without cleanup code for full tiles + for (segment = 0; segment + STACK_ARRAY_COUNT * 4 <= count; segment += STACK_ARRAY_COUNT * 4) + { + min = dotmin; + + 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; + 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. + } + + // If we found a new min + if (0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(min, dotmin))) + { + // copy the new min across all lanes of our min accumulator + min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0x4e)); + min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0xb1)); + + dotmin = min; + + // find first occurrence of that min + size_t test; + for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], min))); index++) // local_count must be a multiple of 4 + { + } + // record where it is. + minIndex = 4 * index + segment + indexTable[test]; + } + } + + // account for work we've already done + count -= segment; + + // Deal with the last < STACK_ARRAY_COUNT vectors + min = dotmin; + index = 0; + + if (btUnlikely(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; + 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\ + 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\ @@ -703,968 +712,953 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa 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; + : [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 #include -#include //for sysctlbyname +#include //for sysctlbyname -static long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ); -static long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult ); -static long _maxdot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult ); -static long _mindot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ); -static long _mindot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult ); -static long _mindot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult ); +static long _maxdot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult); +static long _maxdot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult); +static long _maxdot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult); +static long _mindot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult); +static long _mindot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult); +static long _mindot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult); -long (*_maxdot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _maxdot_large_sel; -long (*_mindot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _mindot_large_sel; +long (*_maxdot_large)(const float *vv, const float *vec, unsigned long count, float *dotResult) = _maxdot_large_sel; +long (*_mindot_large)(const float *vv, const float *vec, unsigned long count, float *dotResult) = _mindot_large_sel; - -static inline uint32_t btGetCpuCapabilities( void ) +static inline uint32_t btGetCpuCapabilities(void) { - static uint32_t capabilities = 0; - static bool testedCapabilities = false; + static uint32_t capabilities = 0; + static bool testedCapabilities = false; - if( 0 == testedCapabilities) - { - uint32_t hasFeature = 0; - size_t featureSize = sizeof( hasFeature ); - int err = sysctlbyname( "hw.optional.neon_hpfp", &hasFeature, &featureSize, NULL, 0 ); + if (0 == testedCapabilities) + { + uint32_t hasFeature = 0; + size_t featureSize = sizeof(hasFeature); + int err = sysctlbyname("hw.optional.neon_hpfp", &hasFeature, &featureSize, NULL, 0); - if( 0 == err && hasFeature) - capabilities |= 0x2000; + if (0 == err && hasFeature) + capabilities |= 0x2000; testedCapabilities = true; - } - - return capabilities; + } + + return capabilities; } - - - -static long _maxdot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult ) +static long _maxdot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult) { + if (btGetCpuCapabilities() & 0x2000) + _maxdot_large = _maxdot_large_v1; + else + _maxdot_large = _maxdot_large_v0; - if( btGetCpuCapabilities() & 0x2000 ) - _maxdot_large = _maxdot_large_v1; - else - _maxdot_large = _maxdot_large_v0; - - return _maxdot_large(vv, vec, count, dotResult); + return _maxdot_large(vv, vec, count, dotResult); } -static long _mindot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult ) +static long _mindot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult) { + if (btGetCpuCapabilities() & 0x2000) + _mindot_large = _mindot_large_v1; + else + _mindot_large = _mindot_large_v0; - if( btGetCpuCapabilities() & 0x2000 ) - _mindot_large = _mindot_large_v1; - else - _mindot_large = _mindot_large_v0; - - return _mindot_large(vv, vec, count, dotResult); + return _mindot_large(vv, vec, count, dotResult); } - - #if defined __arm__ -# define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; }) +#define vld1q_f32_aligned_postincrement(_ptr) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; }) #else //support 64bit arm -# define vld1q_f32_aligned_postincrement( _ptr) ({ float32x4_t _r = ((float32x4_t*)(_ptr))[0]; (_ptr) = (const float*) ((const char*)(_ptr) + 16L); /*return*/ _r; }) +#define vld1q_f32_aligned_postincrement(_ptr) ({ float32x4_t _r = ((float32x4_t*)(_ptr))[0]; (_ptr) = (const float*) ((const char*)(_ptr) + 16L); /*return*/ _r; }) #endif - -long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ) +long _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) { -BT_INFINITY, -BT_INFINITY }; - float32x2_t dotMaxHi = (float32x2_t) { -BT_INFINITY, -BT_INFINITY }; - uint32x2_t indexLo = (uint32x2_t) {0, 1}; - uint32x2_t indexHi = (uint32x2_t) {2, 3}; - uint32x2_t iLo = (uint32x2_t) {static_cast(-1), static_cast(-1)}; - uint32x2_t iHi = (uint32x2_t) {static_cast(-1), static_cast(-1)}; - const uint32x2_t four = (uint32x2_t) {4,4}; + 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){-BT_INFINITY, -BT_INFINITY}; + float32x2_t dotMaxHi = (float32x2_t){-BT_INFINITY, -BT_INFINITY}; + uint32x2_t indexLo = (uint32x2_t){0, 1}; + uint32x2_t indexHi = (uint32x2_t){2, 3}; + uint32x2_t iLo = (uint32x2_t){static_cast(-1), static_cast(-1)}; + uint32x2_t iHi = (uint32x2_t){static_cast(-1), static_cast(-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); + 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); - 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); - } + 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); - 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); + 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 _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult ) +long _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) { static_cast(-1), static_cast(-1), static_cast(-1), static_cast(-1) }; - float32x4_t maxDot = (float32x4_t) { -BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_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); + 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){static_cast(-1), static_cast(-1), static_cast(-1), static_cast(-1)}; + float32x4_t maxDot = (float32x4_t){-BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY}; - 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); - } + 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); - 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)); - // 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; + xy0 = vmulq_f32(xy0, vLo); + xy1 = vmulq_f32(xy1, vLo); - 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; + 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); - 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; + 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); - 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); - + 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 _mindot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ) +long _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) { BT_INFINITY, BT_INFINITY }; - float32x2_t dotMinHi = (float32x2_t) { BT_INFINITY, BT_INFINITY }; - uint32x2_t indexLo = (uint32x2_t) {0, 1}; - uint32x2_t indexHi = (uint32x2_t) {2, 3}; - uint32x2_t iLo = (uint32x2_t) {static_cast(-1), static_cast(-1)}; - uint32x2_t iHi = (uint32x2_t) {static_cast(-1), static_cast(-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); - } + 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){BT_INFINITY, BT_INFINITY}; + float32x2_t dotMinHi = (float32x2_t){BT_INFINITY, BT_INFINITY}; + uint32x2_t indexLo = (uint32x2_t){0, 1}; + uint32x2_t indexHi = (uint32x2_t){2, 3}; + uint32x2_t iLo = (uint32x2_t){static_cast(-1), static_cast(-1)}; + uint32x2_t iHi = (uint32x2_t){static_cast(-1), static_cast(-1)}; + const uint32x2_t four = (uint32x2_t){4, 4}; - 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); + 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 _mindot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult ) +long _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) { static_cast(-1), static_cast(-1), static_cast(-1), static_cast(-1) }; - float32x4_t minDot = (float32x4_t) { BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_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); - + 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){static_cast(-1), static_cast(-1), static_cast(-1), static_cast(-1)}; + float32x4_t minDot = (float32x4_t){BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_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 +#error Unhandled __APPLE__ arch #endif -#endif /* __APPLE__ */ - - +#endif /* __APPLE__ */ diff --git a/Engine/lib/bullet/src/LinearMath/btVector3.h b/Engine/lib/bullet/src/LinearMath/btVector3.h index fdf3fd796..d65ed9808 100644 --- a/Engine/lib/bullet/src/LinearMath/btVector3.h +++ b/Engine/lib/bullet/src/LinearMath/btVector3.h @@ -12,8 +12,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_VECTOR3_H #define BT_VECTOR3_H @@ -28,25 +26,24 @@ subject to the following restrictions: #else #define btVector3Data btVector3FloatData #define btVector3DataName "btVector3FloatData" -#endif //BT_USE_DOUBLE_PRECISION +#endif //BT_USE_DOUBLE_PRECISION #if defined BT_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' +#pragma warning(disable : 4556) // value of intrinsic immediate argument '4294967239' is out of range '0 - 255' #endif - -#define BT_SHUFFLE(x,y,z,w) ((w)<<6 | (z)<<4 | (y)<<2 | (x)) +#define BT_SHUFFLE(x, y, z, w) (((w) << 6 | (z) << 4 | (y) << 2 | (x)) & 0xff) //#define bt_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) ) -#define bt_pshufd_ps( _a, _mask ) _mm_shuffle_ps((_a), (_a), (_mask) ) -#define bt_splat3_ps( _a, _i ) bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i, 3) ) -#define bt_splat_ps( _a, _i ) bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i,_i) ) +#define bt_pshufd_ps(_a, _mask) _mm_shuffle_ps((_a), (_a), (_mask)) +#define bt_splat3_ps(_a, _i) bt_pshufd_ps((_a), BT_SHUFFLE(_i, _i, _i, 3)) +#define bt_splat_ps(_a, _i) bt_pshufd_ps((_a), BT_SHUFFLE(_i, _i, _i, _i)) #define btv3AbsiMask (_mm_set_epi32(0x00000000, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF)) -#define btvAbsMask (_mm_set_epi32( 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF)) +#define btvAbsMask (_mm_set_epi32(0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF)) #define btvFFF0Mask (_mm_set_epi32(0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF)) #define btv3AbsfMask btCastiTo128f(btv3AbsiMask) #define btvFFF0fMask btCastiTo128f(btvFFF0Mask) @@ -55,9 +52,9 @@ subject to the following restrictions: //there is an issue with XCode 3.2 (LCx errors) #define btvMzeroMask (_mm_set_ps(-0.0f, -0.0f, -0.0f, -0.0f)) -#define v1110 (_mm_set_ps(0.0f, 1.0f, 1.0f, 1.0f)) -#define vHalf (_mm_set_ps(0.5f, 0.5f, 0.5f, 0.5f)) -#define v1_5 (_mm_set_ps(1.5f, 1.5f, 1.5f, 1.5f)) +#define v1110 (_mm_set_ps(0.0f, 1.0f, 1.0f, 1.0f)) +#define vHalf (_mm_set_ps(0.5f, 0.5f, 0.5f, 0.5f)) +#define v1_5 (_mm_set_ps(1.5f, 1.5f, 1.5f, 1.5f)) //const __m128 ATTRIBUTE_ALIGNED16(btvMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f}; //const __m128 ATTRIBUTE_ALIGNED16(v1110) = {1.0f, 1.0f, 1.0f, 0.0f}; @@ -70,7 +67,7 @@ subject to the following restrictions: const float32x4_t ATTRIBUTE_ALIGNED16(btvMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f}; const int32x4_t ATTRIBUTE_ALIGNED16(btvFFF0Mask) = (int32x4_t){static_cast(0xFFFFFFFF), - static_cast(0xFFFFFFFF), static_cast(0xFFFFFFFF), 0x0}; + static_cast(0xFFFFFFFF), static_cast(0xFFFFFFFF), 0x0}; const int32x4_t ATTRIBUTE_ALIGNED16(btvAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF}; const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0}; @@ -80,50 +77,48 @@ const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FF * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers */ -ATTRIBUTE_ALIGNED16(class) btVector3 +ATTRIBUTE_ALIGNED16(class) +btVector3 { public: - BT_DECLARE_ALIGNED_ALLOCATOR(); -#if defined (__SPU__) && defined (__CELLOS_LV2__) - btScalar m_floats[4]; +#if defined(__SPU__) && defined(__CELLOS_LV2__) + btScalar m_floats[4]; + public: - SIMD_FORCE_INLINE const vec_float4& get128() const + SIMD_FORCE_INLINE const vec_float4& get128() const { return *((const vec_float4*)&m_floats[0]); } + public: -#else //__CELLOS_LV2__ __SPU__ - #if defined (BT_USE_SSE) || defined(BT_USE_NEON) // _WIN32 || ARM - union { - btSimdFloat4 mVec128; - btScalar m_floats[4]; - }; - SIMD_FORCE_INLINE btSimdFloat4 get128() const - { - return mVec128; - } - SIMD_FORCE_INLINE void set128(btSimdFloat4 v128) - { - mVec128 = v128; - } - #else - btScalar m_floats[4]; - #endif -#endif //__CELLOS_LV2__ __SPU__ - - public: - - /**@brief No initialization constructor */ - SIMD_FORCE_INLINE btVector3() +#else //__CELLOS_LV2__ __SPU__ +#if defined(BT_USE_SSE) || defined(BT_USE_NEON) // _WIN32 || ARM + union { + btSimdFloat4 mVec128; + btScalar m_floats[4]; + }; + SIMD_FORCE_INLINE btSimdFloat4 get128() const { + return mVec128; + } + SIMD_FORCE_INLINE void set128(btSimdFloat4 v128) + { + mVec128 = v128; + } +#else + btScalar m_floats[4]; +#endif +#endif //__CELLOS_LV2__ __SPU__ +public: + /**@brief No initialization constructor */ + SIMD_FORCE_INLINE btVector3() + { } - - - /**@brief Constructor from scalars + /**@brief Constructor from scalars * @param x X value * @param y Y value * @param z Z value @@ -136,9 +131,9 @@ public: m_floats[3] = btScalar(0.f); } -#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) )|| defined (BT_USE_NEON) - // Set Vector - SIMD_FORCE_INLINE btVector3( btSimdFloat4 v) +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) + // Set Vector + SIMD_FORCE_INLINE btVector3(btSimdFloat4 v) { mVec128 = v; } @@ -150,73 +145,72 @@ public: } // Assignment Operator - SIMD_FORCE_INLINE btVector3& - operator=(const btVector3& v) + SIMD_FORCE_INLINE btVector3& + operator=(const btVector3& v) { mVec128 = v.mVec128; - + return *this; } -#endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON) - -/**@brief Add a vector to this one +#endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON) + + /**@brief Add a vector to this one * @param The vector to add to this one */ SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v) { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) mVec128 = _mm_add_ps(mVec128, v.mVec128); #elif defined(BT_USE_NEON) mVec128 = vaddq_f32(mVec128, v.mVec128); #else - m_floats[0] += v.m_floats[0]; + 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 + /**@brief Subtract a vector from this one * @param The vector to subtract */ - SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) + SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) mVec128 = _mm_sub_ps(mVec128, v.mVec128); #elif defined(BT_USE_NEON) mVec128 = vsubq_f32(mVec128, v.mVec128); #else - m_floats[0] -= v.m_floats[0]; + 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 + + /**@brief Scale the vector * @param s Scale factor */ SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s) { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - __m128 vs = _mm_load_ss(&s); // (S 0 0 0) - vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) + __m128 vs = _mm_load_ss(&s); // (S 0 0 0) + vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0) mVec128 = _mm_mul_ps(mVec128, vs); #elif defined(BT_USE_NEON) mVec128 = vmulq_n_f32(mVec128, s); #else - m_floats[0] *= s; + m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; #endif return *this; } - /**@brief Inversely scale the vector + /**@brief Inversely scale the vector * @param s Scale factor to divide by */ - SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) + SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) { btFullAssert(s != btScalar(0.0)); -#if 0 //defined(BT_USE_SSE_IN_API) +#if 0 //defined(BT_USE_SSE_IN_API) // this code is not faster ! __m128 vs = _mm_load_ss(&s); vs = _mm_div_ss(v1110, vs); @@ -230,11 +224,11 @@ public: #endif } - /**@brief Return the dot product + /**@brief Return the dot product * @param v The other vector in the dot product */ SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const { -#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_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); @@ -243,23 +237,23 @@ public: return _mm_cvtss_f32(vd); #elif defined(BT_USE_NEON) float32x4_t vd = vmulq_f32(mVec128, v.mVec128); - float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd)); + 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]; +#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 */ + /**@brief Return the length of the vector squared */ SIMD_FORCE_INLINE btScalar length2() const { return dot(*this); } - /**@brief Return the length of the vector */ + /**@brief Return the length of the vector */ SIMD_FORCE_INLINE btScalar length() const { return btSqrt(length2()); @@ -267,7 +261,7 @@ public: /**@brief Return the norm (length) of the vector */ SIMD_FORCE_INLINE btScalar norm() const - { + { return length(); } @@ -276,24 +270,24 @@ public: { btScalar d = length2(); //workaround for some clang/gcc issue of sqrtf(tiny number) = -INF - if (d>SIMD_EPSILON) + if (d > SIMD_EPSILON) return btSqrt(d); return btScalar(0); } - /**@brief Return the distance squared between the ends of this and another vector + /**@brief Return the distance squared between the ends of this and another vector * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; - /**@brief Return the distance between the ends of this and another vector + /**@brief Return the distance between the ends of this and another vector * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; - SIMD_FORCE_INLINE btVector3& safeNormalize() + SIMD_FORCE_INLINE btVector3& safeNormalize() { btScalar l2 = length2(); //triNormal.normalize(); - if (l2 >= SIMD_EPSILON*SIMD_EPSILON) + if (l2 >= SIMD_EPSILON * SIMD_EPSILON) { (*this) /= btSqrt(l2); } @@ -304,100 +298,97 @@ public: return *this; } - /**@brief Normalize this vector + /**@brief Normalize this vector * x^2 + y^2 + z^2 = 1 */ - SIMD_FORCE_INLINE btVector3& normalize() + SIMD_FORCE_INLINE btVector3& normalize() { - btAssert(!fuzzyZero()); -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - // dot product first +#if defined(BT_USE_SSE_IN_API) && defined(BT_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 + +#if 0 vd = _mm_sqrt_ss(vd); vd = _mm_div_ss(v1110, vd); vd = bt_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 = v1_5; - vd = _mm_mul_ss(vd, vHalf); // 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 +#else - y = _mm_mul_ss(y, z); // y0 * (1.5 - vd * 0.5 * y0 * y0) + // NR step 1/sqrt(x) - vd is x, y is output + y = _mm_rsqrt_ss(vd); // estimate + + // one step NR + z = v1_5; + vd = _mm_mul_ss(vd, vHalf); // 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 = bt_splat_ps(y, 0x80); mVec128 = _mm_mul_ps(mVec128, y); - #endif +#endif - return *this; -#else +#else return *this /= length(); #endif } - /**@brief Return a normalized version of this vector */ + /**@brief Return a normalized version of this vector */ SIMD_FORCE_INLINE btVector3 normalized() const; - /**@brief Return a rotated version of this vector + /**@brief Return a rotated version of this vector * @param wAxis The axis to rotate about * @param angle The angle to rotate by */ - SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const; + SIMD_FORCE_INLINE btVector3 rotate(const btVector3& wAxis, const btScalar angle) const; - /**@brief Return the angle between this and another vector + /**@brief Return the angle between this and another vector * @param v The other vector */ - SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const + SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const { btScalar s = btSqrt(length2() * v.length2()); btFullAssert(s != btScalar(0.0)); return btAcos(dot(v) / s); } - - /**@brief Return a vector will the absolute values of each element */ - SIMD_FORCE_INLINE btVector3 absolute() const - { -#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) + /**@brief Return a vector with the absolute values of each element */ + SIMD_FORCE_INLINE btVector3 absolute() const + { +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) return btVector3(_mm_and_ps(mVec128, btv3AbsfMask)); #elif defined(BT_USE_NEON) return btVector3(vabsq_f32(mVec128)); -#else +#else return btVector3( - btFabs(m_floats[0]), - btFabs(m_floats[1]), + btFabs(m_floats[0]), + btFabs(m_floats[1]), btFabs(m_floats[2])); #endif } - - /**@brief Return the cross product between this and another vector + + /**@brief Return the cross product between this and another vector * @param v The other vector */ SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - __m128 T, V; - - T = bt_pshufd_ps(mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) - V = bt_pshufd_ps(v.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) - +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) + __m128 T, V; + + T = bt_pshufd_ps(mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) + V = bt_pshufd_ps(v.mVec128, BT_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 = bt_pshufd_ps(V, BT_SHUFFLE(1, 2, 0, 3)); return btVector3(V); #elif defined(BT_USE_NEON) @@ -407,7 +398,7 @@ public: 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); @@ -415,7 +406,7 @@ public: // form (Y, Z, X, _); V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow); V = (float32x4_t)vandq_s32((int32x4_t)V, btvFFF0Mask); - + return btVector3(V); #else return btVector3( @@ -427,18 +418,18 @@ public: SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const { -#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) // cross: - __m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) - __m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) - + __m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) + __m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, BT_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, BT_SHUFFLE(1, 2, 0, 3)); - // dot: + // dot: V = _mm_mul_ps(V, mVec128); __m128 z = _mm_movehl_ps(V, V); __m128 y = _mm_shuffle_ps(V, V, 0x55); @@ -454,7 +445,7 @@ public: 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); @@ -462,31 +453,30 @@ public: // form (Y, Z, X, _); V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow); - // dot: + // dot: V = vmulq_f32(mVec128, V); - float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(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]); + 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 + /**@brief Return the axis with the smallest value * Note return values are 0,1,2 for x, y, or z */ SIMD_FORCE_INLINE int minAxis() const { - return m_floats[0] < m_floats[1] ? (m_floats[0] return this, t=1 => return other) */ - SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const + SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - __m128 vt = _mm_load_ss(&t); // (t 0 0 0) - vt = bt_pshufd_ps(vt, 0x80); // (rt rt rt 0.0) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) + __m128 vt = _mm_load_ss(&t); // (t 0 0 0) + vt = bt_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 btVector3(vl); #elif defined(BT_USE_NEON) float32x4_t vl = vsubq_f32(v.mVec128, mVec128); vl = vmulq_n_f32(vl, t); vl = vaddq_f32(vl, mVec128); - + return btVector3(vl); -#else - return - btVector3( m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, - m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, - m_floats[2] + (v.m_floats[2] - m_floats[2]) * t); +#else + return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, + m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, + m_floats[2] + (v.m_floats[2] - m_floats[2]) * t); #endif } - /**@brief Elementwise multiply this vector by the other + /**@brief Elementwise multiply this vector by the other * @param v The other vector */ SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v) { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) mVec128 = _mm_mul_ps(mVec128, v.mVec128); #elif defined(BT_USE_NEON) mVec128 = vmulq_f32(mVec128, v.mVec128); -#else - m_floats[0] *= v.m_floats[0]; +#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 */ - SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } - /**@brief Return the y value */ - SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } - /**@brief Return the z value */ - SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } - /**@brief Set the x value */ - SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x;}; - /**@brief Set the y value */ - SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y;}; - /**@brief Set the z value */ - SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z;}; - /**@brief Set the w value */ - SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w;}; - /**@brief Return the x value */ - SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } - /**@brief Return the y value */ - SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } - /**@brief Return the z value */ - SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } - /**@brief Return the w value */ - SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x; }; + /**@brief Set the y value */ + SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y; }; + /**@brief Set the z value */ + SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z; }; + /**@brief Set the w value */ + SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w; }; + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } + /**@brief Return the w value */ + SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } - //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } + //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. - SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } - SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } + SIMD_FORCE_INLINE operator btScalar*() { return &m_floats[0]; } + SIMD_FORCE_INLINE operator const btScalar*() const { return &m_floats[0]; } - SIMD_FORCE_INLINE bool operator==(const btVector3& other) const + SIMD_FORCE_INLINE bool operator==(const btVector3& other) const { -#if defined(BT_USE_SSE_IN_API) && defined (BT_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])); +#if defined(BT_USE_SSE_IN_API) && defined(BT_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 } - SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const + SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const { return !(*this == other); } - /**@brief Set each element to the max of the current values and the values of another btVector3 + /**@brief Set each element to the max of the current values and the values of another btVector3 * @param other The other btVector3 to compare with */ - SIMD_FORCE_INLINE void setMax(const btVector3& other) + SIMD_FORCE_INLINE void setMax(const btVector3& other) { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) mVec128 = _mm_max_ps(mVec128, other.mVec128); #elif defined(BT_USE_NEON) mVec128 = vmaxq_f32(mVec128, other.mVec128); @@ -632,12 +620,12 @@ public: #endif } - /**@brief Set each element to the min of the current values and the values of another btVector3 + /**@brief Set each element to the min of the current values and the values of another btVector3 * @param other The other btVector3 to compare with */ - SIMD_FORCE_INLINE void setMin(const btVector3& other) + SIMD_FORCE_INLINE void setMin(const btVector3& other) { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) mVec128 = _mm_min_ps(mVec128, other.mVec128); #elif defined(BT_USE_NEON) mVec128 = vminq_f32(mVec128, other.mVec128); @@ -649,154 +637,155 @@ public: #endif } - SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z) + SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z) { - m_floats[0]=_x; - m_floats[1]=_y; - m_floats[2]=_z; + m_floats[0] = _x; + m_floats[1] = _y; + m_floats[2] = _z; m_floats[3] = btScalar(0.f); } - void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const + void getSkewSymmetricMatrix(btVector3 * v0, btVector3 * v1, btVector3 * v2) const { -#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - - __m128 V = _mm_and_ps(mVec128, btvFFF0fMask); +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) + + __m128 V = _mm_and_ps(mVec128, btvFFF0fMask); __m128 V0 = _mm_xor_ps(btvMzeroMask, V); __m128 V2 = _mm_movelh_ps(V0, V); - + __m128 V1 = _mm_shuffle_ps(V, V0, 0xCE); - - V0 = _mm_shuffle_ps(V0, V, 0xDB); + + 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. ,-z() ,y()); - v1->setValue(z() ,0. ,-x()); - v2->setValue(-y() ,x() ,0.); + v0->setValue(0., -z(), y()); + v1->setValue(z(), 0., -x()); + v2->setValue(-y(), x(), 0.); #endif } void setZero() { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128); #elif defined(BT_USE_NEON) - int32x4_t vi = vdupq_n_s32(0); + int32x4_t vi = vdupq_n_s32(0); mVec128 = vreinterpretq_f32_s32(vi); -#else - setValue(btScalar(0.),btScalar(0.),btScalar(0.)); +#else + setValue(btScalar(0.), btScalar(0.), btScalar(0.)); #endif } - SIMD_FORCE_INLINE bool isZero() const + SIMD_FORCE_INLINE bool isZero() const { return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0); } - - SIMD_FORCE_INLINE bool fuzzyZero() const + SIMD_FORCE_INLINE bool fuzzyZero() const { - return length2() < SIMD_EPSILON*SIMD_EPSILON; + return length2() < SIMD_EPSILON * SIMD_EPSILON; } - SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const; + 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 serializeFloat(struct btVector3FloatData& dataOut) const; + SIMD_FORCE_INLINE void deSerialize(const struct btVector3FloatData& dataIn); - SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn); + SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData & dataOut) const; - SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData& dataOut) const; + SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn); - SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn); - - /**@brief returns index of maximum dot product between this and vectors in array[] + SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData & dataOut) const; + + SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& 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 */ - SIMD_FORCE_INLINE long maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const; + SIMD_FORCE_INLINE long maxDot(const btVector3* array, long array_count, btScalar& dotOut) const; - /**@brief returns index of minimum dot product between this and vectors in array[] + /**@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 */ - SIMD_FORCE_INLINE long minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const; + * @param dotOut The minimum dot product */ + SIMD_FORCE_INLINE long minDot(const btVector3* array, long array_count, btScalar& dotOut) const; - /* create a vector as btVector3( this->dot( btVector3 v0 ), this->dot( btVector3 v1), this->dot( btVector3 v2 )) */ - SIMD_FORCE_INLINE btVector3 dot3( const btVector3 &v0, const btVector3 &v1, const btVector3 &v2 ) const - { -#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) + /* create a vector as btVector3( this->dot( btVector3 v0 ), this->dot( btVector3 v1), this->dot( btVector3 v2 )) */ + SIMD_FORCE_INLINE btVector3 dot3(const btVector3& v0, const btVector3& v1, const btVector3& v2) const + { +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_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, btvxyzMaskf); + r = _mm_add_ps(r, btCastdTo128f(_mm_move_sd(btCastfTo128d(a2), btCastfTo128d(b1)))); + return btVector3(r); - __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, btvxyzMaskf); - r = _mm_add_ps( r, btCastdTo128f (_mm_move_sd( btCastfTo128d(a2), btCastfTo128d(b1) ))); - return btVector3(r); - #elif defined(BT_USE_NEON) - static const uint32x4_t xyzMask = (const uint32x4_t){ static_cast(-1), static_cast(-1), static_cast(-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 btVector3( vcombine_f32(b0, b1) ); -#else - return btVector3( dot(v0), dot(v1), dot(v2)); + static const uint32x4_t xyzMask = (const uint32x4_t){static_cast(-1), static_cast(-1), static_cast(-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 btVector3(vcombine_f32(b0, b1)); +#else + return btVector3(dot(v0), dot(v1), dot(v2)); #endif - } + } }; /**@brief Return the sum of two vectors (Point symantics)*/ -SIMD_FORCE_INLINE btVector3 -operator+(const btVector3& v1, const btVector3& v2) +SIMD_FORCE_INLINE btVector3 +operator+(const btVector3& v1, const btVector3& v2) { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) return btVector3(_mm_add_ps(v1.mVec128, v2.mVec128)); #elif defined(BT_USE_NEON) return btVector3(vaddq_f32(v1.mVec128, v2.mVec128)); #else return btVector3( - v1.m_floats[0] + v2.m_floats[0], - v1.m_floats[1] + v2.m_floats[1], - v1.m_floats[2] + v2.m_floats[2]); + 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 */ -SIMD_FORCE_INLINE btVector3 -operator*(const btVector3& v1, const btVector3& v2) +SIMD_FORCE_INLINE btVector3 +operator*(const btVector3& v1, const btVector3& v2) { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) return btVector3(_mm_mul_ps(v1.mVec128, v2.mVec128)); #elif defined(BT_USE_NEON) return btVector3(vmulq_f32(v1.mVec128, v2.mVec128)); #else return btVector3( - v1.m_floats[0] * v2.m_floats[0], - v1.m_floats[1] * v2.m_floats[1], - v1.m_floats[2] * v2.m_floats[2]); + 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 */ -SIMD_FORCE_INLINE btVector3 +SIMD_FORCE_INLINE btVector3 operator-(const btVector3& v1, const btVector3& v2) { -#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) +#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) // without _mm_and_ps this code causes slowdown in Concave moving __m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128); @@ -806,33 +795,33 @@ operator-(const btVector3& v1, const btVector3& v2) return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask)); #else return btVector3( - v1.m_floats[0] - v2.m_floats[0], - v1.m_floats[1] - v2.m_floats[1], - v1.m_floats[2] - v2.m_floats[2]); + 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 */ -SIMD_FORCE_INLINE btVector3 +SIMD_FORCE_INLINE btVector3 operator-(const btVector3& v) { -#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)) +#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) __m128 r = _mm_xor_ps(v.mVec128, btvMzeroMask); - return btVector3(_mm_and_ps(r, btvFFF0fMask)); + return btVector3(_mm_and_ps(r, btvFFF0fMask)); #elif defined(BT_USE_NEON) return btVector3((btSimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)btvMzeroMask)); -#else +#else return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); #endif } /**@brief Return the vector scaled by s */ -SIMD_FORCE_INLINE btVector3 +SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v, const btScalar& s) { -#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) - __m128 vs = _mm_load_ss(&s); // (S 0 0 0) - vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0) +#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) + __m128 vs = _mm_load_ss(&s); // (S 0 0 0) + vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0) return btVector3(_mm_mul_ps(v.mVec128, vs)); #elif defined(BT_USE_NEON) float32x4_t r = vmulq_n_f32(v.mVec128, s); @@ -843,10 +832,10 @@ operator*(const btVector3& v, const btScalar& s) } /**@brief Return the vector scaled by s */ -SIMD_FORCE_INLINE btVector3 +SIMD_FORCE_INLINE btVector3 operator*(const btScalar& s, const btVector3& v) -{ - return v * s; +{ + return v * s; } /**@brief Return the vector inversely scaled by s */ @@ -854,7 +843,7 @@ SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v, const btScalar& s) { btFullAssert(s != btScalar(0.0)); -#if 0 //defined(BT_USE_SSE_IN_API) +#if 0 //defined(BT_USE_SSE_IN_API) // this code is not faster ! __m128 vs = _mm_load_ss(&s); vs = _mm_div_ss(v1110, vs); @@ -870,67 +859,65 @@ operator/(const btVector3& v, const btScalar& s) SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v1, const btVector3& v2) { -#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) +#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) __m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128); vec = _mm_and_ps(vec, btvFFF0fMask); - return btVector3(vec); + return btVector3(vec); #elif defined(BT_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 + + 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 btVector3(v); #else return btVector3( - v1.m_floats[0] / v2.m_floats[0], - v1.m_floats[1] / v2.m_floats[1], - v1.m_floats[2] / v2.m_floats[2]); + 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 */ -SIMD_FORCE_INLINE btScalar -btDot(const btVector3& v1, const btVector3& v2) -{ - return v1.dot(v2); +SIMD_FORCE_INLINE btScalar +btDot(const btVector3& v1, const btVector3& v2) +{ + return v1.dot(v2); } - /**@brief Return the distance squared between two vectors */ SIMD_FORCE_INLINE btScalar -btDistance2(const btVector3& v1, const btVector3& v2) -{ - return v1.distance2(v2); +btDistance2(const btVector3& v1, const btVector3& v2) +{ + return v1.distance2(v2); } - /**@brief Return the distance between two vectors */ SIMD_FORCE_INLINE btScalar -btDistance(const btVector3& v1, const btVector3& v2) -{ - return v1.distance(v2); +btDistance(const btVector3& v1, const btVector3& v2) +{ + return v1.distance(v2); } /**@brief Return the angle between two vectors */ SIMD_FORCE_INLINE btScalar -btAngle(const btVector3& v1, const btVector3& v2) -{ - return v1.angle(v2); +btAngle(const btVector3& v1, const btVector3& v2) +{ + return v1.angle(v2); } /**@brief Return the cross product of two vectors */ -SIMD_FORCE_INLINE btVector3 -btCross(const btVector3& v1, const btVector3& v2) -{ - return v1.cross(v2); +SIMD_FORCE_INLINE btVector3 +btCross(const btVector3& v1, const btVector3& v2) +{ + return v1.cross(v2); } SIMD_FORCE_INLINE btScalar @@ -943,14 +930,12 @@ btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3) * @param v1 One vector * @param v2 The other vector * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ -SIMD_FORCE_INLINE btVector3 +SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) { return v1.lerp(v2, t); } - - SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const { return (v - *this).length2(); @@ -966,140 +951,137 @@ SIMD_FORCE_INLINE btVector3 btVector3::normalized() const btVector3 nrm = *this; return nrm.normalize(); -} +} -SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const +SIMD_FORCE_INLINE btVector3 btVector3::rotate(const btVector3& wAxis, const btScalar _angle) const { // wAxis must be a unit lenght vector -#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) - __m128 O = _mm_mul_ps(wAxis.mVec128, mVec128); - btScalar ssin = btSin( _angle ); - __m128 C = wAxis.cross( mVec128 ).mVec128; + __m128 O = _mm_mul_ps(wAxis.mVec128, mVec128); + btScalar ssin = btSin(_angle); + __m128 C = wAxis.cross(mVec128).mVec128; O = _mm_and_ps(O, btvFFF0fMask); - btScalar scos = btCos( _angle ); - - __m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0) - __m128 vcos = _mm_load_ss(&scos); // (S 0 0 0) - - __m128 Y = bt_pshufd_ps(O, 0xC9); // (Y Z X 0) - __m128 Z = bt_pshufd_ps(O, 0xD2); // (Z X Y 0) + btScalar scos = btCos(_angle); + + __m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0) + __m128 vcos = _mm_load_ss(&scos); // (S 0 0 0) + + __m128 Y = bt_pshufd_ps(O, 0xC9); // (Y Z X 0) + __m128 Z = bt_pshufd_ps(O, 0xD2); // (Z X Y 0) O = _mm_add_ps(O, Y); - vsin = bt_pshufd_ps(vsin, 0x80); // (S S S 0) + vsin = bt_pshufd_ps(vsin, 0x80); // (S S S 0) O = _mm_add_ps(O, Z); - vcos = bt_pshufd_ps(vcos, 0x80); // (S S S 0) - - vsin = vsin * C; - O = O * wAxis.mVec128; - __m128 X = mVec128 - O; - - O = O + vsin; + vcos = bt_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; - + O = O + vcos; + return btVector3(O); #else - btVector3 o = wAxis * wAxis.dot( *this ); + btVector3 o = wAxis * wAxis.dot(*this); btVector3 _x = *this - o; btVector3 _y; - _y = wAxis.cross( *this ); + _y = wAxis.cross(*this); - return ( o + _x * btCos( _angle ) + _y * btSin( _angle ) ); + return (o + _x * btCos(_angle) + _y * btSin(_angle)); #endif } -SIMD_FORCE_INLINE long btVector3::maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const +SIMD_FORCE_INLINE long btVector3::maxDot(const btVector3* array, long array_count, btScalar& dotOut) const { -#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON) - #if defined _WIN32 || defined (BT_USE_SSE) - const long scalar_cutoff = 10; - long _maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut ); - #elif defined BT_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 ) +#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined(BT_USE_NEON) +#if defined _WIN32 || defined(BT_USE_SSE) + const long scalar_cutoff = 10; + long _maxdot_large(const float* array, const float* vec, unsigned long array_count, float* dotOut); +#elif defined BT_USE_NEON + const long scalar_cutoff = 4; + extern long (*_maxdot_large)(const float* array, const float* vec, unsigned long array_count, float* dotOut); #endif - { - btScalar maxDot1 = -SIMD_INFINITY; - int i = 0; - int ptIndex = -1; - for( i = 0; i < array_count; i++ ) - { - btScalar dot = array[i].dot(*this); - - if( dot > maxDot1 ) - { - maxDot1 = dot; - ptIndex = i; - } - } - - dotOut = maxDot1; - return ptIndex; - } -#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON) - return _maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut ); + if (array_count < scalar_cutoff) +#endif + { + btScalar maxDot1 = -SIMD_INFINITY; + int i = 0; + int ptIndex = -1; + for (i = 0; i < array_count; i++) + { + btScalar dot = array[i].dot(*this); + + if (dot > maxDot1) + { + maxDot1 = dot; + ptIndex = i; + } + } + + dotOut = maxDot1; + return ptIndex; + } +#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined(BT_USE_NEON) + return _maxdot_large((float*)array, (float*)&m_floats[0], array_count, &dotOut); #endif } -SIMD_FORCE_INLINE long btVector3::minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const +SIMD_FORCE_INLINE long btVector3::minDot(const btVector3* array, long array_count, btScalar& dotOut) const { -#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON) - #if defined BT_USE_SSE - const long scalar_cutoff = 10; - long _mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut ); - #elif defined BT_USE_NEON - const long scalar_cutoff = 4; - extern long (*_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut ); - #else - #error unhandled arch! - #endif - - if( array_count < scalar_cutoff ) +#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined(BT_USE_NEON) +#if defined BT_USE_SSE + const long scalar_cutoff = 10; + long _mindot_large(const float* array, const float* vec, unsigned long array_count, float* dotOut); +#elif defined BT_USE_NEON + const long scalar_cutoff = 4; + extern long (*_mindot_large)(const float* array, const float* vec, unsigned long array_count, float* dotOut); +#else +#error unhandled arch! #endif - { - btScalar minDot = SIMD_INFINITY; - int i = 0; - int ptIndex = -1; - - for( i = 0; i < array_count; i++ ) - { - btScalar dot = array[i].dot(*this); - - if( dot < minDot ) - { - minDot = dot; - ptIndex = i; - } - } - - dotOut = minDot; - - return ptIndex; - } -#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON) - return _mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut ); -#endif//BT_USE_SIMD_VECTOR3 -} + if (array_count < scalar_cutoff) +#endif + { + btScalar minDot = SIMD_INFINITY; + int i = 0; + int ptIndex = -1; + + for (i = 0; i < array_count; i++) + { + btScalar dot = array[i].dot(*this); + + if (dot < minDot) + { + minDot = dot; + ptIndex = i; + } + } + + dotOut = minDot; + + return ptIndex; + } +#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined(BT_USE_NEON) + return _mindot_large((float*)array, (float*)&m_floats[0], array_count, &dotOut); +#endif //BT_USE_SIMD_VECTOR3 +} class btVector4 : public btVector3 { public: - SIMD_FORCE_INLINE btVector4() {} - - SIMD_FORCE_INLINE btVector4(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w) - : btVector3(_x,_y,_z) + SIMD_FORCE_INLINE btVector4(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w) + : btVector3(_x, _y, _z) { m_floats[3] = _w; } -#if (defined (BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined (BT_USE_NEON) +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) SIMD_FORCE_INLINE btVector4(const btSimdFloat4 vec) { mVec128 = vec; @@ -1110,34 +1092,32 @@ public: mVec128 = rhs.mVec128; } - SIMD_FORCE_INLINE btVector4& - operator=(const btVector4& v) + SIMD_FORCE_INLINE btVector4& + operator=(const btVector4& v) { mVec128 = v.mVec128; return *this; } -#endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON) +#endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON) - SIMD_FORCE_INLINE btVector4 absolute4() const + SIMD_FORCE_INLINE btVector4 absolute4() const { -#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) +#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE) return btVector4(_mm_and_ps(mVec128, btvAbsfMask)); #elif defined(BT_USE_NEON) return btVector4(vabsq_f32(mVec128)); -#else +#else return btVector4( - btFabs(m_floats[0]), - btFabs(m_floats[1]), + btFabs(m_floats[0]), + btFabs(m_floats[1]), btFabs(m_floats[2]), btFabs(m_floats[3])); #endif } + btScalar getW() const { return m_floats[3]; } - btScalar getW() const { return m_floats[3];} - - - SIMD_FORCE_INLINE int maxAxis4() const + SIMD_FORCE_INLINE int maxAxis4() const { int maxIndex = -1; btScalar maxVal = btScalar(-BT_LARGE_FLOAT); @@ -1154,7 +1134,7 @@ public: if (m_floats[2] > maxVal) { maxIndex = 2; - maxVal =m_floats[2]; + maxVal = m_floats[2]; } if (m_floats[3] > maxVal) { @@ -1164,7 +1144,6 @@ public: return maxIndex; } - SIMD_FORCE_INLINE int minAxis4() const { int minIndex = -1; @@ -1182,183 +1161,176 @@ public: if (m_floats[2] < minVal) { minIndex = 2; - minVal =m_floats[2]; + minVal = m_floats[2]; } if (m_floats[3] < minVal) { minIndex = 3; - minVal = m_floats[3]; } - + return minIndex; } - - SIMD_FORCE_INLINE int closestAxis4() const + SIMD_FORCE_INLINE int closestAxis4() const { return absolute4().maxAxis4(); } - - - - /**@brief Set x,y,z and zero w + /**@brief Set x,y,z and zero w * @param x Value of x * @param y Value of y * @param z Value of z */ - -/* void getValue(btScalar *m) const + /* void getValue(btScalar *m) const { m[0] = m_floats[0]; m[1] = m_floats[1]; m[2] =m_floats[2]; } */ -/**@brief Set the values + /**@brief Set the values * @param x Value of x * @param y Value of y * @param z Value of z * @param w Value of w */ - SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w) - { - m_floats[0]=_x; - m_floats[1]=_y; - m_floats[2]=_z; - m_floats[3]=_w; - } - - + SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w) + { + m_floats[0] = _x; + m_floats[1] = _y; + m_floats[2] = _z; + m_floats[3] = _w; + } }; - ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal) +SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal) { - #ifdef BT_USE_DOUBLE_PRECISION - unsigned char* dest = (unsigned char*) &destVal; - unsigned char* src = (unsigned char*) &sourceVal; +#ifdef BT_USE_DOUBLE_PRECISION + unsigned char* dest = (unsigned char*)&destVal; + const unsigned char* src = (const 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]; + 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; + unsigned char* dest = (unsigned char*)&destVal; + const unsigned char* src = (const unsigned char*)&sourceVal; dest[0] = src[3]; - dest[1] = src[2]; - dest[2] = src[1]; - dest[3] = src[0]; -#endif //BT_USE_DOUBLE_PRECISION + dest[1] = src[2]; + dest[2] = src[1]; + dest[3] = src[0]; +#endif //BT_USE_DOUBLE_PRECISION } ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec) +SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec) { - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) { - btSwapScalarEndian(sourceVec[i],destVec[i]); + btSwapScalarEndian(sourceVec[i], destVec[i]); } - } ///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector) +SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector) { - - btVector3 swappedVec; - for (int i=0;i<4;i++) + btVector3 swappedVec; + for (int i = 0; i < 4; i++) { - btSwapScalarEndian(vector[i],swappedVec[i]); + btSwapScalarEndian(vector[i], swappedVec[i]); } vector = swappedVec; } template -SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q) +SIMD_FORCE_INLINE void btPlaneSpace1(const T& n, T& p, T& q) { - if (btFabs(n[2]) > SIMDSQRT12) { - // choose p in y-z plane - btScalar a = n[1]*n[1] + n[2]*n[2]; - btScalar k = btRecipSqrt (a); - p[0] = 0; - p[1] = -n[2]*k; - p[2] = n[1]*k; - // 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 - btScalar a = n[0]*n[0] + n[1]*n[1]; - btScalar k = btRecipSqrt (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; - } + if (btFabs(n[2]) > SIMDSQRT12) + { + // choose p in y-z plane + btScalar a = n[1] * n[1] + n[2] * n[2]; + btScalar k = btRecipSqrt(a); + p[0] = 0; + p[1] = -n[2] * k; + p[2] = n[1] * k; + // 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 + btScalar a = n[0] * n[0] + n[1] * n[1]; + btScalar k = btRecipSqrt(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 btVector3FloatData +struct btVector3FloatData { - float m_floats[4]; + float m_floats[4]; }; -struct btVector3DoubleData +struct btVector3DoubleData { - double m_floats[4]; - + double m_floats[4]; }; -SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const +SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const { ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) dataOut.m_floats[i] = float(m_floats[i]); } -SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn) +SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn) { - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) m_floats[i] = btScalar(dataIn.m_floats[i]); } - -SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const +SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const { ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) dataOut.m_floats[i] = double(m_floats[i]); } -SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn) +SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn) { - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) m_floats[i] = btScalar(dataIn.m_floats[i]); } - -SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const +SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const { ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) 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] = dataIn.m_floats[i]; + for (int i = 0; i < 4; i++) + m_floats[i] = (btScalar)dataIn.m_floats[i]; } -#endif //BT_VECTOR3_H +SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3DoubleData& dataIn) +{ + for (int i = 0; i < 4; i++) + m_floats[i] = (btScalar)dataIn.m_floats[i]; +} + +#endif //BT_VECTOR3_H diff --git a/Engine/lib/bullet/src/LinearMath/premake4.lua b/Engine/lib/bullet/src/LinearMath/premake4.lua index 524e2c316..3765811a9 100644 --- a/Engine/lib/bullet/src/LinearMath/premake4.lua +++ b/Engine/lib/bullet/src/LinearMath/premake4.lua @@ -1,10 +1,15 @@ project "LinearMath" kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end includedirs { "..", } files { "*.cpp", - "*.h" + "*.h", + "TaskScheduler/*.cpp", + "TaskScheduler/*.h" } diff --git a/Engine/lib/bullet/src/btBulletCollisionAll.cpp b/Engine/lib/bullet/src/btBulletCollisionAll.cpp new file mode 100644 index 000000000..2851fb3b7 --- /dev/null +++ b/Engine/lib/bullet/src/btBulletCollisionAll.cpp @@ -0,0 +1,96 @@ +#include "BulletCollision/BroadphaseCollision/btAxisSweep3.cpp" +#include "BulletCollision/BroadphaseCollision/btDbvt.cpp" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp" +#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp" +#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp" +#include "BulletCollision/BroadphaseCollision/btDispatcher.cpp" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp" +#include "BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp" +#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp" +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp" +#include "BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btManifoldResult.cpp" +#include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp" +#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp" +#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp" +#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btCollisionObject.cpp" +#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp" +#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.cpp" +#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp" +#include "BulletCollision/CollisionDispatch/btUnionFind.cpp" +#include "BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp" +#include "BulletCollision/CollisionDispatch/btGhostObject.cpp" +#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp" +#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp" +#include "BulletCollision/NarrowPhaseCollision/btConvexCast.cpp" +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp" +#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp" +#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp" +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp" +#include "BulletCollision/CollisionShapes/btBox2dShape.cpp" +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.cpp" +#include "BulletCollision/CollisionShapes/btShapeHull.cpp" +#include "BulletCollision/CollisionShapes/btBoxShape.cpp" +#include "BulletCollision/CollisionShapes/btConvexShape.cpp" +#include "BulletCollision/CollisionShapes/btSphereShape.cpp" +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp" +#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.cpp" +#include "BulletCollision/CollisionShapes/btCapsuleShape.cpp" +#include "BulletCollision/CollisionShapes/btCylinderShape.cpp" +#include "BulletCollision/CollisionShapes/btStridingMeshInterface.cpp" +#include "BulletCollision/CollisionShapes/btCollisionShape.cpp" +#include "BulletCollision/CollisionShapes/btEmptyShape.cpp" +#include "BulletCollision/CollisionShapes/btTetrahedronShape.cpp" +#include "BulletCollision/CollisionShapes/btCompoundShape.cpp" +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp" +#include "BulletCollision/CollisionShapes/btTriangleBuffer.cpp" +#include "BulletCollision/CollisionShapes/btConcaveShape.cpp" +#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp" +#include "BulletCollision/CollisionShapes/btTriangleCallback.cpp" +#include "BulletCollision/CollisionShapes/btConeShape.cpp" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.cpp" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp" +#include "BulletCollision/CollisionShapes/btConvex2dShape.cpp" +#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp" +#include "BulletCollision/CollisionShapes/btConvexHullShape.cpp" +#include "BulletCollision/CollisionShapes/btOptimizedBvh.cpp" +#include "BulletCollision/CollisionShapes/btTriangleMesh.cpp" +#include "BulletCollision/CollisionShapes/btConvexInternalShape.cpp" +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp" +#include "BulletCollision/CollisionShapes/btTriangleMeshShape.cpp" +#include "BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp" +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp" +#include "BulletCollision/CollisionShapes/btSdfCollisionShape.cpp" +#include "BulletCollision/CollisionShapes/btMiniSDF.cpp" +#include "BulletCollision/CollisionShapes/btUniformScalingShape.cpp" +#include "BulletCollision/Gimpact/btContactProcessing.cpp" +#include "BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp" +#include "BulletCollision/Gimpact/btTriangleShapeEx.cpp" +#include "BulletCollision/Gimpact/gim_memory.cpp" +#include "BulletCollision/Gimpact/btGImpactBvh.cpp" +#include "BulletCollision/Gimpact/btGImpactShape.cpp" +#include "BulletCollision/Gimpact/gim_box_set.cpp" +#include "BulletCollision/Gimpact/gim_tri_collision.cpp" +#include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp" +#include "BulletCollision/Gimpact/btGenericPoolAllocator.cpp" +#include "BulletCollision/Gimpact/gim_contact.cpp" diff --git a/Engine/lib/bullet/src/btBulletCollisionCommon.h b/Engine/lib/bullet/src/btBulletCollisionCommon.h index 948e02eb4..4f523756a 100644 --- a/Engine/lib/bullet/src/btBulletCollisionCommon.h +++ b/Engine/lib/bullet/src/btBulletCollisionCommon.h @@ -62,6 +62,4 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btSerializer.h" - -#endif //BULLET_COLLISION_COMMON_H - +#endif //BULLET_COLLISION_COMMON_H diff --git a/Engine/lib/bullet/src/btBulletDynamicsAll.cpp b/Engine/lib/bullet/src/btBulletDynamicsAll.cpp new file mode 100644 index 000000000..a8069e30a --- /dev/null +++ b/Engine/lib/bullet/src/btBulletDynamicsAll.cpp @@ -0,0 +1,42 @@ +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp" +#include "BulletDynamics/Dynamics/btRigidBody.cpp" +#include "BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp" +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp" +#include "BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp" +#include "BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp" +#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btSliderConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btContactConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp" +#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btFixedConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btHingeConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btGearConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp" +#include "BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp" +#include "BulletDynamics/MLCPSolvers/btDantzigLCP.cpp" +#include "BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp" +#include "BulletDynamics/MLCPSolvers/btMLCPSolver.cpp" +#include "BulletDynamics/Featherstone/btMultiBody.cpp" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp" +#include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.cpp" +#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp" +#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp" +#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp" +#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp" +#include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp" +#include "BulletDynamics/Vehicle/btRaycastVehicle.cpp" +#include "BulletDynamics/Vehicle/btWheelInfo.cpp" +#include "BulletDynamics/Character/btKinematicCharacterController.cpp" + diff --git a/Engine/lib/bullet/src/btBulletDynamicsCommon.h b/Engine/lib/bullet/src/btBulletDynamicsCommon.h index 50282bf21..a421fa446 100644 --- a/Engine/lib/bullet/src/btBulletDynamicsCommon.h +++ b/Engine/lib/bullet/src/btBulletDynamicsCommon.h @@ -35,17 +35,9 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btGearConstraint.h" #include "BulletDynamics/ConstraintSolver/btFixedConstraint.h" - #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" - ///Vehicle simulation, with wheel contact simulated by raycasts #include "BulletDynamics/Vehicle/btRaycastVehicle.h" - - - - - -#endif //BULLET_DYNAMICS_COMMON_H - +#endif //BULLET_DYNAMICS_COMMON_H diff --git a/Engine/lib/bullet/src/btLinearMathAll.cpp b/Engine/lib/bullet/src/btLinearMathAll.cpp new file mode 100644 index 000000000..808f41280 --- /dev/null +++ b/Engine/lib/bullet/src/btLinearMathAll.cpp @@ -0,0 +1,14 @@ +#include "LinearMath/btAlignedAllocator.cpp" +#include "LinearMath/btGeometryUtil.cpp" +#include "LinearMath/btSerializer.cpp" +#include "LinearMath/btVector3.cpp" +#include "LinearMath/btConvexHull.cpp" +#include "LinearMath/btPolarDecomposition.cpp" +#include "LinearMath/btSerializer64.cpp" +#include "LinearMath/btConvexHullComputer.cpp" +#include "LinearMath/btQuickprof.cpp" +#include "LinearMath/btThreads.cpp" +#include "LinearMath/TaskScheduler/btTaskScheduler.cpp" +#include "LinearMath/TaskScheduler/btThreadSupportPosix.cpp" +#include "LinearMath/TaskScheduler/btThreadSupportWin32.cpp" + diff --git a/Engine/lib/bullet/src/clew/clew.c b/Engine/lib/bullet/src/clew/clew.c index a07b0aad7..90caced53 100644 --- a/Engine/lib/bullet/src/clew/clew.c +++ b/Engine/lib/bullet/src/clew/clew.c @@ -9,23 +9,23 @@ #include "clew.h" #ifdef _WIN32 - #define WIN32_LEAN_AND_MEAN - #define VC_EXTRALEAN - #include +#define WIN32_LEAN_AND_MEAN +#define VC_EXTRALEAN +#include - typedef HMODULE CLEW_DYNLIB_HANDLE; +typedef HMODULE CLEW_DYNLIB_HANDLE; - #define CLEW_DYNLIB_OPEN LoadLibrary - #define CLEW_DYNLIB_CLOSE FreeLibrary - #define CLEW_DYNLIB_IMPORT GetProcAddress +#define CLEW_DYNLIB_OPEN LoadLibraryA +#define CLEW_DYNLIB_CLOSE FreeLibrary +#define CLEW_DYNLIB_IMPORT GetProcAddress #else - #include - - typedef void* CLEW_DYNLIB_HANDLE; +#include - #define CLEW_DYNLIB_OPEN(path) dlopen(path, RTLD_NOW | RTLD_GLOBAL) - #define CLEW_DYNLIB_CLOSE dlclose - #define CLEW_DYNLIB_IMPORT dlsym +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 @@ -34,279 +34,341 @@ 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; +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; +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; +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; - +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; - } + if (module != NULL) + { + // Ignore errors + CLEW_DYNLIB_CLOSE(module); + module = NULL; + } } int clewInit(const char* path) { - int error = 0; + int error = 0; - // Check if already initialized - if (module != NULL) - { - return CLEW_SUCCESS; - } + // Check if already initialized + if (module != NULL) + { + return CLEW_SUCCESS; + } - // Load library - module = CLEW_DYNLIB_OPEN(path); + // Load library + module = CLEW_DYNLIB_OPEN(path); - // Check for errors - if (module == NULL) - { - return CLEW_ERROR_OPEN_FAILED; - } + // Check for errors + if (module == NULL) + { + return CLEW_ERROR_OPEN_FAILED; + } - // Set unloading - error = atexit(clewExit); + // Set unloading + error = atexit(clewExit); - if (error) - { - // Failure queuing atexit, shutdown with error - CLEW_DYNLIB_CLOSE(module); - module = NULL; + if (error) + { + // Failure queuing atexit, shutdown with error + CLEW_DYNLIB_CLOSE(module); + module = NULL; - return CLEW_ERROR_ATEXIT_FAILED; - } + 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"); + // 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"); + __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"); + __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; + 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 + 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 + , + "" // -13 + , + "" // -14 + , + "" // -15 + , + "" // -16 + , + "" // -17 + , + "" // -18 + , + "" // -19 - , "" // -20 - , "" // -21 - , "" // -22 - , "" // -23 - , "" // -24 - , "" // -25 - , "" // -26 - , "" // -27 - , "" // -28 - , "" // -29 + , + "" // -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 - }; + , + "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]; + return strings[-error]; } diff --git a/Engine/lib/bullet/src/clew/clew.h b/Engine/lib/bullet/src/clew/clew.h index ee0fef18b..cba858523 100644 --- a/Engine/lib/bullet/src/clew/clew.h +++ b/Engine/lib/bullet/src/clew/clew.h @@ -11,7 +11,7 @@ //! \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 +//! 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. @@ -19,11 +19,11 @@ //! library (http://glew.sourceforge.net/) // Run-time dynamic linking functionality based on concepts used in GLEW -#ifdef __OPENCL_CL_H +#ifdef __OPENCL_CL_H #error cl.h included before clew.h #endif -#ifdef __OPENCL_CL_PLATFORM_H +#ifdef __OPENCL_CL_PLATFORM_H #error cl_platform.h included before clew.h #endif @@ -55,238 +55,239 @@ * 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 +/* Contains #defines for AVAILABLE_MAC_OS_X_VERSION_10_6_AND_LATER below */ +#include #endif #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif #if defined(_WIN32) - #define CL_API_ENTRY - #define CL_API_CALL __stdcall - #define CL_CALLBACK __stdcall +#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 +#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 + //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 +#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 +#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)) +#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; + /* 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; + 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_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_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_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 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_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 +#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 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))); +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_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_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_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 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_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( "" ) +#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( "" ) +#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 +#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; + /* 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. @@ -302,7 +303,6 @@ typedef unsigned int cl_GLenum; * Maintaining proper alignment is the user's responsibility. */ - #ifdef _MSC_VER #if defined(_M_IX86) #if _M_IX86_FP >= 0 @@ -318,904 +318,1218 @@ typedef unsigned int cl_GLenum; #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 +#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 +#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 +#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 +#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 +#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) +#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) +#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 +#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 +#define CL_NAMED_STRUCT_SUPPORTED #endif -#if defined( CL_NAMED_STRUCT_SUPPORTED) && defined( _MSC_VER ) -#define __extension__ __pragma(warning(suppress:4201)) +#if defined(CL_NAMED_STRUCT_SUPPORTED) && defined(_MSC_VER) +#define __extension__ __pragma(warning(suppress : 4201)) #endif -/* Define cl_vector types */ + /* 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; }; + /* ---- 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; +#if defined(__CL_CHAR2__) + __cl_char2 v2; #endif -}cl_char2; + } 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; }; + 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]; +#if defined(__CL_CHAR2__) + __cl_char2 v2[2]; #endif -#if defined( __CL_CHAR4__) - __cl_char4 v4; +#if defined(__CL_CHAR4__) + __cl_char4 v4; #endif -}cl_char4; + } cl_char4; -/* cl_char3 is identical in size, alignment and behavior to cl_char4. See section 6.1.5. */ -typedef cl_char4 cl_char3; + /* 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; }; + 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]; +#if defined(__CL_CHAR2__) + __cl_char2 v2[4]; #endif -#if defined( __CL_CHAR4__) - __cl_char4 v4[2]; +#if defined(__CL_CHAR4__) + __cl_char4 v4[2]; #endif -#if defined( __CL_CHAR8__ ) - __cl_char8 v8; +#if defined(__CL_CHAR8__) + __cl_char8 v8; #endif -}cl_char8; + } 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; }; + 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]; +#if defined(__CL_CHAR2__) + __cl_char2 v2[8]; #endif -#if defined( __CL_CHAR4__) - __cl_char4 v4[4]; +#if defined(__CL_CHAR4__) + __cl_char4 v4[4]; #endif -#if defined( __CL_CHAR8__ ) - __cl_char8 v8[2]; +#if defined(__CL_CHAR8__) + __cl_char8 v8[2]; #endif -#if defined( __CL_CHAR16__ ) - __cl_char16 v16; +#if defined(__CL_CHAR16__) + __cl_char16 v16; #endif -}cl_char16; + } 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; -/* ---- 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; }; + 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; +#if defined(__CL_UCHAR2__) + __cl_uchar2 v2[2]; #endif -}cl_uchar2; +#if defined(__CL_UCHAR4__) + __cl_uchar4 v4; +#endif + } cl_uchar4; -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; -/* 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(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; }; + 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[4]; +#if defined(__CL_UCHAR2__) + __cl_uchar2 v2[8]; #endif -#if defined( __CL_UCHAR4__) - __cl_uchar4 v4[2]; +#if defined(__CL_UCHAR4__) + __cl_uchar4 v4[4]; #endif -#if defined( __CL_UCHAR8__ ) - __cl_uchar8 v8; +#if defined(__CL_UCHAR8__) + __cl_uchar8 v8[2]; #endif -}cl_uchar8; +#if defined(__CL_UCHAR16__) + __cl_uchar16 v16; +#endif + } cl_uchar16; -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; }; + /* ---- 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_UCHAR2__) - __cl_uchar2 v2[8]; +#if defined(__CL_SHORT2__) + __cl_short2 v2; #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_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_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; + /* 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(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; }; + 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[2]; +#if defined(__CL_SHORT2__) + __cl_short2 v2[4]; #endif -#if defined( __CL_SHORT4__) - __cl_short4 v4; +#if defined(__CL_SHORT4__) + __cl_short4 v4[2]; #endif -}cl_short4; +#if defined(__CL_SHORT8__) + __cl_short8 v8; +#endif + } cl_short8; -/* 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(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; -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; }; + /* ---- 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_SHORT2__) - __cl_short2 v2[4]; +#if defined(__CL_USHORT2__) + __cl_ushort2 v2; #endif -#if defined( __CL_SHORT4__) - __cl_short4 v4[2]; -#endif -#if defined( __CL_SHORT8__ ) - __cl_short8 v8; -#endif -}cl_short8; + } cl_ushort2; -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; }; + 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_SHORT2__) - __cl_short2 v2[8]; +#if defined(__CL_USHORT2__) + __cl_ushort2 v2[2]; #endif -#if defined( __CL_SHORT4__) - __cl_short4 v4[4]; +#if defined(__CL_USHORT4__) + __cl_ushort4 v4; #endif -#if defined( __CL_SHORT8__ ) - __cl_short8 v8[2]; -#endif -#if defined( __CL_SHORT16__ ) - __cl_short16 v16; -#endif -}cl_short16; + } cl_ushort4; + /* cl_ushort3 is identical in size, alignment and behavior to cl_ushort4. See section 6.1.5. */ + typedef cl_ushort4 cl_ushort3; -/* ---- 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; }; + 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; +#if defined(__CL_USHORT2__) + __cl_ushort2 v2[4]; #endif -}cl_ushort2; +#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(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; }; + 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[2]; +#if defined(__CL_USHORT2__) + __cl_ushort2 v2[8]; #endif -#if defined( __CL_USHORT4__) - __cl_ushort4 v4; +#if defined(__CL_USHORT4__) + __cl_ushort4 v4[4]; #endif -}cl_ushort4; +#if defined(__CL_USHORT8__) + __cl_ushort8 v8[2]; +#endif +#if defined(__CL_USHORT16__) + __cl_ushort16 v16; +#endif + } cl_ushort16; -/* cl_ushort3 is identical in size, alignment and behavior to cl_ushort4. See section 6.1.5. */ -typedef cl_ushort4 cl_ushort3; + /* ---- 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_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; }; + 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_USHORT2__) - __cl_ushort2 v2[4]; +#if defined(__CL_INT2__) + __cl_int2 v2[2]; #endif -#if defined( __CL_USHORT4__) - __cl_ushort4 v4[2]; +#if defined(__CL_INT4__) + __cl_int4 v4; #endif -#if defined( __CL_USHORT8__ ) - __cl_ushort8 v8; -#endif -}cl_ushort8; + } cl_int4; -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_int3 is identical in size, alignment and behavior to cl_int4. See section 6.1.5. */ + typedef cl_int4 cl_int3; -/* ---- 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; }; + 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; +#if defined(__CL_INT2__) + __cl_int2 v2[4]; #endif -}cl_int2; +#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(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; }; + 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[2]; +#if defined(__CL_INT2__) + __cl_int2 v2[8]; #endif -#if defined( __CL_INT4__) - __cl_int4 v4; +#if defined(__CL_INT4__) + __cl_int4 v4[4]; #endif -}cl_int4; +#if defined(__CL_INT8__) + __cl_int8 v8[2]; +#endif +#if defined(__CL_INT16__) + __cl_int16 v16; +#endif + } cl_int16; -/* cl_int3 is identical in size, alignment and behavior to cl_int4. See section 6.1.5. */ -typedef cl_int4 cl_int3; + /* ---- 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_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; }; + 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_INT2__) - __cl_int2 v2[4]; +#if defined(__CL_UINT2__) + __cl_uint2 v2[2]; #endif -#if defined( __CL_INT4__) - __cl_int4 v4[2]; +#if defined(__CL_UINT4__) + __cl_uint4 v4; #endif -#if defined( __CL_INT8__ ) - __cl_int8 v8; -#endif -}cl_int8; + } cl_uint4; -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_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; -/* ---- 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; }; + 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; +#if defined(__CL_UINT2__) + __cl_uint2 v2[8]; #endif -}cl_uint2; +#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; -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; }; + /* ---- 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_UINT2__) - __cl_uint2 v2[2]; +#if defined(__CL_LONG2__) + __cl_long2 v2; #endif -#if defined( __CL_UINT4__) - __cl_uint4 v4; -#endif -}cl_uint4; + } cl_long2; -/* 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_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; -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; + /* 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_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; }; + 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_UINT2__) - __cl_uint2 v2[8]; +#if defined(__CL_LONG2__) + __cl_long2 v2[4]; #endif -#if defined( __CL_UINT4__) - __cl_uint4 v4[4]; +#if defined(__CL_LONG4__) + __cl_long4 v4[2]; #endif -#if defined( __CL_UINT8__ ) - __cl_uint8 v8[2]; +#if defined(__CL_LONG8__) + __cl_long8 v8; #endif -#if defined( __CL_UINT16__ ) - __cl_uint16 v16; -#endif -}cl_uint16; + } cl_long8; -/* ---- 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; }; + 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; +#if defined(__CL_LONG2__) + __cl_long2 v2[8]; #endif -}cl_long2; +#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; -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; }; + /* ---- 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_LONG2__) - __cl_long2 v2[2]; +#if defined(__CL_ULONG2__) + __cl_ulong2 v2; #endif -#if defined( __CL_LONG4__) - __cl_long4 v4; -#endif -}cl_long4; + } cl_ulong2; -/* 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_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; -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; + /* 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_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; }; + 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_LONG2__) - __cl_long2 v2[8]; +#if defined(__CL_ULONG2__) + __cl_ulong2 v2[4]; #endif -#if defined( __CL_LONG4__) - __cl_long4 v4[4]; +#if defined(__CL_ULONG4__) + __cl_ulong4 v4[2]; #endif -#if defined( __CL_LONG8__ ) - __cl_long8 v8[2]; +#if defined(__CL_ULONG8__) + __cl_ulong8 v8; #endif -#if defined( __CL_LONG16__ ) - __cl_long16 v16; -#endif -}cl_long16; + } 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_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; + /* --- cl_floatn ---- */ -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; }; + 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_ULONG2__) - __cl_ulong2 v2[2]; +#if defined(__CL_FLOAT2__) + __cl_float2 v2; #endif -#if defined( __CL_ULONG4__) - __cl_ulong4 v4; -#endif -}cl_ulong4; + } cl_float2; -/* 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_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; -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; + /* 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_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; }; + 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_ULONG2__) - __cl_ulong2 v2[8]; +#if defined(__CL_FLOAT2__) + __cl_float2 v2[4]; #endif -#if defined( __CL_ULONG4__) - __cl_ulong4 v4[4]; +#if defined(__CL_FLOAT4__) + __cl_float4 v4[2]; #endif -#if defined( __CL_ULONG8__ ) - __cl_ulong8 v8[2]; +#if defined(__CL_FLOAT8__) + __cl_float8 v8; #endif -#if defined( __CL_ULONG16__ ) - __cl_ulong16 v16; -#endif -}cl_ulong16; + } 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_floatn ---- */ + /* --- cl_doublen ---- */ -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; }; + 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_FLOAT2__) - __cl_float2 v2; +#if defined(__CL_DOUBLE2__) + __cl_double2 v2; #endif -}cl_float2; + } cl_double2; -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; }; + 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_FLOAT2__) - __cl_float2 v2[2]; +#if defined(__CL_DOUBLE2__) + __cl_double2 v2[2]; #endif -#if defined( __CL_FLOAT4__) - __cl_float4 v4; +#if defined(__CL_DOUBLE4__) + __cl_double4 v4; #endif -}cl_float4; + } cl_double4; -/* cl_float3 is identical in size, alignment and behavior to cl_float4. See section 6.1.5. */ -typedef cl_float4 cl_float3; + /* 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_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; }; + 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_FLOAT2__) - __cl_float2 v2[4]; +#if defined(__CL_DOUBLE2__) + __cl_double2 v2[4]; #endif -#if defined( __CL_FLOAT4__) - __cl_float4 v4[2]; +#if defined(__CL_DOUBLE4__) + __cl_double4 v4[2]; #endif -#if defined( __CL_FLOAT8__ ) - __cl_float8 v8; +#if defined(__CL_DOUBLE8__) + __cl_double8 v8; #endif -}cl_float8; + } cl_double8; -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; }; + 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_FLOAT2__) - __cl_float2 v2[8]; +#if defined(__CL_DOUBLE2__) + __cl_double2 v2[8]; #endif -#if defined( __CL_FLOAT4__) - __cl_float4 v4[4]; +#if defined(__CL_DOUBLE4__) + __cl_double4 v4[4]; #endif -#if defined( __CL_FLOAT8__ ) - __cl_float8 v8[2]; +#if defined(__CL_DOUBLE8__) + __cl_double8 v8[2]; #endif -#if defined( __CL_FLOAT16__ ) - __cl_float16 v16; +#if defined(__CL_DOUBLE16__) + __cl_double16 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; + } cl_double16; /* Macro to facilitate debugging * Usage: @@ -1237,967 +1551,967 @@ typedef union * 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" +#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 -/******************************************************************************/ + // 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 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 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 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_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; + 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_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 +#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 +#define CL_VERSION_1_0 1 +#define CL_VERSION_1_1 1 /* cl_bool */ -#define CL_FALSE 0 -#define CL_TRUE 1 +#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 +#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 +#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 +#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 +#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) +#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 +#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 +#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) +#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) +#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 +#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 +#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 +#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) +#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 +#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 +#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 +#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 +#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 +#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 +#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 +#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 +#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) +#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 +#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 +#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 +#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 +#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_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 +#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 +#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 +#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 - +#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 +#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 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 +#define CLEWAPI extern #else -# ifdef CLEW_BUILD -# define CLEWAPI extern __declspec(dllexport) -# else -# define CLEWAPI extern __declspec(dllimport) -# endif +#ifdef CLEW_BUILD +#define CLEWAPI extern __declspec(dllexport) +#else +#define CLEWAPI extern __declspec(dllimport) +#endif #endif #if defined(_WIN32) @@ -2208,99 +2522,97 @@ typedef CL_API_ENTRY void * (CL_API_CALL * PFNCLGETEXTENSIONFUNCTIONADDRESS)(con #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 ; + // 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 ; + 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 ; + 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 ) +#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! /* @@ -2313,82 +2625,81 @@ CLEW_FUN_EXPORT PFNCLGETEXTENSIONFUNCTIONADDRESS __clewGetExtensionFuncti * 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 ) +#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 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() -#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 Load OpenCL dynamic library and set function entry points -int clewInit (const char*); + //! \brief Exit clew and unload OpenCL dynamic library + void clewExit(); -//! \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); + //! \brief Convert an OpenCL error code to its string equivalent + const char *clewErrorString(cl_int error); #ifdef __cplusplus } diff --git a/Engine/lib/openal-soft/Alc/ALc.c b/Engine/lib/openal-soft/Alc/ALc.c index 597cc890c..8bf2e1da7 100644 --- a/Engine/lib/openal-soft/Alc/ALc.c +++ b/Engine/lib/openal-soft/Alc/ALc.c @@ -75,14 +75,14 @@ static struct BackendInfo BackendList[] = { #ifdef HAVE_COREAUDIO { "core", ALCcoreAudioBackendFactory_getFactory }, #endif -#ifdef HAVE_OSS - { "oss", ALCossBackendFactory_getFactory }, -#endif #ifdef HAVE_SOLARIS { "solaris", ALCsolarisBackendFactory_getFactory }, #endif #ifdef HAVE_SNDIO - { "sndio", ALCsndioBackendFactory_getFactory }, + { "sndio", SndioBackendFactory_getFactory }, +#endif +#ifdef HAVE_OSS + { "oss", ALCossBackendFactory_getFactory }, #endif #ifdef HAVE_QSA { "qsa", ALCqsaBackendFactory_getFactory }, @@ -550,14 +550,12 @@ static const struct { DECL(AL_EFFECT_ECHO), DECL(AL_EFFECT_FLANGER), DECL(AL_EFFECT_PITCH_SHIFTER), -#if 0 DECL(AL_EFFECT_FREQUENCY_SHIFTER), +#if 0 DECL(AL_EFFECT_VOCAL_MORPHER), #endif DECL(AL_EFFECT_RING_MODULATOR), -#if 0 DECL(AL_EFFECT_AUTOWAH), -#endif DECL(AL_EFFECT_COMPRESSOR), DECL(AL_EFFECT_EQUALIZER), DECL(AL_EFFECT_DEDICATED_LOW_FREQUENCY_EFFECT), @@ -632,6 +630,10 @@ static const struct { DECL(AL_FLANGER_FEEDBACK), DECL(AL_FLANGER_DELAY), + DECL(AL_FREQUENCY_SHIFTER_FREQUENCY), + DECL(AL_FREQUENCY_SHIFTER_LEFT_DIRECTION), + DECL(AL_FREQUENCY_SHIFTER_RIGHT_DIRECTION), + DECL(AL_RING_MODULATOR_FREQUENCY), DECL(AL_RING_MODULATOR_HIGHPASS_CUTOFF), DECL(AL_RING_MODULATOR_WAVEFORM), @@ -654,6 +656,11 @@ static const struct { DECL(AL_DEDICATED_GAIN), + DECL(AL_AUTOWAH_ATTACK_TIME), + DECL(AL_AUTOWAH_RELEASE_TIME), + DECL(AL_AUTOWAH_RESONANCE), + DECL(AL_AUTOWAH_PEAK_GAIN), + DECL(AL_NUM_RESAMPLERS_SOFT), DECL(AL_DEFAULT_RESAMPLER_SOFT), DECL(AL_SOURCE_RESAMPLER_SOFT), @@ -721,6 +728,7 @@ static const ALchar alExtList[] = "AL_SOFT_deferred_updates " "AL_SOFT_direct_channels " "AL_SOFTX_events " + "AL_SOFTX_filter_gain_ex " "AL_SOFT_gain_clamp_ex " "AL_SOFT_loop_points " "AL_SOFTX_map_buffer " @@ -1157,75 +1165,6 @@ static void alc_initconfig(void) } #define DO_INITCONFIG() alcall_once(&alc_config_once, alc_initconfig) -#ifdef __ANDROID__ -#include - -static JavaVM *gJavaVM; -static pthread_key_t gJVMThreadKey; - -static void CleanupJNIEnv(void* UNUSED(ptr)) -{ - JCALL0(gJavaVM,DetachCurrentThread)(); -} - -void *Android_GetJNIEnv(void) -{ - if(!gJavaVM) - { - WARN("gJavaVM is NULL!\n"); - return NULL; - } - - /* http://developer.android.com/guide/practices/jni.html - * - * All threads are Linux threads, scheduled by the kernel. They're usually - * started from managed code (using Thread.start), but they can also be - * created elsewhere and then attached to the JavaVM. For example, a thread - * started with pthread_create can be attached with the JNI - * AttachCurrentThread or AttachCurrentThreadAsDaemon functions. Until a - * thread is attached, it has no JNIEnv, and cannot make JNI calls. - * Attaching a natively-created thread causes a java.lang.Thread object to - * be constructed and added to the "main" ThreadGroup, making it visible to - * the debugger. Calling AttachCurrentThread on an already-attached thread - * is a no-op. - */ - JNIEnv *env = pthread_getspecific(gJVMThreadKey); - if(!env) - { - int status = JCALL(gJavaVM,AttachCurrentThread)(&env, NULL); - if(status < 0) - { - ERR("Failed to attach current thread\n"); - return NULL; - } - pthread_setspecific(gJVMThreadKey, env); - } - return env; -} - -/* Automatically called by JNI. */ -JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *jvm, void* UNUSED(reserved)) -{ - void *env; - int err; - - gJavaVM = jvm; - if(JCALL(gJavaVM,GetEnv)(&env, JNI_VERSION_1_4) != JNI_OK) - { - ERR("Failed to get JNIEnv with JNI_VERSION_1_4\n"); - return JNI_ERR; - } - - /* Create gJVMThreadKey so we can keep track of the JNIEnv assigned to each - * thread. The JNIEnv *must* be detached before the thread is destroyed. - */ - if((err=pthread_key_create(&gJVMThreadKey, CleanupJNIEnv)) != 0) - ERR("pthread_key_create failed: %d\n", err); - pthread_setspecific(gJVMThreadKey, env); - return JNI_VERSION_1_4; -} -#endif - /************************************************ * Library deinitialization @@ -1306,7 +1245,7 @@ static void ProbeDevices(al_string *list, struct BackendInfo *backendinfo, enum if(backendinfo->getFactory) { ALCbackendFactory *factory = backendinfo->getFactory(); - V(factory,probe)(type); + V(factory,probe)(type, list); } UnlockLists(); @@ -1316,17 +1255,6 @@ static void ProbeAllDevicesList(void) static void ProbeCaptureDeviceList(void) { ProbeDevices(&alcCaptureDeviceList, &CaptureBackend, CAPTURE_DEVICE_PROBE); } -static void AppendDevice(const ALCchar *name, al_string *devnames) -{ - size_t len = strlen(name); - if(len > 0) - alstr_append_range(devnames, name, name+len+1); -} -void AppendAllDevicesList(const ALCchar *name) -{ AppendDevice(name, &alcAllDevicesList); } -void AppendCaptureDeviceList(const ALCchar *name) -{ AppendDevice(name, &alcCaptureDeviceList); } - /************************************************ * Device format information @@ -1707,10 +1635,11 @@ static void alcSetError(ALCdevice *device, ALCenum errorCode) } -struct Compressor *CreateDeviceLimiter(const ALCdevice *device) +static struct Compressor *CreateDeviceLimiter(const ALCdevice *device, const ALfloat threshold) { - return CompressorInit(0.0f, 0.0f, AL_FALSE, AL_TRUE, 0.0f, 0.0f, 0.5f, 2.0f, - 0.0f, -3.0f, 3.0f, device->Frequency); + return CompressorInit(device->RealOut.NumChannels, device->Frequency, + AL_TRUE, AL_TRUE, AL_TRUE, AL_TRUE, AL_TRUE, 0.001f, 0.002f, + 0.0f, 0.0f, threshold, INFINITY, 0.0f, 0.020f, 0.200f); } /* UpdateClockBase @@ -1737,7 +1666,7 @@ static ALCenum UpdateDeviceParams(ALCdevice *device, const ALCint *attrList) { enum HrtfRequestMode hrtf_userreq = Hrtf_Default; enum HrtfRequestMode hrtf_appreq = Hrtf_Default; - ALCenum gainLimiter = device->Limiter ? ALC_TRUE : ALC_FALSE; + ALCenum gainLimiter = device->LimiterState; const ALsizei old_sends = device->NumAuxSends; ALsizei new_sends = device->NumAuxSends; enum DevFmtChannels oldChans; @@ -2053,6 +1982,7 @@ static ALCenum UpdateDeviceParams(ALCdevice *device, const ALCint *attrList) device->RealOut.NumChannels = 0; UpdateClockBase(device); + device->FixedLatency = 0; device->DitherSeed = DITHER_RNG_SEED; @@ -2213,9 +2143,12 @@ static ALCenum UpdateDeviceParams(ALCdevice *device, const ALCint *attrList) break; } } - else if(depth > 24) - depth = 24; - device->DitherDepth = (depth > 0) ? powf(2.0f, (ALfloat)(depth-1)) : 0.0f; + + if(depth > 0) + { + depth = clampi(depth, 2, 24); + device->DitherDepth = powf(2.0f, (ALfloat)(depth-1)); + } } if(!(device->DitherDepth > 0.0f)) TRACE("Dithering disabled\n"); @@ -2223,19 +2156,57 @@ static ALCenum UpdateDeviceParams(ALCdevice *device, const ALCint *attrList) TRACE("Dithering enabled (%g-bit, %g)\n", log2f(device->DitherDepth)+1.0f, device->DitherDepth); + device->LimiterState = gainLimiter; if(ConfigValueBool(alstr_get_cstr(device->DeviceName), NULL, "output-limiter", &val)) gainLimiter = val ? ALC_TRUE : ALC_FALSE; + /* Valid values for gainLimiter are ALC_DONT_CARE_SOFT, ALC_TRUE, and - * ALC_FALSE. We default to on, so ALC_DONT_CARE_SOFT is the same as - * ALC_TRUE. + * ALC_FALSE. For ALC_DONT_CARE_SOFT, use the limiter for integer-based + * output (where samples must be clamped), and don't for floating-point + * (which can take unclamped samples). */ + if(gainLimiter == ALC_DONT_CARE_SOFT) + { + switch(device->FmtType) + { + case DevFmtByte: + case DevFmtUByte: + case DevFmtShort: + case DevFmtUShort: + case DevFmtInt: + case DevFmtUInt: + gainLimiter = ALC_TRUE; + break; + case DevFmtFloat: + gainLimiter = ALC_FALSE; + break; + } + } if(gainLimiter != ALC_FALSE) { - if(!device->Limiter || device->Frequency != GetCompressorSampleRate(device->Limiter)) + ALfloat thrshld = 1.0f; + switch(device->FmtType) { - al_free(device->Limiter); - device->Limiter = CreateDeviceLimiter(device); + case DevFmtByte: + case DevFmtUByte: + thrshld = 127.0f / 128.0f; + break; + case DevFmtShort: + case DevFmtUShort: + thrshld = 32767.0f / 32768.0f; + break; + case DevFmtInt: + case DevFmtUInt: + case DevFmtFloat: + break; } + if(device->DitherDepth > 0.0f) + thrshld -= 1.0f / device->DitherDepth; + + al_free(device->Limiter); + device->Limiter = CreateDeviceLimiter(device, log10f(thrshld) * 20.0f); + device->FixedLatency += (ALuint)(GetCompressorLookAhead(device->Limiter) * + DEVICE_CLOCK_RES / device->Frequency); } else { @@ -2246,6 +2217,8 @@ static ALCenum UpdateDeviceParams(ALCdevice *device, const ALCint *attrList) aluSelectPostProcess(device); + TRACE("Fixed device latency: %uns\n", device->FixedLatency); + /* Need to delay returning failure until replacement Send arrays have been * allocated with the appropriate size. */ @@ -2400,11 +2373,13 @@ static void InitDevice(ALCdevice *device, enum DeviceType type) device->Flags = 0; device->Render_Mode = NormalRender; device->AvgSpeakerDist = 0.0f; + device->LimiterState = ALC_DONT_CARE_SOFT; ATOMIC_INIT(&device->ContextList, NULL); device->ClockBase = 0; device->SamplesDone = 0; + device->FixedLatency = 0; device->SourcesMax = 0; device->AuxiliaryEffectSlotMax = 0; @@ -2638,7 +2613,6 @@ static ALvoid InitContext(ALCcontext *Context) Context->MetersPerUnit = AL_DEFAULT_METERS_PER_UNIT; ATOMIC_FLAG_TEST_AND_SET(&Context->PropsClean, almemory_order_relaxed); ATOMIC_INIT(&Context->DeferUpdates, AL_FALSE); - almtx_init(&Context->EventThrdLock, almtx_plain); alsem_init(&Context->EventSem, 0); Context->AsyncEvents = NULL; ATOMIC_INIT(&Context->EnabledEvts, 0); @@ -2665,6 +2639,11 @@ static ALvoid InitContext(ALCcontext *Context) listener->Params.MetersPerUnit; listener->Params.SourceDistanceModel = Context->SourceDistanceModel; listener->Params.DistanceModel = Context->DistanceModel; + + + Context->AsyncEvents = ll_ringbuffer_create(63, sizeof(AsyncEvent), false); + if(althrd_create(&Context->EventThread, EventThread, Context) != althrd_success) + ERR("Failed to start event thread! Expect problems.\n"); } @@ -2773,17 +2752,7 @@ static void FreeContext(ALCcontext *context) } TRACE("Freed "SZFMT" listener property object%s\n", count, (count==1)?"":"s"); - if(ATOMIC_EXCHANGE(&context->EnabledEvts, 0, almemory_order_acq_rel)) - { - static const AsyncEvent kill_evt = { 0 }; - while(ll_ringbuffer_write(context->AsyncEvents, (const char*)&kill_evt, 1) == 0) - althrd_yield(); - alsem_post(&context->EventSem); - althrd_join(context->EventThread, NULL); - } - almtx_destroy(&context->EventCbLock); - almtx_destroy(&context->EventThrdLock); alsem_destroy(&context->EventSem); ll_ringbuffer_free(context->AsyncEvents); @@ -2807,6 +2776,7 @@ static void FreeContext(ALCcontext *context) */ static bool ReleaseContext(ALCcontext *context, ALCdevice *device) { + static const AsyncEvent kill_evt = ASYNC_EVENT(EventType_KillThread); ALCcontext *origctx, *newhead; bool ret = true; @@ -2839,6 +2809,16 @@ static bool ReleaseContext(ALCcontext *context, ALCdevice *device) ret = !!newhead; V0(device->Backend,unlock)(); + /* Make sure the context is finished and no longer processing in the mixer + * before sending the message queue kill event. The backend's lock does + * this, although waiting for a non-odd mix count would work too. + */ + + while(ll_ringbuffer_write(context->AsyncEvents, (const char*)&kill_evt, 1) == 0) + althrd_yield(); + alsem_post(&context->EventSem); + althrd_join(context->EventThread, NULL); + ALCcontext_DecRef(context); return ret; } @@ -3620,7 +3600,7 @@ ALC_API void ALC_APIENTRY alcGetInteger64vSOFT(ALCdevice *device, ALCenum pname, values[i++] = ALC_OUTPUT_LIMITER_SOFT; values[i++] = device->Limiter ? ALC_TRUE : ALC_FALSE; - clock = V0(device->Backend,getClockLatency)(); + clock = GetClockLatency(device); values[i++] = ALC_DEVICE_CLOCK_SOFT; values[i++] = clock.ClockTime; @@ -3646,7 +3626,7 @@ ALC_API void ALC_APIENTRY alcGetInteger64vSOFT(ALCdevice *device, ALCenum pname, case ALC_DEVICE_LATENCY_SOFT: almtx_lock(&device->BackendLock); - clock = V0(device->Backend,getClockLatency)(); + clock = GetClockLatency(device); almtx_unlock(&device->BackendLock); *values = clock.Latency; break; @@ -3657,7 +3637,7 @@ ALC_API void ALC_APIENTRY alcGetInteger64vSOFT(ALCdevice *device, ALCenum pname, else { almtx_lock(&device->BackendLock); - clock = V0(device->Backend,getClockLatency)(); + clock = GetClockLatency(device); almtx_unlock(&device->BackendLock); values[0] = clock.ClockTime; values[1] = clock.Latency; @@ -4073,6 +4053,7 @@ ALC_API ALCdevice* ALC_APIENTRY alcOpenDevice(const ALCchar *deviceName) device->IsHeadphones = AL_FALSE; device->AmbiLayout = AmbiLayout_Default; device->AmbiScale = AmbiNorm_Default; + device->LimiterState = ALC_TRUE; device->NumUpdates = 3; device->UpdateSize = 1024; @@ -4211,8 +4192,6 @@ ALC_API ALCdevice* ALC_APIENTRY alcOpenDevice(const ALCchar *deviceName) ERR("Unsupported ambi-format: %s\n", fmt); } - device->Limiter = CreateDeviceLimiter(device); - { ALCdevice *head = ATOMIC_LOAD_SEQ(&DeviceList); do { @@ -4394,6 +4373,12 @@ ALC_API ALCboolean ALC_APIENTRY alcCaptureCloseDevice(ALCdevice *device) } UnlockLists(); + almtx_lock(&device->BackendLock); + if((device->Flags&DEVICE_RUNNING)) + V0(device->Backend,stop)(); + device->Flags &= ~DEVICE_RUNNING; + almtx_unlock(&device->BackendLock); + ALCdevice_DecRef(device); return ALC_TRUE; @@ -4534,8 +4519,6 @@ ALC_API ALCdevice* ALC_APIENTRY alcLoopbackOpenDeviceSOFT(const ALCchar *deviceN // Open the "backend" V(device->Backend,open)("Loopback"); - device->Limiter = CreateDeviceLimiter(device); - { ALCdevice *head = ATOMIC_LOAD_SEQ(&DeviceList); do { diff --git a/Engine/lib/openal-soft/Alc/ALu.c b/Engine/lib/openal-soft/Alc/ALu.c index 81914850c..03abb116b 100644 --- a/Engine/lib/openal-soft/Alc/ALu.c +++ b/Engine/lib/openal-soft/Alc/ALu.c @@ -145,16 +145,6 @@ static inline HrtfDirectMixerFunc SelectHrtfMixer(void) } -/* Prior to VS2013, MSVC lacks the round() family of functions. */ -#if defined(_MSC_VER) && _MSC_VER < 1800 -static float roundf(float val) -{ - if(val < 0.0f) - return ceilf(val-0.5f); - return floorf(val+0.5f); -} -#endif - /* This RNG method was created based on the math found in opusdec. It's quick, * and starting with a seed value of 22222, is suitable for generating * whitenoise. @@ -221,32 +211,31 @@ void aluInit(void) static void SendSourceStoppedEvent(ALCcontext *context, ALuint id) { + AsyncEvent evt = ASYNC_EVENT(EventType_SourceStateChange); ALbitfieldSOFT enabledevt; - AsyncEvent evt; size_t strpos; ALuint scale; enabledevt = ATOMIC_LOAD(&context->EnabledEvts, almemory_order_acquire); if(!(enabledevt&EventType_SourceStateChange)) return; - evt.EnumType = EventType_SourceStateChange; - evt.Type = AL_EVENT_TYPE_SOURCE_STATE_CHANGED_SOFT; - evt.ObjectId = id; - evt.Param = AL_STOPPED; + evt.u.user.type = AL_EVENT_TYPE_SOURCE_STATE_CHANGED_SOFT; + evt.u.user.id = id; + evt.u.user.param = AL_STOPPED; /* Normally snprintf would be used, but this is called from the mixer and * that function's not real-time safe, so we have to construct it manually. */ - strcpy(evt.Message, "Source ID "); strpos = 10; + strcpy(evt.u.user.msg, "Source ID "); strpos = 10; scale = 1000000000; while(scale > 0 && scale > id) scale /= 10; while(scale > 0) { - evt.Message[strpos++] = '0' + ((id/scale)%10); + evt.u.user.msg[strpos++] = '0' + ((id/scale)%10); scale /= 10; } - strcpy(evt.Message+strpos, " state changed to AL_STOPPED"); + strcpy(evt.u.user.msg+strpos, " state changed to AL_STOPPED"); if(ll_ringbuffer_write(context->AsyncEvents, (const char*)&evt, 1) == 1) alsem_post(&context->EventSem); @@ -342,9 +331,7 @@ void aluSelectPostProcess(ALCdevice *device) } -/* Prepares the interpolator for a given rate (determined by increment). A - * result of AL_FALSE indicates that the filter output will completely cut - * the input signal. +/* Prepares the interpolator for a given rate (determined by increment). * * With a bit of work, and a trade of memory for CPU cost, this could be * modified for use with an interpolated increment for buttery-smooth pitch @@ -369,7 +356,7 @@ void BsincPrepare(const ALuint increment, BsincState *state, const BSincTable *t state->sf = sf; state->m = table->m[si]; - state->l = -((state->m/2) - 1); + state->l = (state->m/2) - 1; state->filter = table->Tab + table->filterOffset[si]; } @@ -475,12 +462,40 @@ static bool CalcEffectSlotParams(ALeffectslot *slot, ALCcontext *context, bool f slot->Params.AirAbsorptionGainHF = 1.0f; } - /* Swap effect states. No need to play with the ref counts since they - * keep the same number of refs. - */ state = props->State; - props->State = slot->Params.EffectState; - slot->Params.EffectState = state; + + if(state == slot->Params.EffectState) + { + /* If the effect state is the same as current, we can decrement its + * count safely to remove it from the update object (it can't reach + * 0 refs since the current params also hold a reference). + */ + DecrementRef(&state->Ref); + props->State = NULL; + } + else + { + /* Otherwise, replace it and send off the old one with a release + * event. + */ + AsyncEvent evt = ASYNC_EVENT(EventType_ReleaseEffectState); + evt.u.EffectState = slot->Params.EffectState; + + slot->Params.EffectState = state; + props->State = NULL; + + if(LIKELY(ll_ringbuffer_write(context->AsyncEvents, (const char*)&evt, 1) != 0)) + alsem_post(&context->EventSem); + else + { + /* If writing the event failed, the queue was probably full. + * Store the old state in the property object where it can + * eventually be cleaned up sometime later (not ideal, but + * better than blocking or leaking). + */ + props->State = evt.u.EffectState; + } + } ATOMIC_REPLACE_HEAD(struct ALeffectslotProps*, &context->FreeEffectslotProps, props); } @@ -656,24 +671,26 @@ static void CalcPanningAndFilters(ALvoice *voice, const ALfloat Azi, const ALflo NfcFilterAdjust(&voice->Direct.Params[0].NFCtrlFilter, w0); for(i = 0;i < MAX_AMBI_ORDER+1;i++) - voice->Direct.ChannelsPerOrder[i] = Device->Dry.NumChannelsPerOrder[i]; + voice->Direct.ChannelsPerOrder[i] = Device->NumChannelsPerOrder[i]; voice->Flags |= VOICE_HAS_NFC; } - if(Device->Render_Mode == StereoPair) - CalcAnglePairwiseCoeffs(Azi, Elev, Spread, coeffs); - else - CalcAngleCoeffs(Azi, Elev, Spread, coeffs); + /* A scalar of 1.5 for plain stereo results in +/-60 degrees being + * moved to +/-90 degrees for direct right and left speaker + * responses. + */ + CalcAngleCoeffs((Device->Render_Mode==StereoPair) ? ScaleAzimuthFront(Azi, 1.5f) : Azi, + Elev, Spread, coeffs); /* NOTE: W needs to be scaled by sqrt(2) due to FuMa normalization. */ - ComputeDryPanGains(&Device->Dry, coeffs, DryGain*1.414213562f, + ComputePanGains(&Device->Dry, coeffs, DryGain*SQRTF_2, voice->Direct.Params[0].Gains.Target); for(i = 0;i < NumSends;i++) { const ALeffectslot *Slot = SendSlots[i]; if(Slot) - ComputePanningGainsBF(Slot->ChanMap, Slot->NumChannels, - coeffs, WetGain[i]*1.414213562f, voice->Send[i].Params[0].Gains.Target + ComputePanningGainsBF(Slot->ChanMap, Slot->NumChannels, coeffs, + WetGain[i]*SQRTF_2, voice->Send[i].Params[0].Gains.Target ); } } @@ -682,8 +699,6 @@ static void CalcPanningAndFilters(ALvoice *voice, const ALfloat Azi, const ALflo /* Local B-Format sources have their XYZ channels rotated according * to the orientation. */ - const ALfloat sqrt_2 = sqrtf(2.0f); - const ALfloat sqrt_3 = sqrtf(3.0f); ALfloat N[3], V[3], U[3]; aluMatrixf matrix; @@ -726,25 +741,25 @@ static void CalcPanningAndFilters(ALvoice *voice, const ALfloat Azi, const ALflo * outputs on the columns. */ aluMatrixfSet(&matrix, - // ACN0 ACN1 ACN2 ACN3 - sqrt_2, 0.0f, 0.0f, 0.0f, // Ambi W - 0.0f, -N[0]*sqrt_3, N[1]*sqrt_3, -N[2]*sqrt_3, // Ambi X - 0.0f, U[0]*sqrt_3, -U[1]*sqrt_3, U[2]*sqrt_3, // Ambi Y - 0.0f, -V[0]*sqrt_3, V[1]*sqrt_3, -V[2]*sqrt_3 // Ambi Z + // ACN0 ACN1 ACN2 ACN3 + SQRTF_2, 0.0f, 0.0f, 0.0f, // Ambi W + 0.0f, -N[0]*SQRTF_3, N[1]*SQRTF_3, -N[2]*SQRTF_3, // Ambi X + 0.0f, U[0]*SQRTF_3, -U[1]*SQRTF_3, U[2]*SQRTF_3, // Ambi Y + 0.0f, -V[0]*SQRTF_3, V[1]*SQRTF_3, -V[2]*SQRTF_3 // Ambi Z ); voice->Direct.Buffer = Device->FOAOut.Buffer; voice->Direct.Channels = Device->FOAOut.NumChannels; for(c = 0;c < num_channels;c++) - ComputeFirstOrderGains(&Device->FOAOut, matrix.m[c], DryGain, - voice->Direct.Params[c].Gains.Target); + ComputePanGains(&Device->FOAOut, matrix.m[c], DryGain, + voice->Direct.Params[c].Gains.Target); for(i = 0;i < NumSends;i++) { const ALeffectslot *Slot = SendSlots[i]; if(Slot) { for(c = 0;c < num_channels;c++) - ComputeFirstOrderGainsBF(Slot->ChanMap, Slot->NumChannels, + ComputePanningGainsBF(Slot->ChanMap, Slot->NumChannels, matrix.m[c], WetGain[i], voice->Send[i].Params[c].Gains.Target ); } @@ -900,17 +915,15 @@ static void CalcPanningAndFilters(ALvoice *voice, const ALfloat Azi, const ALflo NfcFilterAdjust(&voice->Direct.Params[c].NFCtrlFilter, w0); for(i = 0;i < MAX_AMBI_ORDER+1;i++) - voice->Direct.ChannelsPerOrder[i] = Device->Dry.NumChannelsPerOrder[i]; + voice->Direct.ChannelsPerOrder[i] = Device->NumChannelsPerOrder[i]; voice->Flags |= VOICE_HAS_NFC; } /* Calculate the directional coefficients once, which apply to all * input channels. */ - if(Device->Render_Mode == StereoPair) - CalcAnglePairwiseCoeffs(Azi, Elev, Spread, coeffs); - else - CalcAngleCoeffs(Azi, Elev, Spread, coeffs); + CalcAngleCoeffs((Device->Render_Mode==StereoPair) ? ScaleAzimuthFront(Azi, 1.5f) : Azi, + Elev, Spread, coeffs); for(c = 0;c < num_channels;c++) { @@ -925,9 +938,8 @@ static void CalcPanningAndFilters(ALvoice *voice, const ALfloat Azi, const ALflo continue; } - ComputeDryPanGains(&Device->Dry, - coeffs, DryGain * downmix_gain, voice->Direct.Params[c].Gains.Target - ); + ComputePanGains(&Device->Dry, coeffs, DryGain * downmix_gain, + voice->Direct.Params[c].Gains.Target); } for(i = 0;i < NumSends;i++) @@ -963,7 +975,7 @@ static void CalcPanningAndFilters(ALvoice *voice, const ALfloat Azi, const ALflo NfcFilterAdjust(&voice->Direct.Params[c].NFCtrlFilter, w0); for(i = 0;i < MAX_AMBI_ORDER+1;i++) - voice->Direct.ChannelsPerOrder[i] = Device->Dry.NumChannelsPerOrder[i]; + voice->Direct.ChannelsPerOrder[i] = Device->NumChannelsPerOrder[i]; voice->Flags |= VOICE_HAS_NFC; } @@ -982,14 +994,14 @@ static void CalcPanningAndFilters(ALvoice *voice, const ALfloat Azi, const ALflo continue; } - if(Device->Render_Mode == StereoPair) - CalcAnglePairwiseCoeffs(chans[c].angle, chans[c].elevation, Spread, coeffs); - else - CalcAngleCoeffs(chans[c].angle, chans[c].elevation, Spread, coeffs); - ComputeDryPanGains(&Device->Dry, - coeffs, DryGain, voice->Direct.Params[c].Gains.Target + CalcAngleCoeffs( + (Device->Render_Mode==StereoPair) ? ScaleAzimuthFront(chans[c].angle, 3.0f) + : chans[c].angle, + chans[c].elevation, Spread, coeffs ); + ComputePanGains(&Device->Dry, coeffs, DryGain, + voice->Direct.Params[c].Gains.Target); for(i = 0;i < NumSends;i++) { const ALeffectslot *Slot = SendSlots[i]; @@ -1622,7 +1634,7 @@ static void ApplyDistanceComp(ALfloat (*restrict Samples)[BUFFERSIZE], DistanceC continue; } - if(SamplesToDo >= base) + if(LIKELY(SamplesToDo >= base)) { for(i = 0;i < base;i++) Values[i] = distbuf[i]; @@ -1650,6 +1662,9 @@ static void ApplyDither(ALfloat (*restrict Samples)[BUFFERSIZE], ALuint *dither_ ALuint seed = *dither_seed; ALsizei c, i; + ASSUME(numchans > 0); + ASSUME(SamplesToDo > 0); + /* Dithering. Step 1, generate whitenoise (uniform distribution of random * values between -1 and +1). Step 2 is to add the noise to the samples, * before rounding and after scaling up to the desired quantization depth. @@ -1663,7 +1678,7 @@ static void ApplyDither(ALfloat (*restrict Samples)[BUFFERSIZE], ALuint *dither_ ALuint rng0 = dither_rng(&seed); ALuint rng1 = dither_rng(&seed); val += (ALfloat)(rng0*(1.0/UINT_MAX) - rng1*(1.0/UINT_MAX)); - samples[i] = fastf2i(val) * invscale; + samples[i] = fast_roundf(val) * invscale; } } *dither_seed = seed; @@ -1674,9 +1689,9 @@ static inline ALfloat Conv_ALfloat(ALfloat val) { return val; } static inline ALint Conv_ALint(ALfloat val) { - /* Floats have a 23-bit mantissa. A bit of the exponent helps out along - * with the sign bit, giving 25 bits. So [-16777216, +16777216] is the max - * integer range normalized floats can be converted to before losing + /* Floats have a 23-bit mantissa. There is an implied 1 bit in the mantissa + * along with the sign bit, giving 25 bits total, so [-16777216, +16777216] + * is the max value a normalized float can be scaled to before losing * precision. */ return fastf2i(clampf(val*16777216.0f, -16777216.0f, 16777215.0f))<<7; @@ -1702,6 +1717,10 @@ static void Write##A(const ALfloat (*restrict InBuffer)[BUFFERSIZE], \ ALsizei numchans) \ { \ ALsizei i, j; \ + \ + ASSUME(numchans > 0); \ + ASSUME(SamplesToDo > 0); \ + \ for(j = 0;j < numchans;j++) \ { \ const ALfloat *restrict in = ASSUME_ALIGNED(InBuffer[j], 16); \ @@ -1819,8 +1838,7 @@ void aluMixData(ALCdevice *device, ALvoid *OutBuffer, ALsizei NumSamples) SamplesToDo, device->RealOut.NumChannels); if(device->Limiter) - ApplyCompression(device->Limiter, device->RealOut.NumChannels, SamplesToDo, - device->RealOut.Buffer); + ApplyCompression(device->Limiter, SamplesToDo, device->RealOut.Buffer); if(device->DitherDepth > 0.0f) ApplyDither(device->RealOut.Buffer, &device->DitherSeed, device->DitherDepth, @@ -1833,27 +1851,16 @@ void aluMixData(ALCdevice *device, ALvoid *OutBuffer, ALsizei NumSamples) switch(device->FmtType) { - case DevFmtByte: - WriteI8(Buffer, OutBuffer, SamplesDone, SamplesToDo, Channels); - break; - case DevFmtUByte: - WriteUI8(Buffer, OutBuffer, SamplesDone, SamplesToDo, Channels); - break; - case DevFmtShort: - WriteI16(Buffer, OutBuffer, SamplesDone, SamplesToDo, Channels); - break; - case DevFmtUShort: - WriteUI16(Buffer, OutBuffer, SamplesDone, SamplesToDo, Channels); - break; - case DevFmtInt: - WriteI32(Buffer, OutBuffer, SamplesDone, SamplesToDo, Channels); - break; - case DevFmtUInt: - WriteUI32(Buffer, OutBuffer, SamplesDone, SamplesToDo, Channels); - break; - case DevFmtFloat: - WriteF32(Buffer, OutBuffer, SamplesDone, SamplesToDo, Channels); - break; +#define HANDLE_WRITE(T, S) case T: \ + Write##S(Buffer, OutBuffer, SamplesDone, SamplesToDo, Channels); break; + HANDLE_WRITE(DevFmtByte, I8) + HANDLE_WRITE(DevFmtUByte, UI8) + HANDLE_WRITE(DevFmtShort, I16) + HANDLE_WRITE(DevFmtUShort, UI16) + HANDLE_WRITE(DevFmtInt, I32) + HANDLE_WRITE(DevFmtUInt, UI32) + HANDLE_WRITE(DevFmtFloat, F32) +#undef HANDLE_WRITE } } @@ -1865,35 +1872,24 @@ void aluMixData(ALCdevice *device, ALvoid *OutBuffer, ALsizei NumSamples) void aluHandleDisconnect(ALCdevice *device, const char *msg, ...) { + AsyncEvent evt = ASYNC_EVENT(EventType_Disconnected); ALCcontext *ctx; - AsyncEvent evt; va_list args; int msglen; if(!ATOMIC_EXCHANGE(&device->Connected, AL_FALSE, almemory_order_acq_rel)) return; - evt.EnumType = EventType_Disconnected; - evt.Type = AL_EVENT_TYPE_DISCONNECTED_SOFT; - evt.ObjectId = 0; - evt.Param = 0; + evt.u.user.type = AL_EVENT_TYPE_DISCONNECTED_SOFT; + evt.u.user.id = 0; + evt.u.user.param = 0; va_start(args, msg); - msglen = vsnprintf(evt.Message, sizeof(evt.Message), msg, args); + msglen = vsnprintf(evt.u.user.msg, sizeof(evt.u.user.msg), msg, args); va_end(args); - if(msglen < 0 || (size_t)msglen >= sizeof(evt.Message)) - { - evt.Message[sizeof(evt.Message)-1] = 0; - msglen = (int)strlen(evt.Message); - } - if(msglen > 0) - msg = evt.Message; - else - { - msg = ""; - msglen = (int)strlen(msg); - } + if(msglen < 0 || (size_t)msglen >= sizeof(evt.u.user.msg)) + evt.u.user.msg[sizeof(evt.u.user.msg)-1] = 0; ctx = ATOMIC_LOAD_SEQ(&device->ContextList); while(ctx) diff --git a/Engine/lib/openal-soft/Alc/alconfig.c b/Engine/lib/openal-soft/Alc/alconfig.c index 5dbf59d2b..3d0ed1404 100644 --- a/Engine/lib/openal-soft/Alc/alconfig.c +++ b/Engine/lib/openal-soft/Alc/alconfig.c @@ -36,6 +36,9 @@ #include #include #endif +#ifdef __APPLE__ +#include +#endif #include "alMain.h" #include "alconfig.h" @@ -478,6 +481,26 @@ void ReadALConfig(void) alstr_clear(&fname); } +#ifdef __APPLE__ + CFBundleRef mainBundle = CFBundleGetMainBundle(); + if(mainBundle) + { + unsigned char fileName[PATH_MAX]; + CFURLRef configURL; + + if((configURL=CFBundleCopyResourceURL(mainBundle, CFSTR(".alsoftrc"), CFSTR(""), NULL)) && + CFURLGetFileSystemRepresentation(configURL, true, fileName, sizeof(fileName))) + { + f = al_fopen((const char*)fileName, "r"); + if(f) + { + LoadConfigFromFile(f); + fclose(f); + } + } + } +#endif + if((str=getenv("HOME")) != NULL && *str) { alstr_copy_cstr(&fname, str); diff --git a/Engine/lib/openal-soft/Alc/backends/alsa.c b/Engine/lib/openal-soft/Alc/backends/alsa.c index e0fdc070b..a967fff0d 100644 --- a/Engine/lib/openal-soft/Alc/backends/alsa.c +++ b/Engine/lib/openal-soft/Alc/backends/alsa.c @@ -1153,10 +1153,17 @@ error2: static ALCboolean ALCcaptureAlsa_start(ALCcaptureAlsa *self) { - int err = snd_pcm_start(self->pcmHandle); + int err = snd_pcm_prepare(self->pcmHandle); + if(err < 0) + ERR("prepare failed: %s\n", snd_strerror(err)); + else + { + err = snd_pcm_start(self->pcmHandle); + if(err < 0) + ERR("start failed: %s\n", snd_strerror(err)); + } if(err < 0) { - ERR("start failed: %s\n", snd_strerror(err)); aluHandleDisconnect(STATIC_CAST(ALCbackend, self)->mDevice, "Capture state failure: %s", snd_strerror(err)); return ALC_FALSE; @@ -1368,11 +1375,6 @@ static ClockLatency ALCcaptureAlsa_getClockLatency(ALCcaptureAlsa *self) } -static inline void AppendAllDevicesList2(const DevMap *entry) -{ AppendAllDevicesList(alstr_get_cstr(entry->name)); } -static inline void AppendCaptureDeviceList2(const DevMap *entry) -{ AppendCaptureDeviceList(alstr_get_cstr(entry->name)); } - typedef struct ALCalsaBackendFactory { DERIVE_FROM_TYPE(ALCbackendFactory); } ALCalsaBackendFactory; @@ -1410,19 +1412,25 @@ static ALCboolean ALCalsaBackendFactory_querySupport(ALCalsaBackendFactory* UNUS return ALC_FALSE; } -static void ALCalsaBackendFactory_probe(ALCalsaBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCalsaBackendFactory_probe(ALCalsaBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { +#define APPEND_OUTNAME(i) do { \ + if(!alstr_empty((i)->name)) \ + alstr_append_range(outnames, VECTOR_BEGIN((i)->name), \ + VECTOR_END((i)->name)+1); \ +} while(0) case ALL_DEVICE_PROBE: probe_devices(SND_PCM_STREAM_PLAYBACK, &PlaybackDevices); - VECTOR_FOR_EACH(const DevMap, PlaybackDevices, AppendAllDevicesList2); + VECTOR_FOR_EACH(const DevMap, PlaybackDevices, APPEND_OUTNAME); break; case CAPTURE_DEVICE_PROBE: probe_devices(SND_PCM_STREAM_CAPTURE, &CaptureDevices); - VECTOR_FOR_EACH(const DevMap, CaptureDevices, AppendCaptureDeviceList2); + VECTOR_FOR_EACH(const DevMap, CaptureDevices, APPEND_OUTNAME); break; +#undef APPEND_OUTNAME } } diff --git a/Engine/lib/openal-soft/Alc/backends/base.c b/Engine/lib/openal-soft/Alc/backends/base.c index a451fee9b..9d8614b1e 100644 --- a/Engine/lib/openal-soft/Alc/backends/base.c +++ b/Engine/lib/openal-soft/Alc/backends/base.c @@ -12,6 +12,7 @@ extern inline ALuint64 GetDeviceClockTime(ALCdevice *device); extern inline void ALCdevice_Lock(ALCdevice *device); extern inline void ALCdevice_Unlock(ALCdevice *device); +extern inline ClockLatency GetClockLatency(ALCdevice *device); /* Base ALCbackend method implementations. */ void ALCbackend_Construct(ALCbackend *self, ALCdevice *device) diff --git a/Engine/lib/openal-soft/Alc/backends/base.h b/Engine/lib/openal-soft/Alc/backends/base.h index ba92b4ac6..03db56e99 100644 --- a/Engine/lib/openal-soft/Alc/backends/base.h +++ b/Engine/lib/openal-soft/Alc/backends/base.h @@ -3,6 +3,7 @@ #include "alMain.h" #include "threads.h" +#include "alstring.h" #ifdef __cplusplus @@ -115,7 +116,7 @@ struct ALCbackendFactoryVtable { ALCboolean (*const querySupport)(ALCbackendFactory *self, ALCbackend_Type type); - void (*const probe)(ALCbackendFactory *self, enum DevProbe type); + void (*const probe)(ALCbackendFactory *self, enum DevProbe type, al_string *outnames); ALCbackend* (*const createBackend)(ALCbackendFactory *self, ALCdevice *device, ALCbackend_Type type); }; @@ -124,7 +125,7 @@ struct ALCbackendFactoryVtable { DECLARE_THUNK(T, ALCbackendFactory, ALCboolean, init) \ DECLARE_THUNK(T, ALCbackendFactory, void, deinit) \ DECLARE_THUNK1(T, ALCbackendFactory, ALCboolean, querySupport, ALCbackend_Type) \ -DECLARE_THUNK1(T, ALCbackendFactory, void, probe, enum DevProbe) \ +DECLARE_THUNK2(T, ALCbackendFactory, void, probe, enum DevProbe, al_string*) \ DECLARE_THUNK2(T, ALCbackendFactory, ALCbackend*, createBackend, ALCdevice*, ALCbackend_Type) \ \ static const struct ALCbackendFactoryVtable T##_ALCbackendFactory_vtable = { \ @@ -142,7 +143,7 @@ ALCbackendFactory *ALCcoreAudioBackendFactory_getFactory(void); ALCbackendFactory *ALCossBackendFactory_getFactory(void); ALCbackendFactory *ALCjackBackendFactory_getFactory(void); ALCbackendFactory *ALCsolarisBackendFactory_getFactory(void); -ALCbackendFactory *ALCsndioBackendFactory_getFactory(void); +ALCbackendFactory *SndioBackendFactory_getFactory(void); ALCbackendFactory *ALCqsaBackendFactory_getFactory(void); ALCbackendFactory *ALCwasapiBackendFactory_getFactory(void); ALCbackendFactory *ALCdsoundBackendFactory_getFactory(void); @@ -161,6 +162,15 @@ inline void ALCdevice_Lock(ALCdevice *device) inline void ALCdevice_Unlock(ALCdevice *device) { V0(device->Backend,unlock)(); } + +inline ClockLatency GetClockLatency(ALCdevice *device) +{ + ClockLatency ret = V0(device->Backend,getClockLatency)(); + ret.Latency += device->FixedLatency; + return ret; +} + + #ifdef __cplusplus } /* extern "C" */ #endif diff --git a/Engine/lib/openal-soft/Alc/backends/coreaudio.c b/Engine/lib/openal-soft/Alc/backends/coreaudio.c index a8787f7b0..adb01fa6f 100644 --- a/Engine/lib/openal-soft/Alc/backends/coreaudio.c +++ b/Engine/lib/openal-soft/Alc/backends/coreaudio.c @@ -28,7 +28,6 @@ #include "alu.h" #include "ringbuffer.h" -#include #include #include #include @@ -112,7 +111,11 @@ static ALCenum ALCcoreAudioPlayback_open(ALCcoreAudioPlayback *self, const ALCch /* open the default output unit */ desc.componentType = kAudioUnitType_Output; +#if TARGET_OS_IOS + desc.componentSubType = kAudioUnitSubType_RemoteIO; +#else desc.componentSubType = kAudioUnitSubType_DefaultOutput; +#endif desc.componentManufacturer = kAudioUnitManufacturer_Apple; desc.componentFlags = 0; desc.componentFlagsMask = 0; @@ -451,7 +454,6 @@ static ALCenum ALCcoreAudioCapture_open(ALCcoreAudioCapture *self, const ALCchar AudioStreamBasicDescription outputFormat; // The AudioUnit output format AURenderCallbackStruct input; AudioComponentDescription desc; - AudioDeviceID inputDevice; UInt32 outputFrameCount; UInt32 propertySize; AudioObjectPropertyAddress propertyAddress; @@ -465,7 +467,11 @@ static ALCenum ALCcoreAudioCapture_open(ALCcoreAudioCapture *self, const ALCchar return ALC_INVALID_VALUE; desc.componentType = kAudioUnitType_Output; +#if TARGET_OS_IOS + desc.componentSubType = kAudioUnitSubType_RemoteIO; +#else desc.componentSubType = kAudioUnitSubType_HALOutput; +#endif desc.componentManufacturer = kAudioUnitManufacturer_Apple; desc.componentFlags = 0; desc.componentFlagsMask = 0; @@ -504,7 +510,9 @@ static ALCenum ALCcoreAudioCapture_open(ALCcoreAudioCapture *self, const ALCchar goto error; } +#if !TARGET_OS_IOS // Get the default input device + AudioDeviceID inputDevice = kAudioDeviceUnknown; propertySize = sizeof(AudioDeviceID); propertyAddress.mSelector = kAudioHardwarePropertyDefaultInputDevice; @@ -517,7 +525,6 @@ static ALCenum ALCcoreAudioCapture_open(ALCcoreAudioCapture *self, const ALCchar ERR("AudioObjectGetPropertyData failed\n"); goto error; } - if(inputDevice == kAudioDeviceUnknown) { ERR("No input device found\n"); @@ -531,6 +538,7 @@ static ALCenum ALCcoreAudioCapture_open(ALCcoreAudioCapture *self, const ALCchar ERR("AudioUnitSetProperty failed\n"); goto error; } +#endif // set capture callback input.inputProc = ALCcoreAudioCapture_RecordProc; @@ -752,7 +760,7 @@ ALCbackendFactory *ALCcoreAudioBackendFactory_getFactory(void); static ALCboolean ALCcoreAudioBackendFactory_init(ALCcoreAudioBackendFactory *self); static DECLARE_FORWARD(ALCcoreAudioBackendFactory, ALCbackendFactory, void, deinit) static ALCboolean ALCcoreAudioBackendFactory_querySupport(ALCcoreAudioBackendFactory *self, ALCbackend_Type type); -static void ALCcoreAudioBackendFactory_probe(ALCcoreAudioBackendFactory *self, enum DevProbe type); +static void ALCcoreAudioBackendFactory_probe(ALCcoreAudioBackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCcoreAudioBackendFactory_createBackend(ALCcoreAudioBackendFactory *self, ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCcoreAudioBackendFactory); @@ -776,15 +784,13 @@ static ALCboolean ALCcoreAudioBackendFactory_querySupport(ALCcoreAudioBackendFac return ALC_FALSE; } -static void ALCcoreAudioBackendFactory_probe(ALCcoreAudioBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCcoreAudioBackendFactory_probe(ALCcoreAudioBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { case ALL_DEVICE_PROBE: - AppendAllDevicesList(ca_device); - break; case CAPTURE_DEVICE_PROBE: - AppendCaptureDeviceList(ca_device); + alstr_append_range(outnames, ca_device, ca_device+sizeof(ca_device)); break; } } diff --git a/Engine/lib/openal-soft/Alc/backends/dsound.c b/Engine/lib/openal-soft/Alc/backends/dsound.c index 6bab641c7..c368cffb7 100644 --- a/Engine/lib/openal-soft/Alc/backends/dsound.c +++ b/Engine/lib/openal-soft/Alc/backends/dsound.c @@ -969,11 +969,6 @@ done: } -static inline void AppendAllDevicesList2(const DevMap *entry) -{ AppendAllDevicesList(alstr_get_cstr(entry->name)); } -static inline void AppendCaptureDeviceList2(const DevMap *entry) -{ AppendCaptureDeviceList(alstr_get_cstr(entry->name)); } - typedef struct ALCdsoundBackendFactory { DERIVE_FROM_TYPE(ALCbackendFactory); } ALCdsoundBackendFactory; @@ -984,7 +979,7 @@ ALCbackendFactory *ALCdsoundBackendFactory_getFactory(void); static ALCboolean ALCdsoundBackendFactory_init(ALCdsoundBackendFactory *self); static void ALCdsoundBackendFactory_deinit(ALCdsoundBackendFactory *self); static ALCboolean ALCdsoundBackendFactory_querySupport(ALCdsoundBackendFactory *self, ALCbackend_Type type); -static void ALCdsoundBackendFactory_probe(ALCdsoundBackendFactory *self, enum DevProbe type); +static void ALCdsoundBackendFactory_probe(ALCdsoundBackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCdsoundBackendFactory_createBackend(ALCdsoundBackendFactory *self, ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCdsoundBackendFactory); @@ -1028,7 +1023,7 @@ static ALCboolean ALCdsoundBackendFactory_querySupport(ALCdsoundBackendFactory* return ALC_FALSE; } -static void ALCdsoundBackendFactory_probe(ALCdsoundBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCdsoundBackendFactory_probe(ALCdsoundBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { HRESULT hr, hrcom; @@ -1036,12 +1031,17 @@ static void ALCdsoundBackendFactory_probe(ALCdsoundBackendFactory* UNUSED(self), hrcom = CoInitialize(NULL); switch(type) { +#define APPEND_OUTNAME(e) do { \ + if(!alstr_empty((e)->name)) \ + alstr_append_range(outnames, VECTOR_BEGIN((e)->name), \ + VECTOR_END((e)->name)+1); \ +} while(0) case ALL_DEVICE_PROBE: clear_devlist(&PlaybackDevices); hr = DirectSoundEnumerateW(DSoundEnumDevices, &PlaybackDevices); if(FAILED(hr)) ERR("Error enumerating DirectSound playback devices (0x%lx)!\n", hr); - VECTOR_FOR_EACH(const DevMap, PlaybackDevices, AppendAllDevicesList2); + VECTOR_FOR_EACH(const DevMap, PlaybackDevices, APPEND_OUTNAME); break; case CAPTURE_DEVICE_PROBE: @@ -1049,8 +1049,9 @@ static void ALCdsoundBackendFactory_probe(ALCdsoundBackendFactory* UNUSED(self), hr = DirectSoundCaptureEnumerateW(DSoundEnumDevices, &CaptureDevices); if(FAILED(hr)) ERR("Error enumerating DirectSound capture devices (0x%lx)!\n", hr); - VECTOR_FOR_EACH(const DevMap, CaptureDevices, AppendCaptureDeviceList2); + VECTOR_FOR_EACH(const DevMap, CaptureDevices, APPEND_OUTNAME); break; +#undef APPEND_OUTNAME } if(SUCCEEDED(hrcom)) CoUninitialize(); diff --git a/Engine/lib/openal-soft/Alc/backends/jack.c b/Engine/lib/openal-soft/Alc/backends/jack.c index 67e3c1068..fdbe93f2c 100644 --- a/Engine/lib/openal-soft/Alc/backends/jack.c +++ b/Engine/lib/openal-soft/Alc/backends/jack.c @@ -571,12 +571,12 @@ static ALCboolean ALCjackBackendFactory_querySupport(ALCjackBackendFactory* UNUS return ALC_FALSE; } -static void ALCjackBackendFactory_probe(ALCjackBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCjackBackendFactory_probe(ALCjackBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { case ALL_DEVICE_PROBE: - AppendAllDevicesList(jackDevice); + alstr_append_range(outnames, jackDevice, jackDevice+sizeof(jackDevice)); break; case CAPTURE_DEVICE_PROBE: diff --git a/Engine/lib/openal-soft/Alc/backends/loopback.c b/Engine/lib/openal-soft/Alc/backends/loopback.c index 9186a92f6..e99400868 100644 --- a/Engine/lib/openal-soft/Alc/backends/loopback.c +++ b/Engine/lib/openal-soft/Alc/backends/loopback.c @@ -87,7 +87,7 @@ ALCbackendFactory *ALCloopbackFactory_getFactory(void); static ALCboolean ALCloopbackFactory_init(ALCloopbackFactory *self); static DECLARE_FORWARD(ALCloopbackFactory, ALCbackendFactory, void, deinit) static ALCboolean ALCloopbackFactory_querySupport(ALCloopbackFactory *self, ALCbackend_Type type); -static void ALCloopbackFactory_probe(ALCloopbackFactory *self, enum DevProbe type); +static void ALCloopbackFactory_probe(ALCloopbackFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCloopbackFactory_createBackend(ALCloopbackFactory *self, ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCloopbackFactory); @@ -110,7 +110,7 @@ static ALCboolean ALCloopbackFactory_querySupport(ALCloopbackFactory* UNUSED(sel return ALC_FALSE; } -static void ALCloopbackFactory_probe(ALCloopbackFactory* UNUSED(self), enum DevProbe UNUSED(type)) +static void ALCloopbackFactory_probe(ALCloopbackFactory* UNUSED(self), enum DevProbe UNUSED(type), al_string* UNUSED(outnames)) { } diff --git a/Engine/lib/openal-soft/Alc/backends/null.c b/Engine/lib/openal-soft/Alc/backends/null.c index 2c2db54ef..d1c110e82 100644 --- a/Engine/lib/openal-soft/Alc/backends/null.c +++ b/Engine/lib/openal-soft/Alc/backends/null.c @@ -171,7 +171,7 @@ ALCbackendFactory *ALCnullBackendFactory_getFactory(void); static ALCboolean ALCnullBackendFactory_init(ALCnullBackendFactory *self); static DECLARE_FORWARD(ALCnullBackendFactory, ALCbackendFactory, void, deinit) static ALCboolean ALCnullBackendFactory_querySupport(ALCnullBackendFactory *self, ALCbackend_Type type); -static void ALCnullBackendFactory_probe(ALCnullBackendFactory *self, enum DevProbe type); +static void ALCnullBackendFactory_probe(ALCnullBackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCnullBackendFactory_createBackend(ALCnullBackendFactory *self, ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCnullBackendFactory); @@ -195,14 +195,13 @@ static ALCboolean ALCnullBackendFactory_querySupport(ALCnullBackendFactory* UNUS return ALC_FALSE; } -static void ALCnullBackendFactory_probe(ALCnullBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCnullBackendFactory_probe(ALCnullBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { case ALL_DEVICE_PROBE: - AppendAllDevicesList(nullDevice); - break; case CAPTURE_DEVICE_PROBE: + alstr_append_range(outnames, nullDevice, nullDevice+sizeof(nullDevice)); break; } } diff --git a/Engine/lib/openal-soft/Alc/backends/opensl.c b/Engine/lib/openal-soft/Alc/backends/opensl.c index a5ad3b098..d8ae001b3 100644 --- a/Engine/lib/openal-soft/Alc/backends/opensl.c +++ b/Engine/lib/openal-soft/Alc/backends/opensl.c @@ -206,6 +206,9 @@ static void ALCopenslPlayback_Destruct(ALCopenslPlayback* self) self->mEngineObj = NULL; self->mEngine = NULL; + ll_ringbuffer_free(self->mRing); + self->mRing = NULL; + alsem_destroy(&self->mSem); ALCbackend_Destruct(STATIC_CAST(ALCbackend, self)); @@ -251,19 +254,16 @@ static int ALCopenslPlayback_mixerProc(void *arg) result = VCALL(self->mBufferQueueObj,GetInterface)(SL_IID_PLAY, &player); PRINTERR(result, "bufferQueue->GetInterface SL_IID_PLAY"); } - if(SL_RESULT_SUCCESS != result) - { - ALCopenslPlayback_lock(self); - aluHandleDisconnect(device, "Failed to get playback buffer: 0x%08x", result); - ALCopenslPlayback_unlock(self); - return 1; - } ALCopenslPlayback_lock(self); - while(!ATOMIC_LOAD(&self->mKillNow, almemory_order_acquire) && + if(SL_RESULT_SUCCESS != result) + aluHandleDisconnect(device, "Failed to get playback buffer: 0x%08x", result); + + while(SL_RESULT_SUCCESS == result && + !ATOMIC_LOAD(&self->mKillNow, almemory_order_acquire) && ATOMIC_LOAD(&device->Connected, almemory_order_acquire)) { - size_t todo, len0, len1; + size_t todo; if(ll_ringbuffer_write_space(self->mRing) == 0) { @@ -292,34 +292,33 @@ static int ALCopenslPlayback_mixerProc(void *arg) } ll_ringbuffer_get_write_vector(self->mRing, data); + + aluMixData(device, data[0].buf, data[0].len*device->UpdateSize); + if(data[1].len > 0) + aluMixData(device, data[1].buf, data[1].len*device->UpdateSize); + todo = data[0].len+data[1].len; + ll_ringbuffer_write_advance(self->mRing, todo); - len0 = minu(todo, data[0].len); - len1 = minu(todo-len0, data[1].len); - - aluMixData(device, data[0].buf, len0*device->UpdateSize); - for(size_t i = 0;i < len0;i++) + for(size_t i = 0;i < todo;i++) { + if(!data[0].len) + { + data[0] = data[1]; + data[1].buf = NULL; + data[1].len = 0; + } + result = VCALL(bufferQueue,Enqueue)(data[0].buf, device->UpdateSize*self->mFrameSize); PRINTERR(result, "bufferQueue->Enqueue"); - if(SL_RESULT_SUCCESS == result) - ll_ringbuffer_write_advance(self->mRing, 1); - - data[0].buf += device->UpdateSize*self->mFrameSize; - } - - if(len1 > 0) - { - aluMixData(device, data[1].buf, len1*device->UpdateSize); - for(size_t i = 0;i < len1;i++) + if(SL_RESULT_SUCCESS != result) { - result = VCALL(bufferQueue,Enqueue)(data[1].buf, device->UpdateSize*self->mFrameSize); - PRINTERR(result, "bufferQueue->Enqueue"); - if(SL_RESULT_SUCCESS == result) - ll_ringbuffer_write_advance(self->mRing, 1); - - data[1].buf += device->UpdateSize*self->mFrameSize; + aluHandleDisconnect(device, "Failed to queue audio: 0x%08x", result); + break; } + + data[0].len--; + data[0].buf += device->UpdateSize*self->mFrameSize; } } ALCopenslPlayback_unlock(self); @@ -392,19 +391,24 @@ static ALCboolean ALCopenslPlayback_reset(ALCopenslPlayback *self) SLInterfaceID ids[2]; SLboolean reqs[2]; SLresult result; - JNIEnv *env; if(self->mBufferQueueObj != NULL) VCALL0(self->mBufferQueueObj,Destroy)(); self->mBufferQueueObj = NULL; + ll_ringbuffer_free(self->mRing); + self->mRing = NULL; + sampleRate = device->Frequency; - if(!(device->Flags&DEVICE_FREQUENCY_REQUEST) && (env=Android_GetJNIEnv()) != NULL) +#if 0 + if(!(device->Flags&DEVICE_FREQUENCY_REQUEST)) { /* FIXME: Disabled until I figure out how to get the Context needed for * the getSystemService call. */ -#if 0 + JNIEnv *env = Android_GetJNIEnv(); + jobject jctx = Android_GetContext(); + /* Get necessary stuff for using java.lang.Integer, * android.content.Context, and android.media.AudioManager. */ @@ -440,7 +444,7 @@ static ALCboolean ALCopenslPlayback_reset(ALCopenslPlayback *self) /* Now make the calls. */ //AudioManager audMgr = (AudioManager)getSystemService(Context.AUDIO_SERVICE); strobj = JCALL(env,GetStaticObjectField)(ctx_cls, ctx_audsvc); - jobject audMgr = JCALL(env,CallObjectMethod)(ctx_cls, ctx_getSysSvc, strobj); + jobject audMgr = JCALL(env,CallObjectMethod)(jctx, ctx_getSysSvc, strobj); strchars = JCALL(env,GetStringUTFChars)(strobj, NULL); TRACE("Context.getSystemService(%s) = %p\n", strchars, audMgr); JCALL(env,ReleaseStringUTFChars)(strobj, strchars); @@ -461,8 +465,8 @@ static ALCboolean ALCopenslPlayback_reset(ALCopenslPlayback *self) if(!sampleRate) sampleRate = device->Frequency; else sampleRate = maxu(sampleRate, MIN_OUTPUT_RATE); -#endif } +#endif if(sampleRate != device->Frequency) { @@ -546,6 +550,18 @@ static ALCboolean ALCopenslPlayback_reset(ALCopenslPlayback *self) result = VCALL(self->mBufferQueueObj,Realize)(SL_BOOLEAN_FALSE); PRINTERR(result, "bufferQueue->Realize"); } + if(SL_RESULT_SUCCESS == result) + { + self->mRing = ll_ringbuffer_create(device->NumUpdates, + self->mFrameSize*device->UpdateSize, true + ); + if(!self->mRing) + { + ERR("Out of memory allocating ring buffer %ux%u %u\n", device->UpdateSize, + device->NumUpdates, self->mFrameSize); + result = SL_RESULT_MEMORY_FAILURE; + } + } if(SL_RESULT_SUCCESS != result) { @@ -561,13 +577,10 @@ static ALCboolean ALCopenslPlayback_reset(ALCopenslPlayback *self) static ALCboolean ALCopenslPlayback_start(ALCopenslPlayback *self) { - ALCdevice *device = STATIC_CAST(ALCbackend,self)->mDevice; SLAndroidSimpleBufferQueueItf bufferQueue; SLresult result; - ll_ringbuffer_free(self->mRing); - self->mRing = ll_ringbuffer_create(device->NumUpdates, self->mFrameSize*device->UpdateSize, - true); + ll_ringbuffer_reset(self->mRing); result = VCALL(self->mBufferQueueObj,GetInterface)(SL_IID_ANDROIDSIMPLEBUFFERQUEUE, &bufferQueue); @@ -634,9 +647,6 @@ static void ALCopenslPlayback_stop(ALCopenslPlayback *self) } while(SL_RESULT_SUCCESS == result && state.count > 0); PRINTERR(result, "bufferQueue->GetState"); } - - ll_ringbuffer_free(self->mRing); - self->mRing = NULL; } static ClockLatency ALCopenslPlayback_getClockLatency(ALCopenslPlayback *self) @@ -713,9 +723,6 @@ static void ALCopenslCapture_Construct(ALCopenslCapture *self, ALCdevice *device static void ALCopenslCapture_Destruct(ALCopenslCapture *self) { - ll_ringbuffer_free(self->mRing); - self->mRing = NULL; - if(self->mRecordObj != NULL) VCALL0(self->mRecordObj,Destroy)(); self->mRecordObj = NULL; @@ -725,6 +732,9 @@ static void ALCopenslCapture_Destruct(ALCopenslCapture *self) self->mEngineObj = NULL; self->mEngine = NULL; + ll_ringbuffer_free(self->mRing); + self->mRing = NULL; + ALCbackend_Destruct(STATIC_CAST(ALCbackend, self)); } @@ -843,8 +853,9 @@ static ALCenum ALCopenslCapture_open(ALCopenslCapture *self, const ALCchar *name if(SL_RESULT_SUCCESS == result) { - self->mRing = ll_ringbuffer_create(device->NumUpdates, device->UpdateSize*self->mFrameSize, - false); + self->mRing = ll_ringbuffer_create(device->NumUpdates, + device->UpdateSize*self->mFrameSize, false + ); result = VCALL(self->mRecordObj,GetInterface)(SL_IID_ANDROIDSIMPLEBUFFERQUEUE, &bufferQueue); @@ -941,14 +952,16 @@ static ALCenum ALCopenslCapture_captureSamples(ALCopenslCapture *self, ALCvoid * SLAndroidSimpleBufferQueueItf bufferQueue; ll_ringbuffer_data_t data[2]; SLresult result; - size_t advance; ALCuint i; + result = VCALL(self->mRecordObj,GetInterface)(SL_IID_ANDROIDSIMPLEBUFFERQUEUE, + &bufferQueue); + PRINTERR(result, "recordObj->GetInterface"); + /* Read the desired samples from the ring buffer then advance its read * pointer. */ ll_ringbuffer_get_read_vector(self->mRing, data); - advance = 0; for(i = 0;i < samples;) { ALCuint rem = minu(samples - i, device->UpdateSize - self->mSplOffset); @@ -961,7 +974,11 @@ static ALCenum ALCopenslCapture_captureSamples(ALCopenslCapture *self, ALCvoid * { /* Finished a chunk, reset the offset and advance the read pointer. */ self->mSplOffset = 0; - advance++; + + ll_ringbuffer_read_advance(self->mRing, 1); + result = VCALL(bufferQueue,Enqueue)(data[0].buf, chunk_size); + PRINTERR(result, "bufferQueue->Enqueue"); + if(SL_RESULT_SUCCESS != result) break; data[0].len--; if(!data[0].len) @@ -972,24 +989,6 @@ static ALCenum ALCopenslCapture_captureSamples(ALCopenslCapture *self, ALCvoid * i += rem; } - ll_ringbuffer_read_advance(self->mRing, advance); - - result = VCALL(self->mRecordObj,GetInterface)(SL_IID_ANDROIDSIMPLEBUFFERQUEUE, - &bufferQueue); - PRINTERR(result, "recordObj->GetInterface"); - - /* Enqueue any newly-writable chunks in the ring buffer. */ - ll_ringbuffer_get_write_vector(self->mRing, data); - for(i = 0;i < data[0].len && SL_RESULT_SUCCESS == result;i++) - { - result = VCALL(bufferQueue,Enqueue)(data[0].buf + chunk_size*i, chunk_size); - PRINTERR(result, "bufferQueue->Enqueue"); - } - for(i = 0;i < data[1].len && SL_RESULT_SUCCESS == result;i++) - { - result = VCALL(bufferQueue,Enqueue)(data[1].buf + chunk_size*i, chunk_size); - PRINTERR(result, "bufferQueue->Enqueue"); - } if(SL_RESULT_SUCCESS != result) { @@ -1030,16 +1029,13 @@ static ALCboolean ALCopenslBackendFactory_querySupport(ALCopenslBackendFactory* return ALC_FALSE; } -static void ALCopenslBackendFactory_probe(ALCopenslBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCopenslBackendFactory_probe(ALCopenslBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { case ALL_DEVICE_PROBE: - AppendAllDevicesList(opensl_device); - break; - case CAPTURE_DEVICE_PROBE: - AppendAllDevicesList(opensl_device); + alstr_append_range(outnames, opensl_device, opensl_device+sizeof(opensl_device)); break; } } diff --git a/Engine/lib/openal-soft/Alc/backends/oss.c b/Engine/lib/openal-soft/Alc/backends/oss.c index c0c98c432..71faad259 100644 --- a/Engine/lib/openal-soft/Alc/backends/oss.c +++ b/Engine/lib/openal-soft/Alc/backends/oss.c @@ -786,7 +786,7 @@ ALCbackendFactory *ALCossBackendFactory_getFactory(void); static ALCboolean ALCossBackendFactory_init(ALCossBackendFactory *self); static void ALCossBackendFactory_deinit(ALCossBackendFactory *self); static ALCboolean ALCossBackendFactory_querySupport(ALCossBackendFactory *self, ALCbackend_Type type); -static void ALCossBackendFactory_probe(ALCossBackendFactory *self, enum DevProbe type); +static void ALCossBackendFactory_probe(ALCossBackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCossBackendFactory_createBackend(ALCossBackendFactory *self, ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCossBackendFactory); @@ -820,41 +820,32 @@ ALCboolean ALCossBackendFactory_querySupport(ALCossBackendFactory* UNUSED(self), return ALC_FALSE; } -void ALCossBackendFactory_probe(ALCossBackendFactory* UNUSED(self), enum DevProbe type) +void ALCossBackendFactory_probe(ALCossBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { - struct oss_device *cur; + struct oss_device *cur = NULL; switch(type) { case ALL_DEVICE_PROBE: ALCossListFree(&oss_playback); ALCossListPopulate(&oss_playback, DSP_CAP_OUTPUT); cur = &oss_playback; - while(cur != NULL) - { -#ifdef HAVE_STAT - struct stat buf; - if(stat(cur->path, &buf) == 0) -#endif - AppendAllDevicesList(cur->handle); - cur = cur->next; - } break; case CAPTURE_DEVICE_PROBE: ALCossListFree(&oss_capture); ALCossListPopulate(&oss_capture, DSP_CAP_INPUT); cur = &oss_capture; - while(cur != NULL) - { -#ifdef HAVE_STAT - struct stat buf; - if(stat(cur->path, &buf) == 0) -#endif - AppendCaptureDeviceList(cur->handle); - cur = cur->next; - } break; } + while(cur != NULL) + { +#ifdef HAVE_STAT + struct stat buf; + if(stat(cur->path, &buf) == 0) +#endif + alstr_append_range(outnames, cur->handle, cur->handle+strlen(cur->handle)+1); + cur = cur->next; + } } ALCbackend* ALCossBackendFactory_createBackend(ALCossBackendFactory* UNUSED(self), ALCdevice *device, ALCbackend_Type type) diff --git a/Engine/lib/openal-soft/Alc/backends/portaudio.c b/Engine/lib/openal-soft/Alc/backends/portaudio.c index 9b0d34878..6a6cfa318 100644 --- a/Engine/lib/openal-soft/Alc/backends/portaudio.c +++ b/Engine/lib/openal-soft/Alc/backends/portaudio.c @@ -484,9 +484,8 @@ typedef struct ALCportBackendFactory { static ALCboolean ALCportBackendFactory_init(ALCportBackendFactory *self); static void ALCportBackendFactory_deinit(ALCportBackendFactory *self); static ALCboolean ALCportBackendFactory_querySupport(ALCportBackendFactory *self, ALCbackend_Type type); -static void ALCportBackendFactory_probe(ALCportBackendFactory *self, enum DevProbe type); +static void ALCportBackendFactory_probe(ALCportBackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCportBackendFactory_createBackend(ALCportBackendFactory *self, ALCdevice *device, ALCbackend_Type type); - DEFINE_ALCBACKENDFACTORY_VTABLE(ALCportBackendFactory); @@ -518,15 +517,13 @@ static ALCboolean ALCportBackendFactory_querySupport(ALCportBackendFactory* UNUS return ALC_FALSE; } -static void ALCportBackendFactory_probe(ALCportBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCportBackendFactory_probe(ALCportBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { case ALL_DEVICE_PROBE: - AppendAllDevicesList(pa_device); - break; case CAPTURE_DEVICE_PROBE: - AppendCaptureDeviceList(pa_device); + alstr_append_range(outnames, pa_device, pa_device+sizeof(pa_device)); break; } } diff --git a/Engine/lib/openal-soft/Alc/backends/pulseaudio.c b/Engine/lib/openal-soft/Alc/backends/pulseaudio.c index 74d1a1492..b34d7abc2 100644 --- a/Engine/lib/openal-soft/Alc/backends/pulseaudio.c +++ b/Engine/lib/openal-soft/Alc/backends/pulseaudio.c @@ -1760,9 +1760,8 @@ typedef struct ALCpulseBackendFactory { static ALCboolean ALCpulseBackendFactory_init(ALCpulseBackendFactory *self); static void ALCpulseBackendFactory_deinit(ALCpulseBackendFactory *self); static ALCboolean ALCpulseBackendFactory_querySupport(ALCpulseBackendFactory *self, ALCbackend_Type type); -static void ALCpulseBackendFactory_probe(ALCpulseBackendFactory *self, enum DevProbe type); +static void ALCpulseBackendFactory_probe(ALCpulseBackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCpulseBackendFactory_createBackend(ALCpulseBackendFactory *self, ALCdevice *device, ALCbackend_Type type); - DEFINE_ALCBACKENDFACTORY_VTABLE(ALCpulseBackendFactory); @@ -1835,23 +1834,25 @@ static ALCboolean ALCpulseBackendFactory_querySupport(ALCpulseBackendFactory* UN return ALC_FALSE; } -static void ALCpulseBackendFactory_probe(ALCpulseBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCpulseBackendFactory_probe(ALCpulseBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { +#define APPEND_OUTNAME(e) do { \ + if(!alstr_empty((e)->name)) \ + alstr_append_range(outnames, VECTOR_BEGIN((e)->name), \ + VECTOR_END((e)->name)+1); \ +} while(0) case ALL_DEVICE_PROBE: ALCpulsePlayback_probeDevices(); -#define APPEND_ALL_DEVICES_LIST(e) AppendAllDevicesList(alstr_get_cstr((e)->name)) - VECTOR_FOR_EACH(const DevMap, PlaybackDevices, APPEND_ALL_DEVICES_LIST); -#undef APPEND_ALL_DEVICES_LIST + VECTOR_FOR_EACH(const DevMap, PlaybackDevices, APPEND_OUTNAME); break; case CAPTURE_DEVICE_PROBE: ALCpulseCapture_probeDevices(); -#define APPEND_CAPTURE_DEVICE_LIST(e) AppendCaptureDeviceList(alstr_get_cstr((e)->name)) - VECTOR_FOR_EACH(const DevMap, CaptureDevices, APPEND_CAPTURE_DEVICE_LIST); -#undef APPEND_CAPTURE_DEVICE_LIST + VECTOR_FOR_EACH(const DevMap, CaptureDevices, APPEND_OUTNAME); break; +#undef APPEND_OUTNAME } } @@ -1899,7 +1900,7 @@ static ALCboolean ALCpulseBackendFactory_querySupport(ALCpulseBackendFactory* UN return ALC_FALSE; } -static void ALCpulseBackendFactory_probe(ALCpulseBackendFactory* UNUSED(self), enum DevProbe UNUSED(type)) +static void ALCpulseBackendFactory_probe(ALCpulseBackendFactory* UNUSED(self), enum DevProbe UNUSED(type), al_string* UNUSED(outnames)) { } diff --git a/Engine/lib/openal-soft/Alc/backends/qsa.c b/Engine/lib/openal-soft/Alc/backends/qsa.c index 8f87779ba..816450960 100644 --- a/Engine/lib/openal-soft/Alc/backends/qsa.c +++ b/Engine/lib/openal-soft/Alc/backends/qsa.c @@ -119,6 +119,9 @@ static void deviceList(int type, vector_DevMap *devmap) if(max_cards < 0) return; +#define FREE_NAME(iter) free((iter)->name) + VECTOR_FOR_EACH(DevMap, *devmap, FREE_NAME); +#undef FREE_NAME VECTOR_RESIZE(*devmap, 0, max_cards+1); entry.name = strdup(qsaDevice); @@ -989,7 +992,7 @@ typedef struct ALCqsaBackendFactory { static ALCboolean ALCqsaBackendFactory_init(ALCqsaBackendFactory* UNUSED(self)); static void ALCqsaBackendFactory_deinit(ALCqsaBackendFactory* UNUSED(self)); static ALCboolean ALCqsaBackendFactory_querySupport(ALCqsaBackendFactory* UNUSED(self), ALCbackend_Type type); -static void ALCqsaBackendFactory_probe(ALCqsaBackendFactory* UNUSED(self), enum DevProbe type); +static void ALCqsaBackendFactory_probe(ALCqsaBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames); static ALCbackend* ALCqsaBackendFactory_createBackend(ALCqsaBackendFactory* UNUSED(self), ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCqsaBackendFactory); @@ -1016,33 +1019,25 @@ static ALCboolean ALCqsaBackendFactory_querySupport(ALCqsaBackendFactory* UNUSED return ALC_FALSE; } -static void ALCqsaBackendFactory_probe(ALCqsaBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCqsaBackendFactory_probe(ALCqsaBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch (type) { +#define APPEND_OUTNAME(e) do { \ + const char *n_ = (e)->name; \ + if(n_ && n_[0]) \ + alstr_append_range(outnames, n_, n_+strlen(n_)+1); \ +} while(0) case ALL_DEVICE_PROBE: -#define FREE_NAME(iter) free((iter)->name) - VECTOR_FOR_EACH(DevMap, DeviceNameMap, FREE_NAME); - VECTOR_RESIZE(DeviceNameMap, 0, 0); -#undef FREE_NAME - deviceList(SND_PCM_CHANNEL_PLAYBACK, &DeviceNameMap); -#define APPEND_DEVICE(iter) AppendAllDevicesList((iter)->name) - VECTOR_FOR_EACH(const DevMap, DeviceNameMap, APPEND_DEVICE); -#undef APPEND_DEVICE + VECTOR_FOR_EACH(const DevMap, DeviceNameMap, APPEND_OUTNAME); break; case CAPTURE_DEVICE_PROBE: -#define FREE_NAME(iter) free((iter)->name) - VECTOR_FOR_EACH(DevMap, CaptureNameMap, FREE_NAME); - VECTOR_RESIZE(CaptureNameMap, 0, 0); -#undef FREE_NAME - deviceList(SND_PCM_CHANNEL_CAPTURE, &CaptureNameMap); -#define APPEND_DEVICE(iter) AppendCaptureDeviceList((iter)->name) - VECTOR_FOR_EACH(const DevMap, CaptureNameMap, APPEND_DEVICE); -#undef APPEND_DEVICE + VECTOR_FOR_EACH(const DevMap, CaptureNameMap, APPEND_OUTNAME); break; +#undef APPEND_OUTNAME } } diff --git a/Engine/lib/openal-soft/Alc/backends/sdl2.c b/Engine/lib/openal-soft/Alc/backends/sdl2.c index cf005024f..3495e6bfb 100644 --- a/Engine/lib/openal-soft/Alc/backends/sdl2.c +++ b/Engine/lib/openal-soft/Alc/backends/sdl2.c @@ -221,7 +221,7 @@ ALCbackendFactory *ALCsdl2BackendFactory_getFactory(void); static ALCboolean ALCsdl2BackendFactory_init(ALCsdl2BackendFactory *self); static void ALCsdl2BackendFactory_deinit(ALCsdl2BackendFactory *self); static ALCboolean ALCsdl2BackendFactory_querySupport(ALCsdl2BackendFactory *self, ALCbackend_Type type); -static void ALCsdl2BackendFactory_probe(ALCsdl2BackendFactory *self, enum DevProbe type); +static void ALCsdl2BackendFactory_probe(ALCsdl2BackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCsdl2BackendFactory_createBackend(ALCsdl2BackendFactory *self, ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCsdl2BackendFactory); @@ -252,7 +252,7 @@ static ALCboolean ALCsdl2BackendFactory_querySupport(ALCsdl2BackendFactory* UNUS return ALC_FALSE; } -static void ALCsdl2BackendFactory_probe(ALCsdl2BackendFactory* UNUSED(self), enum DevProbe type) +static void ALCsdl2BackendFactory_probe(ALCsdl2BackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { int num_devices, i; al_string name; @@ -263,12 +263,13 @@ static void ALCsdl2BackendFactory_probe(ALCsdl2BackendFactory* UNUSED(self), enu AL_STRING_INIT(name); num_devices = SDL_GetNumAudioDevices(SDL_FALSE); - AppendAllDevicesList(defaultDeviceName); + alstr_append_range(outnames, defaultDeviceName, defaultDeviceName+sizeof(defaultDeviceName)); for(i = 0;i < num_devices;++i) { alstr_copy_cstr(&name, DEVNAME_PREFIX); alstr_append_cstr(&name, SDL_GetAudioDeviceName(i, SDL_FALSE)); - AppendAllDevicesList(alstr_get_cstr(name)); + if(!alstr_empty(name)) + alstr_append_range(outnames, VECTOR_BEGIN(name), VECTOR_END(name)+1); } alstr_reset(&name); } diff --git a/Engine/lib/openal-soft/Alc/backends/sndio.c b/Engine/lib/openal-soft/Alc/backends/sndio.c index 5aea457b5..dd174cbaa 100644 --- a/Engine/lib/openal-soft/Alc/backends/sndio.c +++ b/Engine/lib/openal-soft/Alc/backends/sndio.c @@ -27,15 +27,17 @@ #include "alMain.h" #include "alu.h" #include "threads.h" +#include "ringbuffer.h" #include "backends/base.h" #include +static const ALCchar sndio_device[] = "SndIO Default"; -typedef struct ALCsndioBackend { +typedef struct SndioPlayback { DERIVE_FROM_TYPE(ALCbackend); struct sio_hdl *sndHandle; @@ -45,40 +47,37 @@ typedef struct ALCsndioBackend { ATOMIC(int) killNow; althrd_t thread; -} ALCsndioBackend; +} SndioPlayback; -static int ALCsndioBackend_mixerProc(void *ptr); +static int SndioPlayback_mixerProc(void *ptr); -static void ALCsndioBackend_Construct(ALCsndioBackend *self, ALCdevice *device); -static void ALCsndioBackend_Destruct(ALCsndioBackend *self); -static ALCenum ALCsndioBackend_open(ALCsndioBackend *self, const ALCchar *name); -static ALCboolean ALCsndioBackend_reset(ALCsndioBackend *self); -static ALCboolean ALCsndioBackend_start(ALCsndioBackend *self); -static void ALCsndioBackend_stop(ALCsndioBackend *self); -static DECLARE_FORWARD2(ALCsndioBackend, ALCbackend, ALCenum, captureSamples, void*, ALCuint) -static DECLARE_FORWARD(ALCsndioBackend, ALCbackend, ALCuint, availableSamples) -static DECLARE_FORWARD(ALCsndioBackend, ALCbackend, ClockLatency, getClockLatency) -static DECLARE_FORWARD(ALCsndioBackend, ALCbackend, void, lock) -static DECLARE_FORWARD(ALCsndioBackend, ALCbackend, void, unlock) -DECLARE_DEFAULT_ALLOCATORS(ALCsndioBackend) +static void SndioPlayback_Construct(SndioPlayback *self, ALCdevice *device); +static void SndioPlayback_Destruct(SndioPlayback *self); +static ALCenum SndioPlayback_open(SndioPlayback *self, const ALCchar *name); +static ALCboolean SndioPlayback_reset(SndioPlayback *self); +static ALCboolean SndioPlayback_start(SndioPlayback *self); +static void SndioPlayback_stop(SndioPlayback *self); +static DECLARE_FORWARD2(SndioPlayback, ALCbackend, ALCenum, captureSamples, void*, ALCuint) +static DECLARE_FORWARD(SndioPlayback, ALCbackend, ALCuint, availableSamples) +static DECLARE_FORWARD(SndioPlayback, ALCbackend, ClockLatency, getClockLatency) +static DECLARE_FORWARD(SndioPlayback, ALCbackend, void, lock) +static DECLARE_FORWARD(SndioPlayback, ALCbackend, void, unlock) +DECLARE_DEFAULT_ALLOCATORS(SndioPlayback) -DEFINE_ALCBACKEND_VTABLE(ALCsndioBackend); +DEFINE_ALCBACKEND_VTABLE(SndioPlayback); -static const ALCchar sndio_device[] = "SndIO Default"; - - -static void ALCsndioBackend_Construct(ALCsndioBackend *self, ALCdevice *device) +static void SndioPlayback_Construct(SndioPlayback *self, ALCdevice *device) { ALCbackend_Construct(STATIC_CAST(ALCbackend, self), device); - SET_VTABLE2(ALCsndioBackend, ALCbackend, self); + SET_VTABLE2(SndioPlayback, ALCbackend, self); self->sndHandle = NULL; self->mix_data = NULL; ATOMIC_INIT(&self->killNow, AL_TRUE); } -static void ALCsndioBackend_Destruct(ALCsndioBackend *self) +static void SndioPlayback_Destruct(SndioPlayback *self) { if(self->sndHandle) sio_close(self->sndHandle); @@ -91,9 +90,9 @@ static void ALCsndioBackend_Destruct(ALCsndioBackend *self) } -static int ALCsndioBackend_mixerProc(void *ptr) +static int SndioPlayback_mixerProc(void *ptr) { - ALCsndioBackend *self = (ALCsndioBackend*)ptr; + SndioPlayback *self = (SndioPlayback*)ptr; ALCdevice *device = STATIC_CAST(ALCbackend, self)->mDevice; ALsizei frameSize; size_t wrote; @@ -109,9 +108,9 @@ static int ALCsndioBackend_mixerProc(void *ptr) ALsizei len = self->data_size; ALubyte *WritePtr = self->mix_data; - ALCsndioBackend_lock(self); + SndioPlayback_lock(self); aluMixData(device, WritePtr, len/frameSize); - ALCsndioBackend_unlock(self); + SndioPlayback_unlock(self); while(len > 0 && !ATOMIC_LOAD(&self->killNow, almemory_order_acquire)) { wrote = sio_write(self->sndHandle, WritePtr, len); @@ -133,7 +132,7 @@ static int ALCsndioBackend_mixerProc(void *ptr) } -static ALCenum ALCsndioBackend_open(ALCsndioBackend *self, const ALCchar *name) +static ALCenum SndioPlayback_open(SndioPlayback *self, const ALCchar *name) { ALCdevice *device = STATIC_CAST(ALCbackend,self)->mDevice; @@ -154,7 +153,7 @@ static ALCenum ALCsndioBackend_open(ALCsndioBackend *self, const ALCchar *name) return ALC_NO_ERROR; } -static ALCboolean ALCsndioBackend_reset(ALCsndioBackend *self) +static ALCboolean SndioPlayback_reset(SndioPlayback *self) { ALCdevice *device = STATIC_CAST(ALCbackend,self)->mDevice; struct sio_par par; @@ -239,7 +238,7 @@ static ALCboolean ALCsndioBackend_reset(ALCsndioBackend *self) return ALC_TRUE; } -static ALCboolean ALCsndioBackend_start(ALCsndioBackend *self) +static ALCboolean SndioPlayback_start(SndioPlayback *self) { ALCdevice *device = STATIC_CAST(ALCbackend,self)->mDevice; @@ -256,7 +255,7 @@ static ALCboolean ALCsndioBackend_start(ALCsndioBackend *self) } ATOMIC_STORE(&self->killNow, AL_FALSE, almemory_order_release); - if(althrd_create(&self->thread, ALCsndioBackend_mixerProc, self) != althrd_success) + if(althrd_create(&self->thread, SndioPlayback_mixerProc, self) != althrd_success) { sio_stop(self->sndHandle); return ALC_FALSE; @@ -265,7 +264,7 @@ static ALCboolean ALCsndioBackend_start(ALCsndioBackend *self) return ALC_TRUE; } -static void ALCsndioBackend_stop(ALCsndioBackend *self) +static void SndioPlayback_stop(SndioPlayback *self) { int res; @@ -281,59 +280,318 @@ static void ALCsndioBackend_stop(ALCsndioBackend *self) } -typedef struct ALCsndioBackendFactory { - DERIVE_FROM_TYPE(ALCbackendFactory); -} ALCsndioBackendFactory; -#define ALCSNDIOBACKENDFACTORY_INITIALIZER { { GET_VTABLE2(ALCsndioBackendFactory, ALCbackendFactory) } } +typedef struct SndioCapture { + DERIVE_FROM_TYPE(ALCbackend); -ALCbackendFactory *ALCsndioBackendFactory_getFactory(void); + struct sio_hdl *sndHandle; -static ALCboolean ALCsndioBackendFactory_init(ALCsndioBackendFactory *self); -static DECLARE_FORWARD(ALCsndioBackendFactory, ALCbackendFactory, void, deinit) -static ALCboolean ALCsndioBackendFactory_querySupport(ALCsndioBackendFactory *self, ALCbackend_Type type); -static void ALCsndioBackendFactory_probe(ALCsndioBackendFactory *self, enum DevProbe type); -static ALCbackend* ALCsndioBackendFactory_createBackend(ALCsndioBackendFactory *self, ALCdevice *device, ALCbackend_Type type); -DEFINE_ALCBACKENDFACTORY_VTABLE(ALCsndioBackendFactory); + ll_ringbuffer_t *ring; + + ATOMIC(int) killNow; + althrd_t thread; +} SndioCapture; + +static int SndioCapture_recordProc(void *ptr); + +static void SndioCapture_Construct(SndioCapture *self, ALCdevice *device); +static void SndioCapture_Destruct(SndioCapture *self); +static ALCenum SndioCapture_open(SndioCapture *self, const ALCchar *name); +static DECLARE_FORWARD(SndioCapture, ALCbackend, ALCboolean, reset) +static ALCboolean SndioCapture_start(SndioCapture *self); +static void SndioCapture_stop(SndioCapture *self); +static ALCenum SndioCapture_captureSamples(SndioCapture *self, void *buffer, ALCuint samples); +static ALCuint SndioCapture_availableSamples(SndioCapture *self); +static DECLARE_FORWARD(SndioCapture, ALCbackend, ClockLatency, getClockLatency) +static DECLARE_FORWARD(SndioCapture, ALCbackend, void, lock) +static DECLARE_FORWARD(SndioCapture, ALCbackend, void, unlock) +DECLARE_DEFAULT_ALLOCATORS(SndioCapture) + +DEFINE_ALCBACKEND_VTABLE(SndioCapture); -ALCbackendFactory *ALCsndioBackendFactory_getFactory(void) +static void SndioCapture_Construct(SndioCapture *self, ALCdevice *device) { - static ALCsndioBackendFactory factory = ALCSNDIOBACKENDFACTORY_INITIALIZER; - return STATIC_CAST(ALCbackendFactory, &factory); + ALCbackend_Construct(STATIC_CAST(ALCbackend, self), device); + SET_VTABLE2(SndioCapture, ALCbackend, self); + + self->sndHandle = NULL; + self->ring = NULL; + ATOMIC_INIT(&self->killNow, AL_TRUE); +} + +static void SndioCapture_Destruct(SndioCapture *self) +{ + if(self->sndHandle) + sio_close(self->sndHandle); + self->sndHandle = NULL; + + ll_ringbuffer_free(self->ring); + self->ring = NULL; + + ALCbackend_Destruct(STATIC_CAST(ALCbackend, self)); } -static ALCboolean ALCsndioBackendFactory_init(ALCsndioBackendFactory* UNUSED(self)) +static int SndioCapture_recordProc(void* ptr) +{ + SndioCapture *self = (SndioCapture*)ptr; + ALCdevice *device = STATIC_CAST(ALCbackend, self)->mDevice; + ALsizei frameSize; + + SetRTPriority(); + althrd_setname(althrd_current(), RECORD_THREAD_NAME); + + frameSize = FrameSizeFromDevFmt(device->FmtChans, device->FmtType, device->AmbiOrder); + + while(!ATOMIC_LOAD(&self->killNow, almemory_order_acquire) && + ATOMIC_LOAD(&device->Connected, almemory_order_acquire)) + { + ll_ringbuffer_data_t data[2]; + size_t total, todo; + + ll_ringbuffer_get_write_vector(self->ring, data); + todo = data[0].len + data[1].len; + if(todo == 0) + { + static char junk[4096]; + sio_read(self->sndHandle, junk, minz(sizeof(junk)/frameSize, device->UpdateSize)*frameSize); + continue; + } + + total = 0; + data[0].len *= frameSize; + data[1].len *= frameSize; + todo = minz(todo, device->UpdateSize) * frameSize; + while(total < todo) + { + size_t got; + + if(!data[0].len) + data[0] = data[1]; + + got = sio_read(self->sndHandle, data[0].buf, minz(todo-total, data[0].len)); + if(!got) + { + SndioCapture_lock(self); + aluHandleDisconnect(device, "Failed to read capture samples"); + SndioCapture_unlock(self); + break; + } + + data[0].buf += got; + data[0].len -= got; + total += got; + } + ll_ringbuffer_write_advance(self->ring, total / frameSize); + } + + return 0; +} + + +static ALCenum SndioCapture_open(SndioCapture *self, const ALCchar *name) +{ + ALCdevice *device = STATIC_CAST(ALCbackend,self)->mDevice; + struct sio_par par; + + if(!name) + name = sndio_device; + else if(strcmp(name, sndio_device) != 0) + return ALC_INVALID_VALUE; + + self->sndHandle = sio_open(NULL, SIO_REC, 0); + if(self->sndHandle == NULL) + { + ERR("Could not open device\n"); + return ALC_INVALID_VALUE; + } + + sio_initpar(&par); + + switch(device->FmtType) + { + case DevFmtByte: + par.bps = 1; + par.sig = 1; + break; + case DevFmtUByte: + par.bps = 1; + par.sig = 0; + break; + case DevFmtShort: + par.bps = 2; + par.sig = 1; + break; + case DevFmtUShort: + par.bps = 2; + par.sig = 0; + break; + case DevFmtInt: + par.bps = 4; + par.sig = 1; + break; + case DevFmtUInt: + par.bps = 4; + par.sig = 0; + break; + case DevFmtFloat: + ERR("%s capture samples not supported\n", DevFmtTypeString(device->FmtType)); + return ALC_INVALID_VALUE; + } + par.bits = par.bps * 8; + par.le = SIO_LE_NATIVE; + par.msb = SIO_LE_NATIVE ? 0 : 1; + par.rchan = ChannelsFromDevFmt(device->FmtChans, device->AmbiOrder); + par.rate = device->Frequency; + + par.appbufsz = maxu(device->UpdateSize*device->NumUpdates, (device->Frequency+9)/10); + par.round = clampu(par.appbufsz/device->NumUpdates, (device->Frequency+99)/100, + (device->Frequency+19)/20); + + device->UpdateSize = par.round; + device->NumUpdates = maxu(par.appbufsz/par.round, 1); + + if(!sio_setpar(self->sndHandle, &par) || !sio_getpar(self->sndHandle, &par)) + { + ERR("Failed to set device parameters\n"); + return ALC_INVALID_VALUE; + } + + if(par.bits != par.bps*8) + { + ERR("Padded samples not supported (%u of %u bits)\n", par.bits, par.bps*8); + return ALC_INVALID_VALUE; + } + + if(!((device->FmtType == DevFmtByte && par.bits == 8 && par.sig != 0) || + (device->FmtType == DevFmtUByte && par.bits == 8 && par.sig == 0) || + (device->FmtType == DevFmtShort && par.bits == 16 && par.sig != 0) || + (device->FmtType == DevFmtUShort && par.bits == 16 && par.sig == 0) || + (device->FmtType == DevFmtInt && par.bits == 32 && par.sig != 0) || + (device->FmtType == DevFmtUInt && par.bits == 32 && par.sig == 0)) || + ChannelsFromDevFmt(device->FmtChans, device->AmbiOrder) != (ALsizei)par.rchan || + device->Frequency != par.rate) + { + ERR("Failed to set format %s %s %uhz, got %c%u %u-channel %uhz instead\n", + DevFmtTypeString(device->FmtType), DevFmtChannelsString(device->FmtChans), + device->Frequency, par.sig?'s':'u', par.bits, par.rchan, par.rate); + return ALC_INVALID_VALUE; + } + + self->ring = ll_ringbuffer_create(device->UpdateSize*device->NumUpdates, par.bps*par.rchan, 0); + if(!self->ring) + { + ERR("Failed to allocate %u-byte ringbuffer\n", + device->UpdateSize*device->NumUpdates*par.bps*par.rchan); + return ALC_OUT_OF_MEMORY; + } + + SetDefaultChannelOrder(device); + + alstr_copy_cstr(&device->DeviceName, name); + + return ALC_NO_ERROR; +} + +static ALCboolean SndioCapture_start(SndioCapture *self) +{ + if(!sio_start(self->sndHandle)) + { + ERR("Error starting playback\n"); + return ALC_FALSE; + } + + ATOMIC_STORE(&self->killNow, AL_FALSE, almemory_order_release); + if(althrd_create(&self->thread, SndioCapture_recordProc, self) != althrd_success) + { + sio_stop(self->sndHandle); + return ALC_FALSE; + } + + return ALC_TRUE; +} + +static void SndioCapture_stop(SndioCapture *self) +{ + int res; + + if(ATOMIC_EXCHANGE(&self->killNow, AL_TRUE, almemory_order_acq_rel)) + return; + althrd_join(self->thread, &res); + + if(!sio_stop(self->sndHandle)) + ERR("Error stopping device\n"); +} + +static ALCenum SndioCapture_captureSamples(SndioCapture *self, void *buffer, ALCuint samples) +{ + ll_ringbuffer_read(self->ring, buffer, samples); + return ALC_NO_ERROR; +} + +static ALCuint SndioCapture_availableSamples(SndioCapture *self) +{ + return ll_ringbuffer_read_space(self->ring); +} + + +typedef struct SndioBackendFactory { + DERIVE_FROM_TYPE(ALCbackendFactory); +} SndioBackendFactory; +#define SNDIOBACKENDFACTORY_INITIALIZER { { GET_VTABLE2(SndioBackendFactory, ALCbackendFactory) } } + +ALCbackendFactory *SndioBackendFactory_getFactory(void); + +static ALCboolean SndioBackendFactory_init(SndioBackendFactory *self); +static DECLARE_FORWARD(SndioBackendFactory, ALCbackendFactory, void, deinit) +static ALCboolean SndioBackendFactory_querySupport(SndioBackendFactory *self, ALCbackend_Type type); +static void SndioBackendFactory_probe(SndioBackendFactory *self, enum DevProbe type, al_string *outnames); +static ALCbackend* SndioBackendFactory_createBackend(SndioBackendFactory *self, ALCdevice *device, ALCbackend_Type type); +DEFINE_ALCBACKENDFACTORY_VTABLE(SndioBackendFactory); + +ALCbackendFactory *SndioBackendFactory_getFactory(void) +{ + static SndioBackendFactory factory = SNDIOBACKENDFACTORY_INITIALIZER; + return STATIC_CAST(ALCbackendFactory, &factory); +} + +static ALCboolean SndioBackendFactory_init(SndioBackendFactory* UNUSED(self)) { /* No dynamic loading */ return ALC_TRUE; } -static ALCboolean ALCsndioBackendFactory_querySupport(ALCsndioBackendFactory* UNUSED(self), ALCbackend_Type type) +static ALCboolean SndioBackendFactory_querySupport(SndioBackendFactory* UNUSED(self), ALCbackend_Type type) { - if(type == ALCbackend_Playback) + if(type == ALCbackend_Playback || type == ALCbackend_Capture) return ALC_TRUE; return ALC_FALSE; } -static void ALCsndioBackendFactory_probe(ALCsndioBackendFactory* UNUSED(self), enum DevProbe type) +static void SndioBackendFactory_probe(SndioBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { case ALL_DEVICE_PROBE: - AppendAllDevicesList(sndio_device); - break; case CAPTURE_DEVICE_PROBE: + alstr_append_range(outnames, sndio_device, sndio_device+sizeof(sndio_device)); break; } } -static ALCbackend* ALCsndioBackendFactory_createBackend(ALCsndioBackendFactory* UNUSED(self), ALCdevice *device, ALCbackend_Type type) +static ALCbackend* SndioBackendFactory_createBackend(SndioBackendFactory* UNUSED(self), ALCdevice *device, ALCbackend_Type type) { if(type == ALCbackend_Playback) { - ALCsndioBackend *backend; - NEW_OBJ(backend, ALCsndioBackend)(device); + SndioPlayback *backend; + NEW_OBJ(backend, SndioPlayback)(device); + if(!backend) return NULL; + return STATIC_CAST(ALCbackend, backend); + } + if(type == ALCbackend_Capture) + { + SndioCapture *backend; + NEW_OBJ(backend, SndioCapture)(device); if(!backend) return NULL; return STATIC_CAST(ALCbackend, backend); } diff --git a/Engine/lib/openal-soft/Alc/backends/solaris.c b/Engine/lib/openal-soft/Alc/backends/solaris.c index f1c4aeaa2..712822043 100644 --- a/Engine/lib/openal-soft/Alc/backends/solaris.c +++ b/Engine/lib/openal-soft/Alc/backends/solaris.c @@ -302,7 +302,7 @@ ALCbackendFactory *ALCsolarisBackendFactory_getFactory(void); static ALCboolean ALCsolarisBackendFactory_init(ALCsolarisBackendFactory *self); static DECLARE_FORWARD(ALCsolarisBackendFactory, ALCbackendFactory, void, deinit) static ALCboolean ALCsolarisBackendFactory_querySupport(ALCsolarisBackendFactory *self, ALCbackend_Type type); -static void ALCsolarisBackendFactory_probe(ALCsolarisBackendFactory *self, enum DevProbe type); +static void ALCsolarisBackendFactory_probe(ALCsolarisBackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCsolarisBackendFactory_createBackend(ALCsolarisBackendFactory *self, ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCsolarisBackendFactory); @@ -327,7 +327,7 @@ static ALCboolean ALCsolarisBackendFactory_querySupport(ALCsolarisBackendFactory return ALC_FALSE; } -static void ALCsolarisBackendFactory_probe(ALCsolarisBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCsolarisBackendFactory_probe(ALCsolarisBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { @@ -337,7 +337,7 @@ static void ALCsolarisBackendFactory_probe(ALCsolarisBackendFactory* UNUSED(self struct stat buf; if(stat(solaris_driver, &buf) == 0) #endif - AppendAllDevicesList(solaris_device); + alstr_append_range(outnames, solaris_device, solaris_device+sizeof(solaris_device)); } break; diff --git a/Engine/lib/openal-soft/Alc/backends/wasapi.c b/Engine/lib/openal-soft/Alc/backends/wasapi.c index 50471f6b6..971a1f72b 100644 --- a/Engine/lib/openal-soft/Alc/backends/wasapi.c +++ b/Engine/lib/openal-soft/Alc/backends/wasapi.c @@ -1919,11 +1919,6 @@ ALCenum ALCwasapiCapture_captureSamples(ALCwasapiCapture *self, ALCvoid *buffer, } -static inline void AppendAllDevicesList2(const DevMap *entry) -{ AppendAllDevicesList(alstr_get_cstr(entry->name)); } -static inline void AppendCaptureDeviceList2(const DevMap *entry) -{ AppendCaptureDeviceList(alstr_get_cstr(entry->name)); } - typedef struct ALCwasapiBackendFactory { DERIVE_FROM_TYPE(ALCbackendFactory); } ALCwasapiBackendFactory; @@ -1932,7 +1927,7 @@ typedef struct ALCwasapiBackendFactory { static ALCboolean ALCwasapiBackendFactory_init(ALCwasapiBackendFactory *self); static void ALCwasapiBackendFactory_deinit(ALCwasapiBackendFactory *self); static ALCboolean ALCwasapiBackendFactory_querySupport(ALCwasapiBackendFactory *self, ALCbackend_Type type); -static void ALCwasapiBackendFactory_probe(ALCwasapiBackendFactory *self, enum DevProbe type); +static void ALCwasapiBackendFactory_probe(ALCwasapiBackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCwasapiBackendFactory_createBackend(ALCwasapiBackendFactory *self, ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCwasapiBackendFactory); @@ -1989,7 +1984,7 @@ static ALCboolean ALCwasapiBackendFactory_querySupport(ALCwasapiBackendFactory* return ALC_FALSE; } -static void ALCwasapiBackendFactory_probe(ALCwasapiBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCwasapiBackendFactory_probe(ALCwasapiBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { ThreadRequest req = { NULL, 0 }; @@ -2003,13 +1998,19 @@ static void ALCwasapiBackendFactory_probe(ALCwasapiBackendFactory* UNUSED(self), hr = WaitForResponse(&req); if(SUCCEEDED(hr)) switch(type) { +#define APPEND_OUTNAME(e) do { \ + if(!alstr_empty((e)->name)) \ + alstr_append_range(outnames, VECTOR_BEGIN((e)->name), \ + VECTOR_END((e)->name)+1); \ +} while(0) case ALL_DEVICE_PROBE: - VECTOR_FOR_EACH(const DevMap, PlaybackDevices, AppendAllDevicesList2); + VECTOR_FOR_EACH(const DevMap, PlaybackDevices, APPEND_OUTNAME); break; case CAPTURE_DEVICE_PROBE: - VECTOR_FOR_EACH(const DevMap, CaptureDevices, AppendCaptureDeviceList2); + VECTOR_FOR_EACH(const DevMap, CaptureDevices, APPEND_OUTNAME); break; +#undef APPEND_OUTNAME } CloseHandle(req.FinishedEvt); req.FinishedEvt = NULL; diff --git a/Engine/lib/openal-soft/Alc/backends/wave.c b/Engine/lib/openal-soft/Alc/backends/wave.c index 557c2bf26..390b2a5f1 100644 --- a/Engine/lib/openal-soft/Alc/backends/wave.c +++ b/Engine/lib/openal-soft/Alc/backends/wave.c @@ -403,7 +403,7 @@ ALCbackendFactory *ALCwaveBackendFactory_getFactory(void); static ALCboolean ALCwaveBackendFactory_init(ALCwaveBackendFactory *self); static DECLARE_FORWARD(ALCwaveBackendFactory, ALCbackendFactory, void, deinit) static ALCboolean ALCwaveBackendFactory_querySupport(ALCwaveBackendFactory *self, ALCbackend_Type type); -static void ALCwaveBackendFactory_probe(ALCwaveBackendFactory *self, enum DevProbe type); +static void ALCwaveBackendFactory_probe(ALCwaveBackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCwaveBackendFactory_createBackend(ALCwaveBackendFactory *self, ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCwaveBackendFactory); @@ -427,12 +427,12 @@ static ALCboolean ALCwaveBackendFactory_querySupport(ALCwaveBackendFactory* UNUS return ALC_FALSE; } -static void ALCwaveBackendFactory_probe(ALCwaveBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCwaveBackendFactory_probe(ALCwaveBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { case ALL_DEVICE_PROBE: - AppendAllDevicesList(waveDevice); + alstr_append_range(outnames, waveDevice, waveDevice+sizeof(waveDevice)); break; case CAPTURE_DEVICE_PROBE: break; diff --git a/Engine/lib/openal-soft/Alc/backends/winmm.c b/Engine/lib/openal-soft/Alc/backends/winmm.c index 2f4c65dff..0d4a02b8e 100644 --- a/Engine/lib/openal-soft/Alc/backends/winmm.c +++ b/Engine/lib/openal-soft/Alc/backends/winmm.c @@ -700,17 +700,6 @@ static ALCuint ALCwinmmCapture_availableSamples(ALCwinmmCapture *self) } -static inline void AppendAllDevicesList2(const al_string *name) -{ - if(!alstr_empty(*name)) - AppendAllDevicesList(alstr_get_cstr(*name)); -} -static inline void AppendCaptureDeviceList2(const al_string *name) -{ - if(!alstr_empty(*name)) - AppendCaptureDeviceList(alstr_get_cstr(*name)); -} - typedef struct ALCwinmmBackendFactory { DERIVE_FROM_TYPE(ALCbackendFactory); } ALCwinmmBackendFactory; @@ -719,7 +708,7 @@ typedef struct ALCwinmmBackendFactory { static ALCboolean ALCwinmmBackendFactory_init(ALCwinmmBackendFactory *self); static void ALCwinmmBackendFactory_deinit(ALCwinmmBackendFactory *self); static ALCboolean ALCwinmmBackendFactory_querySupport(ALCwinmmBackendFactory *self, ALCbackend_Type type); -static void ALCwinmmBackendFactory_probe(ALCwinmmBackendFactory *self, enum DevProbe type); +static void ALCwinmmBackendFactory_probe(ALCwinmmBackendFactory *self, enum DevProbe type, al_string *outnames); static ALCbackend* ALCwinmmBackendFactory_createBackend(ALCwinmmBackendFactory *self, ALCdevice *device, ALCbackend_Type type); DEFINE_ALCBACKENDFACTORY_VTABLE(ALCwinmmBackendFactory); @@ -749,19 +738,24 @@ static ALCboolean ALCwinmmBackendFactory_querySupport(ALCwinmmBackendFactory* UN return ALC_FALSE; } -static void ALCwinmmBackendFactory_probe(ALCwinmmBackendFactory* UNUSED(self), enum DevProbe type) +static void ALCwinmmBackendFactory_probe(ALCwinmmBackendFactory* UNUSED(self), enum DevProbe type, al_string *outnames) { switch(type) { +#define APPEND_OUTNAME(n) do { \ + if(!alstr_empty(*(n))) \ + alstr_append_range(outnames, VECTOR_BEGIN(*(n)), VECTOR_END(*(n))+1); \ +} while(0) case ALL_DEVICE_PROBE: ProbePlaybackDevices(); - VECTOR_FOR_EACH(const al_string, PlaybackDevices, AppendAllDevicesList2); + VECTOR_FOR_EACH(const al_string, PlaybackDevices, APPEND_OUTNAME); break; case CAPTURE_DEVICE_PROBE: ProbeCaptureDevices(); - VECTOR_FOR_EACH(const al_string, CaptureDevices, AppendCaptureDeviceList2); + VECTOR_FOR_EACH(const al_string, CaptureDevices, APPEND_OUTNAME); break; +#undef APPEND_OUTNAME } } diff --git a/Engine/lib/openal-soft/Alc/bformatdec.c b/Engine/lib/openal-soft/Alc/bformatdec.c index dcde7d70a..5233d06fe 100644 --- a/Engine/lib/openal-soft/Alc/bformatdec.c +++ b/Engine/lib/openal-soft/Alc/bformatdec.c @@ -438,7 +438,7 @@ void ambiup_reset(struct AmbiUpsampler *ambiup, const ALCdevice *device, ALfloat { ALfloat coeffs[MAX_AMBI_COEFFS] = { 0.0f }; CalcDirectionCoeffs(Ambi3DPoints[k], 0.0f, coeffs); - ComputeDryPanGains(&device->Dry, coeffs, 1.0f, encgains[k]); + ComputePanGains(&device->Dry, coeffs, 1.0f, encgains[k]); } /* Combine the matrices that do the in->virt and virt->out conversions @@ -450,11 +450,11 @@ void ambiup_reset(struct AmbiUpsampler *ambiup, const ALCdevice *device, ALfloat { for(j = 0;j < device->Dry.NumChannels;j++) { - ALfloat gain=0.0f; + ALdouble gain = 0.0; for(k = 0;k < COUNTOF(Ambi3DDecoder);k++) - gain += Ambi3DDecoder[k][i] * encgains[k][j]; - ambiup->Gains[i][j][HF_BAND] = gain * Ambi3DDecoderHFScale[i]; - ambiup->Gains[i][j][LF_BAND] = gain; + gain += (ALdouble)Ambi3DDecoder[k][i] * encgains[k][j]; + ambiup->Gains[i][j][HF_BAND] = (ALfloat)(gain * Ambi3DDecoderHFScale[i]); + ambiup->Gains[i][j][LF_BAND] = (ALfloat)gain; } } } diff --git a/Engine/lib/openal-soft/Alc/compat.h b/Engine/lib/openal-soft/Alc/compat.h index 093184c81..495bfdf24 100644 --- a/Engine/lib/openal-soft/Alc/compat.h +++ b/Engine/lib/openal-soft/Alc/compat.h @@ -50,14 +50,6 @@ void CloseLib(void *handle); void *GetSymbol(void *handle, const char *name); #endif -#ifdef __ANDROID__ -#define JCALL(obj, func) ((*(obj))->func((obj), EXTRACT_VCALL_ARGS -#define JCALL0(obj, func) ((*(obj))->func((obj) EXTRACT_VCALL_ARGS - -/** Returns a JNIEnv*. */ -void *Android_GetJNIEnv(void); -#endif - #ifdef __cplusplus } /* extern "C" */ #endif diff --git a/Engine/lib/openal-soft/Alc/effects/autowah.c b/Engine/lib/openal-soft/Alc/effects/autowah.c new file mode 100644 index 000000000..ba1180ef6 --- /dev/null +++ b/Engine/lib/openal-soft/Alc/effects/autowah.c @@ -0,0 +1,321 @@ +/** + * OpenAL cross platform audio library + * Copyright (C) 2018 by Raul Herraiz. + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Library General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Library General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * Or go to http://www.gnu.org/copyleft/lgpl.html + */ + +#include "config.h" + +#include +#include + +#include "alMain.h" +#include "alAuxEffectSlot.h" +#include "alError.h" +#include "alu.h" +#include "filters/defs.h" + +#define MIN_FREQ 20.0f +#define MAX_FREQ 2500.0f +#define Q_FACTOR 5.0f + +typedef struct ALautowahState { + DERIVE_FROM_TYPE(ALeffectState); + + /* Effect parameters */ + ALfloat AttackRate; + ALfloat ReleaseRate; + ALfloat ResonanceGain; + ALfloat PeakGain; + ALfloat FreqMinNorm; + ALfloat BandwidthNorm; + ALfloat env_delay; + + /* Filter components derived from the envelope. */ + struct { + ALfloat cos_w0; + ALfloat alpha; + } Env[BUFFERSIZE]; + + struct { + /* Effect filters' history. */ + struct { + ALfloat z1, z2; + } Filter; + + /* Effect gains for each output channel */ + ALfloat CurrentGains[MAX_OUTPUT_CHANNELS]; + ALfloat TargetGains[MAX_OUTPUT_CHANNELS]; + } Chans[MAX_EFFECT_CHANNELS]; + + /* Effects buffers */ + alignas(16) ALfloat BufferOut[BUFFERSIZE]; +} ALautowahState; + +static ALvoid ALautowahState_Destruct(ALautowahState *state); +static ALboolean ALautowahState_deviceUpdate(ALautowahState *state, ALCdevice *device); +static ALvoid ALautowahState_update(ALautowahState *state, const ALCcontext *context, const ALeffectslot *slot, const ALeffectProps *props); +static ALvoid ALautowahState_process(ALautowahState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels); +DECLARE_DEFAULT_ALLOCATORS(ALautowahState) + +DEFINE_ALEFFECTSTATE_VTABLE(ALautowahState); + +static void ALautowahState_Construct(ALautowahState *state) +{ + ALeffectState_Construct(STATIC_CAST(ALeffectState, state)); + SET_VTABLE2(ALautowahState, ALeffectState, state); +} + +static ALvoid ALautowahState_Destruct(ALautowahState *state) +{ + ALeffectState_Destruct(STATIC_CAST(ALeffectState,state)); +} + +static ALboolean ALautowahState_deviceUpdate(ALautowahState *state, ALCdevice *UNUSED(device)) +{ + /* (Re-)initializing parameters and clear the buffers. */ + ALsizei i, j; + + state->AttackRate = 1.0f; + state->ReleaseRate = 1.0f; + state->ResonanceGain = 10.0f; + state->PeakGain = 4.5f; + state->FreqMinNorm = 4.5e-4f; + state->BandwidthNorm = 0.05f; + state->env_delay = 0.0f; + + memset(state->Env, 0, sizeof(state->Env)); + + for(i = 0;i < MAX_EFFECT_CHANNELS;i++) + { + for(j = 0;j < MAX_OUTPUT_CHANNELS;j++) + state->Chans[i].CurrentGains[j] = 0.0f; + state->Chans[i].Filter.z1 = 0.0f; + state->Chans[i].Filter.z2 = 0.0f; + } + + return AL_TRUE; +} + +static ALvoid ALautowahState_update(ALautowahState *state, const ALCcontext *context, const ALeffectslot *slot, const ALeffectProps *props) +{ + const ALCdevice *device = context->Device; + ALfloat ReleaseTime; + ALsizei i; + + ReleaseTime = clampf(props->Autowah.ReleaseTime, 0.001f, 1.0f); + + state->AttackRate = expf(-1.0f / (props->Autowah.AttackTime*device->Frequency)); + state->ReleaseRate = expf(-1.0f / (ReleaseTime*device->Frequency)); + /* 0-20dB Resonance Peak gain */ + state->ResonanceGain = sqrtf(log10f(props->Autowah.Resonance)*10.0f / 3.0f); + state->PeakGain = 1.0f - log10f(props->Autowah.PeakGain/AL_AUTOWAH_MAX_PEAK_GAIN); + state->FreqMinNorm = MIN_FREQ / device->Frequency; + state->BandwidthNorm = (MAX_FREQ-MIN_FREQ) / device->Frequency; + + STATIC_CAST(ALeffectState,state)->OutBuffer = device->FOAOut.Buffer; + STATIC_CAST(ALeffectState,state)->OutChannels = device->FOAOut.NumChannels; + for(i = 0;i < MAX_EFFECT_CHANNELS;i++) + ComputePanGains(&device->FOAOut, IdentityMatrixf.m[i], slot->Params.Gain, + state->Chans[i].TargetGains); +} + +static ALvoid ALautowahState_process(ALautowahState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels) +{ + const ALfloat attack_rate = state->AttackRate; + const ALfloat release_rate = state->ReleaseRate; + const ALfloat res_gain = state->ResonanceGain; + const ALfloat peak_gain = state->PeakGain; + const ALfloat freq_min = state->FreqMinNorm; + const ALfloat bandwidth = state->BandwidthNorm; + ALfloat env_delay; + ALsizei c, i; + + env_delay = state->env_delay; + for(i = 0;i < SamplesToDo;i++) + { + ALfloat w0, sample, a; + + /* Envelope follower described on the book: Audio Effects, Theory, + * Implementation and Application. + */ + sample = peak_gain * fabsf(SamplesIn[0][i]); + a = (sample > env_delay) ? attack_rate : release_rate; + env_delay = lerp(sample, env_delay, a); + + /* Calculate the cos and alpha components for this sample's filter. */ + w0 = minf((bandwidth*env_delay + freq_min), 0.46f) * F_TAU; + state->Env[i].cos_w0 = cosf(w0); + state->Env[i].alpha = sinf(w0)/(2.0f * Q_FACTOR); + } + state->env_delay = env_delay; + + for(c = 0;c < MAX_EFFECT_CHANNELS; c++) + { + /* This effectively inlines BiquadFilter_setParams for a peaking + * filter and BiquadFilter_processC. The alpha and cosine components + * for the filter coefficients were previously calculated with the + * envelope. Because the filter changes for each sample, the + * coefficients are transient and don't need to be held. + */ + ALfloat z1 = state->Chans[c].Filter.z1; + ALfloat z2 = state->Chans[c].Filter.z2; + + for(i = 0;i < SamplesToDo;i++) + { + const ALfloat alpha = state->Env[i].alpha; + const ALfloat cos_w0 = state->Env[i].cos_w0; + ALfloat input, output; + ALfloat a[3], b[3]; + + b[0] = 1.0f + alpha*res_gain; + b[1] = -2.0f * cos_w0; + b[2] = 1.0f - alpha*res_gain; + a[0] = 1.0f + alpha/res_gain; + a[1] = -2.0f * cos_w0; + a[2] = 1.0f - alpha/res_gain; + + input = SamplesIn[c][i]; + output = input*(b[0]/a[0]) + z1; + z1 = input*(b[1]/a[0]) - output*(a[1]/a[0]) + z2; + z2 = input*(b[2]/a[0]) - output*(a[2]/a[0]); + state->BufferOut[i] = output; + } + state->Chans[c].Filter.z1 = z1; + state->Chans[c].Filter.z2 = z2; + + /* Now, mix the processed sound data to the output. */ + MixSamples(state->BufferOut, NumChannels, SamplesOut, state->Chans[c].CurrentGains, + state->Chans[c].TargetGains, SamplesToDo, 0, SamplesToDo); + } +} + +typedef struct AutowahStateFactory { + DERIVE_FROM_TYPE(EffectStateFactory); +} AutowahStateFactory; + +static ALeffectState *AutowahStateFactory_create(AutowahStateFactory *UNUSED(factory)) +{ + ALautowahState *state; + + NEW_OBJ0(state, ALautowahState)(); + if(!state) return NULL; + + return STATIC_CAST(ALeffectState, state); +} + +DEFINE_EFFECTSTATEFACTORY_VTABLE(AutowahStateFactory); + +EffectStateFactory *AutowahStateFactory_getFactory(void) +{ + static AutowahStateFactory AutowahFactory = { { GET_VTABLE2(AutowahStateFactory, EffectStateFactory) } }; + + return STATIC_CAST(EffectStateFactory, &AutowahFactory); +} + +void ALautowah_setParamf(ALeffect *effect, ALCcontext *context, ALenum param, ALfloat val) +{ + ALeffectProps *props = &effect->Props; + switch(param) + { + case AL_AUTOWAH_ATTACK_TIME: + if(!(val >= AL_AUTOWAH_MIN_ATTACK_TIME && val <= AL_AUTOWAH_MAX_ATTACK_TIME)) + SETERR_RETURN(context, AL_INVALID_VALUE,,"Autowah attack time out of range"); + props->Autowah.AttackTime = val; + break; + + case AL_AUTOWAH_RELEASE_TIME: + if(!(val >= AL_AUTOWAH_MIN_RELEASE_TIME && val <= AL_AUTOWAH_MAX_RELEASE_TIME)) + SETERR_RETURN(context, AL_INVALID_VALUE,,"Autowah release time out of range"); + props->Autowah.ReleaseTime = val; + break; + + case AL_AUTOWAH_RESONANCE: + if(!(val >= AL_AUTOWAH_MIN_RESONANCE && val <= AL_AUTOWAH_MAX_RESONANCE)) + SETERR_RETURN(context, AL_INVALID_VALUE,,"Autowah resonance out of range"); + props->Autowah.Resonance = val; + break; + + case AL_AUTOWAH_PEAK_GAIN: + if(!(val >= AL_AUTOWAH_MIN_PEAK_GAIN && val <= AL_AUTOWAH_MAX_PEAK_GAIN)) + SETERR_RETURN(context, AL_INVALID_VALUE,,"Autowah peak gain out of range"); + props->Autowah.PeakGain = val; + break; + + default: + alSetError(context, AL_INVALID_ENUM, "Invalid autowah float property 0x%04x", param); + } +} + +void ALautowah_setParamfv(ALeffect *effect, ALCcontext *context, ALenum param, const ALfloat *vals) +{ + ALautowah_setParamf(effect, context, param, vals[0]); +} + +void ALautowah_setParami(ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALint UNUSED(val)) +{ + alSetError(context, AL_INVALID_ENUM, "Invalid autowah integer property 0x%04x", param); +} + +void ALautowah_setParamiv(ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, const ALint *UNUSED(vals)) +{ + alSetError(context, AL_INVALID_ENUM, "Invalid autowah integer vector property 0x%04x", param); +} + +void ALautowah_getParami(const ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALint *UNUSED(val)) +{ + alSetError(context, AL_INVALID_ENUM, "Invalid autowah integer property 0x%04x", param); +} +void ALautowah_getParamiv(const ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALint *UNUSED(vals)) +{ + alSetError(context, AL_INVALID_ENUM, "Invalid autowah integer vector property 0x%04x", param); +} + +void ALautowah_getParamf(const ALeffect *effect, ALCcontext *context, ALenum param, ALfloat *val) +{ + + const ALeffectProps *props = &effect->Props; + switch(param) + { + case AL_AUTOWAH_ATTACK_TIME: + *val = props->Autowah.AttackTime; + break; + + case AL_AUTOWAH_RELEASE_TIME: + *val = props->Autowah.ReleaseTime; + break; + + case AL_AUTOWAH_RESONANCE: + *val = props->Autowah.Resonance; + break; + + case AL_AUTOWAH_PEAK_GAIN: + *val = props->Autowah.PeakGain; + break; + + default: + alSetError(context, AL_INVALID_ENUM, "Invalid autowah float property 0x%04x", param); + } + +} + +void ALautowah_getParamfv(const ALeffect *effect, ALCcontext *context, ALenum param, ALfloat *vals) +{ + ALautowah_getParamf(effect, context, param, vals); +} + +DEFINE_ALEFFECT_VTABLE(ALautowah); diff --git a/Engine/lib/openal-soft/Alc/effects/chorus.c b/Engine/lib/openal-soft/Alc/effects/chorus.c index ffb2b572e..f2861cf54 100644 --- a/Engine/lib/openal-soft/Alc/effects/chorus.c +++ b/Engine/lib/openal-soft/Alc/effects/chorus.c @@ -149,9 +149,9 @@ static ALvoid ALchorusState_update(ALchorusState *state, const ALCcontext *Conte /* Gains for left and right sides */ CalcAngleCoeffs(-F_PI_2, 0.0f, 0.0f, coeffs); - ComputeDryPanGains(&device->Dry, coeffs, Slot->Params.Gain, state->Gains[0].Target); + ComputePanGains(&device->Dry, coeffs, Slot->Params.Gain, state->Gains[0].Target); CalcAngleCoeffs( F_PI_2, 0.0f, 0.0f, coeffs); - ComputeDryPanGains(&device->Dry, coeffs, Slot->Params.Gain, state->Gains[1].Target); + ComputePanGains(&device->Dry, coeffs, Slot->Params.Gain, state->Gains[1].Target); phase = props->Chorus.Phase; rate = props->Chorus.Rate; diff --git a/Engine/lib/openal-soft/Alc/effects/compressor.c b/Engine/lib/openal-soft/Alc/effects/compressor.c index 4ec28f9aa..2b4a76b0b 100644 --- a/Engine/lib/openal-soft/Alc/effects/compressor.c +++ b/Engine/lib/openal-soft/Alc/effects/compressor.c @@ -27,6 +27,13 @@ #include "alu.h" +#define AMP_ENVELOPE_MIN 0.5f +#define AMP_ENVELOPE_MAX 2.0f + +#define ATTACK_TIME 0.1f /* 100ms to rise from min to max */ +#define RELEASE_TIME 0.2f /* 200ms to drop from max to min */ + + typedef struct ALcompressorState { DERIVE_FROM_TYPE(ALeffectState); @@ -35,9 +42,9 @@ typedef struct ALcompressorState { /* Effect parameters */ ALboolean Enabled; - ALfloat AttackRate; - ALfloat ReleaseRate; - ALfloat GainCtrl; + ALfloat AttackMult; + ALfloat ReleaseMult; + ALfloat EnvFollower; } ALcompressorState; static ALvoid ALcompressorState_Destruct(ALcompressorState *state); @@ -55,9 +62,9 @@ static void ALcompressorState_Construct(ALcompressorState *state) SET_VTABLE2(ALcompressorState, ALeffectState, state); state->Enabled = AL_TRUE; - state->AttackRate = 0.0f; - state->ReleaseRate = 0.0f; - state->GainCtrl = 1.0f; + state->AttackMult = 1.0f; + state->ReleaseMult = 1.0f; + state->EnvFollower = 1.0f; } static ALvoid ALcompressorState_Destruct(ALcompressorState *state) @@ -67,11 +74,17 @@ static ALvoid ALcompressorState_Destruct(ALcompressorState *state) static ALboolean ALcompressorState_deviceUpdate(ALcompressorState *state, ALCdevice *device) { - const ALfloat attackTime = device->Frequency * 0.2f; /* 200ms Attack */ - const ALfloat releaseTime = device->Frequency * 0.4f; /* 400ms Release */ + /* Number of samples to do a full attack and release (non-integer sample + * counts are okay). + */ + const ALfloat attackCount = (ALfloat)device->Frequency * ATTACK_TIME; + const ALfloat releaseCount = (ALfloat)device->Frequency * RELEASE_TIME; - state->AttackRate = 1.0f / attackTime; - state->ReleaseRate = 1.0f / releaseTime; + /* Calculate per-sample multipliers to attack and release at the desired + * rates. + */ + state->AttackMult = powf(AMP_ENVELOPE_MAX/AMP_ENVELOPE_MIN, 1.0f/attackCount); + state->ReleaseMult = powf(AMP_ENVELOPE_MIN/AMP_ENVELOPE_MAX, 1.0f/releaseCount); return AL_TRUE; } @@ -86,8 +99,7 @@ static ALvoid ALcompressorState_update(ALcompressorState *state, const ALCcontex STATIC_CAST(ALeffectState,state)->OutBuffer = device->FOAOut.Buffer; STATIC_CAST(ALeffectState,state)->OutChannels = device->FOAOut.NumChannels; for(i = 0;i < 4;i++) - ComputeFirstOrderGains(&device->FOAOut, IdentityMatrixf.m[i], - slot->Params.Gain, state->Gain[i]); + ComputePanGains(&device->FOAOut, IdentityMatrixf.m[i], slot->Params.Gain, state->Gain[i]); } static ALvoid ALcompressorState_process(ALcompressorState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels) @@ -97,71 +109,52 @@ static ALvoid ALcompressorState_process(ALcompressorState *state, ALsizei Sample for(base = 0;base < SamplesToDo;) { - ALfloat temps[64][4]; - ALsizei td = mini(64, SamplesToDo-base); - - /* Load samples into the temp buffer first. */ - for(j = 0;j < 4;j++) - { - for(i = 0;i < td;i++) - temps[i][j] = SamplesIn[j][i+base]; - } + ALfloat gains[256]; + ALsizei td = mini(256, SamplesToDo-base); + ALfloat env = state->EnvFollower; + /* Generate the per-sample gains from the signal envelope. */ if(state->Enabled) { - ALfloat gain = state->GainCtrl; - ALfloat output, amplitude; - - for(i = 0;i < td;i++) + for(i = 0;i < td;++i) { - /* Roughly calculate the maximum amplitude from the 4-channel - * signal, and attack or release the gain control to reach it. + /* Clamp the absolute amplitude to the defined envelope limits, + * then attack or release the envelope to reach it. */ - amplitude = fabsf(temps[i][0]); - amplitude = maxf(amplitude + fabsf(temps[i][1]), - maxf(amplitude + fabsf(temps[i][2]), - amplitude + fabsf(temps[i][3]))); - if(amplitude > gain) - gain = minf(gain+state->AttackRate, amplitude); - else if(amplitude < gain) - gain = maxf(gain-state->ReleaseRate, amplitude); + ALfloat amplitude = clampf(fabsf(SamplesIn[0][base+i]), + AMP_ENVELOPE_MIN, AMP_ENVELOPE_MAX); + if(amplitude > env) + env = minf(env*state->AttackMult, amplitude); + else if(amplitude < env) + env = maxf(env*state->ReleaseMult, amplitude); - /* Apply the inverse of the gain control to normalize/compress - * the volume. */ - output = 1.0f / clampf(gain, 0.5f, 2.0f); - for(j = 0;j < 4;j++) - temps[i][j] *= output; + /* Apply the reciprocal of the envelope to normalize the volume + * (compress the dynamic range). + */ + gains[i] = 1.0f / env; } - - state->GainCtrl = gain; } else { - ALfloat gain = state->GainCtrl; - ALfloat output, amplitude; - - for(i = 0;i < td;i++) + /* Same as above, except the amplitude is forced to 1. This helps + * ensure smooth gain changes when the compressor is turned on and + * off. + */ + for(i = 0;i < td;++i) { - /* Same as above, except the amplitude is forced to 1. This - * helps ensure smooth gain changes when the compressor is - * turned on and off. - */ - amplitude = 1.0f; - if(amplitude > gain) - gain = minf(gain+state->AttackRate, amplitude); - else if(amplitude < gain) - gain = maxf(gain-state->ReleaseRate, amplitude); + ALfloat amplitude = 1.0f; + if(amplitude > env) + env = minf(env*state->AttackMult, amplitude); + else if(amplitude < env) + env = maxf(env*state->ReleaseMult, amplitude); - output = 1.0f / clampf(gain, 0.5f, 2.0f); - for(j = 0;j < 4;j++) - temps[i][j] *= output; + gains[i] = 1.0f / env; } - - state->GainCtrl = gain; } + state->EnvFollower = env; - /* Now mix to the output. */ - for(j = 0;j < 4;j++) + /* Now compress the signal amplitude to output. */ + for(j = 0;j < MAX_EFFECT_CHANNELS;j++) { for(k = 0;k < NumChannels;k++) { @@ -170,7 +163,7 @@ static ALvoid ALcompressorState_process(ALcompressorState *state, ALsizei Sample continue; for(i = 0;i < td;i++) - SamplesOut[k][base+i] += gain * temps[i][j]; + SamplesOut[k][base+i] += SamplesIn[j][base+i] * gains[i] * gain; } } diff --git a/Engine/lib/openal-soft/Alc/effects/dedicated.c b/Engine/lib/openal-soft/Alc/effects/dedicated.c index 62a3894fe..0e1fd3895 100644 --- a/Engine/lib/openal-soft/Alc/effects/dedicated.c +++ b/Engine/lib/openal-soft/Alc/effects/dedicated.c @@ -102,7 +102,7 @@ static ALvoid ALdedicatedState_update(ALdedicatedState *state, const ALCcontext STATIC_CAST(ALeffectState,state)->OutBuffer = device->Dry.Buffer; STATIC_CAST(ALeffectState,state)->OutChannels = device->Dry.NumChannels; - ComputeDryPanGains(&device->Dry, coeffs, Gain, state->TargetGains); + ComputePanGains(&device->Dry, coeffs, Gain, state->TargetGains); } } } diff --git a/Engine/lib/openal-soft/Alc/effects/distortion.c b/Engine/lib/openal-soft/Alc/effects/distortion.c index f4e9969c0..de8da4fe5 100644 --- a/Engine/lib/openal-soft/Alc/effects/distortion.c +++ b/Engine/lib/openal-soft/Alc/effects/distortion.c @@ -104,8 +104,7 @@ static ALvoid ALdistortionState_update(ALdistortionState *state, const ALCcontex ); CalcAngleCoeffs(0.0f, 0.0f, 0.0f, coeffs); - ComputeDryPanGains(&device->Dry, coeffs, slot->Params.Gain * props->Distortion.Gain, - state->Gain); + ComputePanGains(&device->Dry, coeffs, slot->Params.Gain*props->Distortion.Gain, state->Gain); } static ALvoid ALdistortionState_process(ALdistortionState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels) diff --git a/Engine/lib/openal-soft/Alc/effects/echo.c b/Engine/lib/openal-soft/Alc/effects/echo.c index 676b17e8e..4570fcb19 100644 --- a/Engine/lib/openal-soft/Alc/effects/echo.c +++ b/Engine/lib/openal-soft/Alc/effects/echo.c @@ -141,11 +141,11 @@ static ALvoid ALechoState_update(ALechoState *state, const ALCcontext *context, /* First tap panning */ CalcAngleCoeffs(-F_PI_2*lrpan, 0.0f, spread, coeffs); - ComputeDryPanGains(&device->Dry, coeffs, slot->Params.Gain, state->Gains[0].Target); + ComputePanGains(&device->Dry, coeffs, slot->Params.Gain, state->Gains[0].Target); /* Second tap panning */ CalcAngleCoeffs( F_PI_2*lrpan, 0.0f, spread, coeffs); - ComputeDryPanGains(&device->Dry, coeffs, slot->Params.Gain, state->Gains[1].Target); + ComputePanGains(&device->Dry, coeffs, slot->Params.Gain, state->Gains[1].Target); } static ALvoid ALechoState_process(ALechoState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels) diff --git a/Engine/lib/openal-soft/Alc/effects/equalizer.c b/Engine/lib/openal-soft/Alc/effects/equalizer.c index 8ff56fb5d..171061271 100644 --- a/Engine/lib/openal-soft/Alc/effects/equalizer.c +++ b/Engine/lib/openal-soft/Alc/effects/equalizer.c @@ -76,12 +76,12 @@ typedef struct ALequalizerState { DERIVE_FROM_TYPE(ALeffectState); struct { + /* Effect parameters */ + BiquadFilter filter[4]; + /* Effect gains for each channel */ ALfloat CurrentGains[MAX_OUTPUT_CHANNELS]; ALfloat TargetGains[MAX_OUTPUT_CHANNELS]; - - /* Effect parameters */ - BiquadFilter filter[4]; } Chans[MAX_EFFECT_CHANNELS]; ALfloat SampleBuffer[MAX_EFFECT_CHANNELS][BUFFERSIZE]; @@ -128,12 +128,6 @@ static ALvoid ALequalizerState_update(ALequalizerState *state, const ALCcontext ALfloat gain, f0norm; ALuint i; - STATIC_CAST(ALeffectState,state)->OutBuffer = device->FOAOut.Buffer; - STATIC_CAST(ALeffectState,state)->OutChannels = device->FOAOut.NumChannels; - for(i = 0;i < MAX_EFFECT_CHANNELS;i++) - ComputeFirstOrderGains(&device->FOAOut, IdentityMatrixf.m[i], - slot->Params.Gain, state->Chans[i].TargetGains); - /* Calculate coefficients for the each type of filter. Note that the shelf * filters' gain is for the reference frequency, which is the centerpoint * of the transition band. @@ -174,6 +168,12 @@ static ALvoid ALequalizerState_update(ALequalizerState *state, const ALCcontext BiquadFilter_copyParams(&state->Chans[i].filter[2], &state->Chans[0].filter[2]); BiquadFilter_copyParams(&state->Chans[i].filter[3], &state->Chans[0].filter[3]); } + + STATIC_CAST(ALeffectState,state)->OutBuffer = device->FOAOut.Buffer; + STATIC_CAST(ALeffectState,state)->OutChannels = device->FOAOut.NumChannels; + for(i = 0;i < MAX_EFFECT_CHANNELS;i++) + ComputePanGains(&device->FOAOut, IdentityMatrixf.m[i], slot->Params.Gain, + state->Chans[i].TargetGains); } static ALvoid ALequalizerState_process(ALequalizerState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels) diff --git a/Engine/lib/openal-soft/Alc/effects/fshifter.c b/Engine/lib/openal-soft/Alc/effects/fshifter.c new file mode 100644 index 000000000..7d72472ae --- /dev/null +++ b/Engine/lib/openal-soft/Alc/effects/fshifter.c @@ -0,0 +1,329 @@ +/** + * OpenAL cross platform audio library + * Copyright (C) 2018 by Raul Herraiz. + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Library General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Library General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * Or go to http://www.gnu.org/copyleft/lgpl.html + */ + +#include "config.h" + +#include +#include + +#include "alMain.h" +#include "alAuxEffectSlot.h" +#include "alError.h" +#include "alu.h" +#include "filters/defs.h" + +#include "alcomplex.h" + +#define HIL_SIZE 1024 +#define OVERSAMP (1<<2) + +#define HIL_STEP (HIL_SIZE / OVERSAMP) +#define FIFO_LATENCY (HIL_STEP * (OVERSAMP-1)) + + +typedef struct ALfshifterState { + DERIVE_FROM_TYPE(ALeffectState); + + /* Effect parameters */ + ALsizei count; + ALsizei PhaseStep; + ALsizei Phase; + ALdouble ld_sign; + + /*Effects buffers*/ + ALfloat InFIFO[HIL_SIZE]; + ALcomplex OutFIFO[HIL_SIZE]; + ALcomplex OutputAccum[HIL_SIZE]; + ALcomplex Analytic[HIL_SIZE]; + ALcomplex Outdata[BUFFERSIZE]; + + alignas(16) ALfloat BufferOut[BUFFERSIZE]; + + /* Effect gains for each output channel */ + ALfloat CurrentGains[MAX_OUTPUT_CHANNELS]; + ALfloat TargetGains[MAX_OUTPUT_CHANNELS]; +} ALfshifterState; + +static ALvoid ALfshifterState_Destruct(ALfshifterState *state); +static ALboolean ALfshifterState_deviceUpdate(ALfshifterState *state, ALCdevice *device); +static ALvoid ALfshifterState_update(ALfshifterState *state, const ALCcontext *context, const ALeffectslot *slot, const ALeffectProps *props); +static ALvoid ALfshifterState_process(ALfshifterState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels); +DECLARE_DEFAULT_ALLOCATORS(ALfshifterState) + +DEFINE_ALEFFECTSTATE_VTABLE(ALfshifterState); + +/* Define a Hann window, used to filter the HIL input and output. */ +alignas(16) static ALdouble HannWindow[HIL_SIZE]; + +static void InitHannWindow(void) +{ + ALsizei i; + + /* Create lookup table of the Hann window for the desired size, i.e. HIL_SIZE */ + for(i = 0;i < HIL_SIZE>>1;i++) + { + ALdouble val = sin(M_PI * (ALdouble)i / (ALdouble)(HIL_SIZE-1)); + HannWindow[i] = HannWindow[HIL_SIZE-1-i] = val * val; + } +} + +static alonce_flag HannInitOnce = AL_ONCE_FLAG_INIT; + +static void ALfshifterState_Construct(ALfshifterState *state) +{ + ALeffectState_Construct(STATIC_CAST(ALeffectState, state)); + SET_VTABLE2(ALfshifterState, ALeffectState, state); + + alcall_once(&HannInitOnce, InitHannWindow); +} + +static ALvoid ALfshifterState_Destruct(ALfshifterState *state) +{ + ALeffectState_Destruct(STATIC_CAST(ALeffectState,state)); +} + +static ALboolean ALfshifterState_deviceUpdate(ALfshifterState *state, ALCdevice *UNUSED(device)) +{ + /* (Re-)initializing parameters and clear the buffers. */ + state->count = FIFO_LATENCY; + state->PhaseStep = 0; + state->Phase = 0; + state->ld_sign = 1.0; + + memset(state->InFIFO, 0, sizeof(state->InFIFO)); + memset(state->OutFIFO, 0, sizeof(state->OutFIFO)); + memset(state->OutputAccum, 0, sizeof(state->OutputAccum)); + memset(state->Analytic, 0, sizeof(state->Analytic)); + + memset(state->CurrentGains, 0, sizeof(state->CurrentGains)); + memset(state->TargetGains, 0, sizeof(state->TargetGains)); + + return AL_TRUE; +} + +static ALvoid ALfshifterState_update(ALfshifterState *state, const ALCcontext *context, const ALeffectslot *slot, const ALeffectProps *props) +{ + const ALCdevice *device = context->Device; + ALfloat coeffs[MAX_AMBI_COEFFS]; + ALfloat step; + + step = props->Fshifter.Frequency / (ALfloat)device->Frequency; + state->PhaseStep = fastf2i(minf(step, 0.5f) * FRACTIONONE); + + switch(props->Fshifter.LeftDirection) + { + case AL_FREQUENCY_SHIFTER_DIRECTION_DOWN: + state->ld_sign = -1.0; + break; + + case AL_FREQUENCY_SHIFTER_DIRECTION_UP: + state->ld_sign = 1.0; + break; + + case AL_FREQUENCY_SHIFTER_DIRECTION_OFF: + state->Phase = 0; + state->PhaseStep = 0; + break; + } + + CalcAngleCoeffs(0.0f, 0.0f, 0.0f, coeffs); + ComputePanGains(&device->Dry, coeffs, slot->Params.Gain, state->TargetGains); +} + +static ALvoid ALfshifterState_process(ALfshifterState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels) +{ + static const ALcomplex complex_zero = { 0.0, 0.0 }; + ALfloat *restrict BufferOut = state->BufferOut; + ALsizei j, k, base; + + for(base = 0;base < SamplesToDo;) + { + ALsizei todo = mini(HIL_SIZE-state->count, SamplesToDo-base); + + ASSUME(todo > 0); + + /* Fill FIFO buffer with samples data */ + k = state->count; + for(j = 0;j < todo;j++,k++) + { + state->InFIFO[k] = SamplesIn[0][base+j]; + state->Outdata[base+j] = state->OutFIFO[k-FIFO_LATENCY]; + } + state->count += todo; + base += todo; + + /* Check whether FIFO buffer is filled */ + if(state->count < HIL_SIZE) continue; + + state->count = FIFO_LATENCY; + + /* Real signal windowing and store in Analytic buffer */ + for(k = 0;k < HIL_SIZE;k++) + { + state->Analytic[k].Real = state->InFIFO[k] * HannWindow[k]; + state->Analytic[k].Imag = 0.0; + } + + /* Processing signal by Discrete Hilbert Transform (analytical signal). */ + complex_hilbert(state->Analytic, HIL_SIZE); + + /* Windowing and add to output accumulator */ + for(k = 0;k < HIL_SIZE;k++) + { + state->OutputAccum[k].Real += 2.0/OVERSAMP*HannWindow[k]*state->Analytic[k].Real; + state->OutputAccum[k].Imag += 2.0/OVERSAMP*HannWindow[k]*state->Analytic[k].Imag; + } + + /* Shift accumulator, input & output FIFO */ + for(k = 0;k < HIL_STEP;k++) state->OutFIFO[k] = state->OutputAccum[k]; + for(j = 0;k < HIL_SIZE;k++,j++) state->OutputAccum[j] = state->OutputAccum[k]; + for(;j < HIL_SIZE;j++) state->OutputAccum[j] = complex_zero; + for(k = 0;k < FIFO_LATENCY;k++) + state->InFIFO[k] = state->InFIFO[k+HIL_STEP]; + } + + /* Process frequency shifter using the analytic signal obtained. */ + for(k = 0;k < SamplesToDo;k++) + { + ALdouble phase = state->Phase * ((1.0/FRACTIONONE) * 2.0*M_PI); + BufferOut[k] = (ALfloat)(state->Outdata[k].Real*cos(phase) + + state->Outdata[k].Imag*sin(phase)*state->ld_sign); + + state->Phase += state->PhaseStep; + state->Phase &= FRACTIONMASK; + } + + /* Now, mix the processed sound data to the output. */ + MixSamples(BufferOut, NumChannels, SamplesOut, state->CurrentGains, state->TargetGains, + maxi(SamplesToDo, 512), 0, SamplesToDo); +} + +typedef struct FshifterStateFactory { + DERIVE_FROM_TYPE(EffectStateFactory); +} FshifterStateFactory; + +static ALeffectState *FshifterStateFactory_create(FshifterStateFactory *UNUSED(factory)) +{ + ALfshifterState *state; + + NEW_OBJ0(state, ALfshifterState)(); + if(!state) return NULL; + + return STATIC_CAST(ALeffectState, state); +} + +DEFINE_EFFECTSTATEFACTORY_VTABLE(FshifterStateFactory); + +EffectStateFactory *FshifterStateFactory_getFactory(void) +{ + static FshifterStateFactory FshifterFactory = { { GET_VTABLE2(FshifterStateFactory, EffectStateFactory) } }; + + return STATIC_CAST(EffectStateFactory, &FshifterFactory); +} + +void ALfshifter_setParamf(ALeffect *effect, ALCcontext *context, ALenum param, ALfloat val) +{ + ALeffectProps *props = &effect->Props; + switch(param) + { + case AL_FREQUENCY_SHIFTER_FREQUENCY: + if(!(val >= AL_FREQUENCY_SHIFTER_MIN_FREQUENCY && val <= AL_FREQUENCY_SHIFTER_MAX_FREQUENCY)) + SETERR_RETURN(context, AL_INVALID_VALUE,,"Frequency shifter frequency out of range"); + props->Fshifter.Frequency = val; + break; + + default: + alSetError(context, AL_INVALID_ENUM, "Invalid frequency shifter float property 0x%04x", param); + } +} + +void ALfshifter_setParamfv(ALeffect *effect, ALCcontext *context, ALenum param, const ALfloat *vals) +{ + ALfshifter_setParamf(effect, context, param, vals[0]); +} + +void ALfshifter_setParami(ALeffect *effect, ALCcontext *context, ALenum param, ALint val) +{ + ALeffectProps *props = &effect->Props; + switch(param) + { + case AL_FREQUENCY_SHIFTER_LEFT_DIRECTION: + if(!(val >= AL_FREQUENCY_SHIFTER_MIN_LEFT_DIRECTION && val <= AL_FREQUENCY_SHIFTER_MAX_LEFT_DIRECTION)) + SETERR_RETURN(context, AL_INVALID_VALUE,,"Frequency shifter left direction out of range"); + props->Fshifter.LeftDirection = val; + break; + + case AL_FREQUENCY_SHIFTER_RIGHT_DIRECTION: + if(!(val >= AL_FREQUENCY_SHIFTER_MIN_RIGHT_DIRECTION && val <= AL_FREQUENCY_SHIFTER_MAX_RIGHT_DIRECTION)) + SETERR_RETURN(context, AL_INVALID_VALUE,,"Frequency shifter right direction out of range"); + props->Fshifter.RightDirection = val; + break; + + default: + alSetError(context, AL_INVALID_ENUM, "Invalid frequency shifter integer property 0x%04x", param); + } +} +void ALfshifter_setParamiv(ALeffect *effect, ALCcontext *context, ALenum param, const ALint *vals) +{ + ALfshifter_setParami(effect, context, param, vals[0]); +} + +void ALfshifter_getParami(const ALeffect *effect, ALCcontext *context, ALenum param, ALint *val) +{ + const ALeffectProps *props = &effect->Props; + switch(param) + { + case AL_FREQUENCY_SHIFTER_LEFT_DIRECTION: + *val = props->Fshifter.LeftDirection; + break; + case AL_FREQUENCY_SHIFTER_RIGHT_DIRECTION: + *val = props->Fshifter.RightDirection; + break; + default: + alSetError(context, AL_INVALID_ENUM, "Invalid frequency shifter integer property 0x%04x", param); + } +} +void ALfshifter_getParamiv(const ALeffect *effect, ALCcontext *context, ALenum param, ALint *vals) +{ + ALfshifter_getParami(effect, context, param, vals); +} + +void ALfshifter_getParamf(const ALeffect *effect, ALCcontext *context, ALenum param, ALfloat *val) +{ + + const ALeffectProps *props = &effect->Props; + switch(param) + { + case AL_FREQUENCY_SHIFTER_FREQUENCY: + *val = props->Fshifter.Frequency; + break; + + default: + alSetError(context, AL_INVALID_ENUM, "Invalid frequency shifter float property 0x%04x", param); + } + +} + +void ALfshifter_getParamfv(const ALeffect *effect, ALCcontext *context, ALenum param, ALfloat *vals) +{ + ALfshifter_getParamf(effect, context, param, vals); +} + +DEFINE_ALEFFECT_VTABLE(ALfshifter); diff --git a/Engine/lib/openal-soft/Alc/effects/modulator.c b/Engine/lib/openal-soft/Alc/effects/modulator.c index 7f1a2cad0..e368adb8c 100644 --- a/Engine/lib/openal-soft/Alc/effects/modulator.c +++ b/Engine/lib/openal-soft/Alc/effects/modulator.c @@ -40,8 +40,6 @@ typedef struct ALmodulatorState { ALsizei index; ALsizei step; - alignas(16) ALfloat ModSamples[MAX_UPDATE_SAMPLES]; - struct { BiquadFilter Filter; @@ -65,17 +63,22 @@ DEFINE_ALEFFECTSTATE_VTABLE(ALmodulatorState); static inline ALfloat Sin(ALsizei index) { - return sinf(index*(F_TAU/WAVEFORM_FRACONE) - F_PI)*0.5f + 0.5f; + return sinf((ALfloat)index * (F_TAU / WAVEFORM_FRACONE)); } static inline ALfloat Saw(ALsizei index) { - return (ALfloat)index / WAVEFORM_FRACONE; + return (ALfloat)index*(2.0f/WAVEFORM_FRACONE) - 1.0f; } static inline ALfloat Square(ALsizei index) { - return (ALfloat)((index >> (WAVEFORM_FRACBITS - 1)) & 1); + return (ALfloat)(((index>>(WAVEFORM_FRACBITS-2))&2) - 1); +} + +static inline ALfloat One(ALsizei UNUSED(index)) +{ + return 1.0f; } #define DECL_TEMPLATE(func) \ @@ -94,6 +97,7 @@ static void Modulate##func(ALfloat *restrict dst, ALsizei index, \ DECL_TEMPLATE(Sin) DECL_TEMPLATE(Saw) DECL_TEMPLATE(Square) +DECL_TEMPLATE(One) #undef DECL_TEMPLATE @@ -127,47 +131,45 @@ static ALboolean ALmodulatorState_deviceUpdate(ALmodulatorState *state, ALCdevic static ALvoid ALmodulatorState_update(ALmodulatorState *state, const ALCcontext *context, const ALeffectslot *slot, const ALeffectProps *props) { const ALCdevice *device = context->Device; - ALfloat cw, a; + ALfloat f0norm; ALsizei i; - if(props->Modulator.Waveform == AL_RING_MODULATOR_SINUSOID) + state->step = fastf2i(props->Modulator.Frequency / (ALfloat)device->Frequency * + WAVEFORM_FRACONE); + state->step = clampi(state->step, 0, WAVEFORM_FRACONE-1); + + if(state->step == 0) + state->GetSamples = ModulateOne; + else if(props->Modulator.Waveform == AL_RING_MODULATOR_SINUSOID) state->GetSamples = ModulateSin; else if(props->Modulator.Waveform == AL_RING_MODULATOR_SAWTOOTH) state->GetSamples = ModulateSaw; else /*if(Slot->Params.EffectProps.Modulator.Waveform == AL_RING_MODULATOR_SQUARE)*/ state->GetSamples = ModulateSquare; - state->step = float2int(props->Modulator.Frequency*WAVEFORM_FRACONE/device->Frequency + 0.5f); - state->step = clampi(state->step, 1, WAVEFORM_FRACONE-1); - - /* Custom filter coeffs, which match the old version instead of a low-shelf. */ - cw = cosf(F_TAU * props->Modulator.HighPassCutoff / device->Frequency); - a = (2.0f-cw) - sqrtf(powf(2.0f-cw, 2.0f) - 1.0f); - - state->Chans[0].Filter.b0 = a; - state->Chans[0].Filter.b1 = -a; - state->Chans[0].Filter.b2 = 0.0f; - state->Chans[0].Filter.a1 = -a; - state->Chans[0].Filter.a2 = 0.0f; + f0norm = props->Modulator.HighPassCutoff / (ALfloat)device->Frequency; + f0norm = clampf(f0norm, 1.0f/512.0f, 0.49f); + /* Bandwidth value is constant in octaves. */ + BiquadFilter_setParams(&state->Chans[0].Filter, BiquadType_HighPass, 1.0f, + f0norm, calc_rcpQ_from_bandwidth(f0norm, 0.75f)); for(i = 1;i < MAX_EFFECT_CHANNELS;i++) BiquadFilter_copyParams(&state->Chans[i].Filter, &state->Chans[0].Filter); STATIC_CAST(ALeffectState,state)->OutBuffer = device->FOAOut.Buffer; STATIC_CAST(ALeffectState,state)->OutChannels = device->FOAOut.NumChannels; for(i = 0;i < MAX_EFFECT_CHANNELS;i++) - ComputeFirstOrderGains(&device->FOAOut, IdentityMatrixf.m[i], - slot->Params.Gain, state->Chans[i].TargetGains); + ComputePanGains(&device->FOAOut, IdentityMatrixf.m[i], slot->Params.Gain, + state->Chans[i].TargetGains); } static ALvoid ALmodulatorState_process(ALmodulatorState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels) { - ALfloat *restrict modsamples = ASSUME_ALIGNED(state->ModSamples, 16); const ALsizei step = state->step; ALsizei base; for(base = 0;base < SamplesToDo;) { - alignas(16) ALfloat temps[2][MAX_UPDATE_SAMPLES]; + alignas(16) ALfloat modsamples[MAX_UPDATE_SAMPLES]; ALsizei td = mini(MAX_UPDATE_SAMPLES, SamplesToDo-base); ALsizei c, i; @@ -177,11 +179,13 @@ static ALvoid ALmodulatorState_process(ALmodulatorState *state, ALsizei SamplesT for(c = 0;c < MAX_EFFECT_CHANNELS;c++) { - BiquadFilter_process(&state->Chans[c].Filter, temps[0], &SamplesIn[c][base], td); - for(i = 0;i < td;i++) - temps[1][i] = temps[0][i] * modsamples[i]; + alignas(16) ALfloat temps[MAX_UPDATE_SAMPLES]; - MixSamples(temps[1], NumChannels, SamplesOut, state->Chans[c].CurrentGains, + BiquadFilter_process(&state->Chans[c].Filter, temps, &SamplesIn[c][base], td); + for(i = 0;i < td;i++) + temps[i] *= modsamples[i]; + + MixSamples(temps, NumChannels, SamplesOut, state->Chans[c].CurrentGains, state->Chans[c].TargetGains, SamplesToDo-base, base, td); } diff --git a/Engine/lib/openal-soft/Alc/effects/pshifter.c b/Engine/lib/openal-soft/Alc/effects/pshifter.c index 618573431..ed18e9a85 100644 --- a/Engine/lib/openal-soft/Alc/effects/pshifter.c +++ b/Engine/lib/openal-soft/Alc/effects/pshifter.c @@ -29,6 +29,8 @@ #include "alu.h" #include "filters/defs.h" +#include "alcomplex.h" + #define STFT_SIZE 1024 #define STFT_HALF_SIZE (STFT_SIZE>>1) @@ -37,10 +39,6 @@ #define STFT_STEP (STFT_SIZE / OVERSAMP) #define FIFO_LATENCY (STFT_STEP * (OVERSAMP-1)) -typedef struct ALcomplex { - ALdouble Real; - ALdouble Imag; -} ALcomplex; typedef struct ALphasor { ALdouble Amplitude; @@ -52,6 +50,7 @@ typedef struct ALFrequencyDomain { ALdouble Frequency; } ALfrequencyDomain; + typedef struct ALpshifterState { DERIVE_FROM_TYPE(ALeffectState); @@ -106,26 +105,32 @@ static void InitHannWindow(void) static alonce_flag HannInitOnce = AL_ONCE_FLAG_INIT; -/* Fast double-to-int conversion. Assumes the FPU is already in round-to-zero - * mode. */ -static inline ALint fastd2i(ALdouble d) +static inline ALint double2int(ALdouble d) { - /* NOTE: SSE2 is required for the efficient double-to-int opcodes on x86. - * Otherwise, we need to rely on x87's fistp opcode with it already in - * round-to-zero mode. x86-64 guarantees SSE2 support. - */ -#if (defined(__i386__) && !defined(__SSE2_MATH__)) || (defined(_M_IX86_FP) && (_M_IX86_FP < 2)) -#ifdef HAVE_LRINTF - return lrint(d); -#elif defined(_MSC_VER) && defined(_M_IX86) - ALint i; - __asm fld d - __asm fistp i - return i; -#else - return (ALint)d; -#endif +#if ((defined(__GNUC__) || defined(__clang__)) && (defined(__i386__) || defined(__x86_64__)) && \ + !defined(__SSE2_MATH__)) || (defined(_MSC_VER) && defined(_M_IX86_FP) && _M_IX86_FP < 2) + ALint sign, shift; + ALint64 mant; + union { + ALdouble d; + ALint64 i64; + } conv; + + conv.d = d; + sign = (conv.i64>>63) | 1; + shift = ((conv.i64>>52)&0x7ff) - (1023+52); + + /* Over/underflow */ + if(UNLIKELY(shift >= 63 || shift < -52)) + return 0; + + mant = (conv.i64&I64(0xfffffffffffff)) | I64(0x10000000000000); + if(LIKELY(shift < 0)) + return (ALint)(mant >> -shift) * sign; + return (ALint)(mant << shift) * sign; + #else + return (ALint)d; #endif } @@ -143,7 +148,7 @@ static inline ALphasor rect2polar(ALcomplex number) } /* Converts ALphasor to ALcomplex */ -static inline ALcomplex polar2rect(ALphasor number) +static inline ALcomplex polar2rect(ALphasor number) { ALcomplex cartesian; @@ -153,96 +158,6 @@ static inline ALcomplex polar2rect(ALphasor number) return cartesian; } -/* Addition of two complex numbers (ALcomplex format) */ -static inline ALcomplex complex_add(ALcomplex a, ALcomplex b) -{ - ALcomplex result; - - result.Real = a.Real + b.Real; - result.Imag = a.Imag + b.Imag; - - return result; -} - -/* Subtraction of two complex numbers (ALcomplex format) */ -static inline ALcomplex complex_sub(ALcomplex a, ALcomplex b) -{ - ALcomplex result; - - result.Real = a.Real - b.Real; - result.Imag = a.Imag - b.Imag; - - return result; -} - -/* Multiplication of two complex numbers (ALcomplex format) */ -static inline ALcomplex complex_mult(ALcomplex a, ALcomplex b) -{ - ALcomplex result; - - result.Real = a.Real*b.Real - a.Imag*b.Imag; - result.Imag = a.Imag*b.Real + a.Real*b.Imag; - - return result; -} - -/* Iterative implementation of 2-radix FFT (In-place algorithm). Sign = -1 is - * FFT and 1 is iFFT (inverse). Fills FFTBuffer[0...FFTSize-1] with the - * Discrete Fourier Transform (DFT) of the time domain data stored in - * FFTBuffer[0...FFTSize-1]. FFTBuffer is an array of complex numbers - * (ALcomplex), FFTSize MUST BE power of two. - */ -static inline ALvoid FFT(ALcomplex *FFTBuffer, ALsizei FFTSize, ALdouble Sign) -{ - ALsizei i, j, k, mask, step, step2; - ALcomplex temp, u, w; - ALdouble arg; - - /* Bit-reversal permutation applied to a sequence of FFTSize items */ - for(i = 1;i < FFTSize-1;i++) - { - for(mask = 0x1, j = 0;mask < FFTSize;mask <<= 1) - { - if((i&mask) != 0) - j++; - j <<= 1; - } - j >>= 1; - - if(i < j) - { - temp = FFTBuffer[i]; - FFTBuffer[i] = FFTBuffer[j]; - FFTBuffer[j] = temp; - } - } - - /* Iterative form of Danielson–Lanczos lemma */ - for(i = 1, step = 2;i < FFTSize;i<<=1, step<<=1) - { - step2 = step >> 1; - arg = M_PI / step2; - - w.Real = cos(arg); - w.Imag = sin(arg) * Sign; - - u.Real = 1.0; - u.Imag = 0.0; - - for(j = 0;j < step2;j++) - { - for(k = j;k < FFTSize;k+=step) - { - temp = complex_mult(FFTBuffer[k+step2], u); - FFTBuffer[k+step2] = complex_sub(FFTBuffer[k], temp); - FFTBuffer[k] = complex_add(FFTBuffer[k], temp); - } - - u = complex_mult(u, w); - } - } -} - static void ALpshifterState_Construct(ALpshifterState *state) { @@ -289,11 +204,11 @@ static ALvoid ALpshifterState_update(ALpshifterState *state, const ALCcontext *c pitch = powf(2.0f, (ALfloat)(props->Pshifter.CoarseTune*100 + props->Pshifter.FineTune) / 1200.0f ); - state->PitchShiftI = (ALsizei)(pitch*FRACTIONONE + 0.5f); - state->PitchShift = state->PitchShiftI * (1.0f/FRACTIONONE); + state->PitchShiftI = fastf2i(pitch*FRACTIONONE); + state->PitchShift = state->PitchShiftI * (1.0f/FRACTIONONE); CalcAngleCoeffs(0.0f, 0.0f, 0.0f, coeffs); - ComputeDryPanGains(&device->Dry, coeffs, slot->Params.Gain, state->TargetGains); + ComputePanGains(&device->Dry, coeffs, slot->Params.Gain, state->TargetGains); } static ALvoid ALpshifterState_process(ALpshifterState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels) @@ -331,7 +246,7 @@ static ALvoid ALpshifterState_process(ALpshifterState *state, ALsizei SamplesToD /* ANALYSIS */ /* Apply FFT to FFTbuffer data */ - FFT(state->FFTbuffer, STFT_SIZE, -1.0); + complex_fft(state->FFTbuffer, STFT_SIZE, -1.0); /* Analyze the obtained data. Since the real FFT is symmetric, only * STFT_HALF_SIZE+1 samples are needed. @@ -349,7 +264,7 @@ static ALvoid ALpshifterState_process(ALpshifterState *state, ALsizei SamplesToD tmp = (component.Phase - state->LastPhase[k]) - k*expected; /* Map delta phase into +/- Pi interval */ - qpd = fastd2i(tmp / M_PI); + qpd = double2int(tmp / M_PI); tmp -= M_PI * (qpd + (qpd%2)); /* Get deviation from bin frequency from the +/- Pi interval */ @@ -411,7 +326,7 @@ static ALvoid ALpshifterState_process(ALpshifterState *state, ALsizei SamplesToD } /* Apply iFFT to buffer data */ - FFT(state->FFTbuffer, STFT_SIZE, 1.0); + complex_fft(state->FFTbuffer, STFT_SIZE, 1.0); /* Windowing and add to output */ for(k = 0;k < STFT_SIZE;k++) diff --git a/Engine/lib/openal-soft/Alc/effects/reverb.c b/Engine/lib/openal-soft/Alc/effects/reverb.c index 12e78bdfc..8ebc089ea 100644 --- a/Engine/lib/openal-soft/Alc/effects/reverb.c +++ b/Engine/lib/openal-soft/Alc/effects/reverb.c @@ -223,20 +223,16 @@ typedef struct DelayLineI { typedef struct VecAllpass { DelayLineI Delay; + ALfloat Coeff; ALsizei Offset[NUM_LINES][2]; } VecAllpass; typedef struct T60Filter { /* Two filters are used to adjust the signal. One to control the low - * frequencies, and one to control the high frequencies. The HF filter also - * adjusts the overall output gain, affecting the remaining mid-band. + * frequencies, and one to control the high frequencies. */ - ALfloat HFCoeffs[3]; - ALfloat LFCoeffs[3]; - - /* The HF and LF filters each keep a delay component. */ - ALfloat HFState; - ALfloat LFState; + ALfloat MidGain[2]; + BiquadFilter HFFilter, LFFilter; } T60Filter; typedef struct EarlyReflections { @@ -250,7 +246,7 @@ typedef struct EarlyReflections { */ DelayLineI Delay; ALsizei Offset[NUM_LINES][2]; - ALfloat Coeff[NUM_LINES]; + ALfloat Coeff[NUM_LINES][2]; /* The gain for each output channel based on 3D panning. */ ALfloat CurrentGain[NUM_LINES][MAX_OUTPUT_CHANNELS]; @@ -258,15 +254,15 @@ typedef struct EarlyReflections { } EarlyReflections; typedef struct LateReverb { - /* Attenuation to compensate for the modal density and decay rate of the - * late lines. - */ - ALfloat DensityGain; - /* A recursive delay line is used fill in the reverb tail. */ DelayLineI Delay; ALsizei Offset[NUM_LINES][2]; + /* Attenuation to compensate for the modal density and decay rate of the + * late lines. + */ + ALfloat DensityGain[2]; + /* T60 decay filters are used to simulate absorption. */ T60Filter T60[NUM_LINES]; @@ -278,7 +274,7 @@ typedef struct LateReverb { ALfloat PanGain[NUM_LINES][MAX_OUTPUT_CHANNELS]; } LateReverb; -typedef struct ALreverbState { +typedef struct ReverbState { DERIVE_FROM_TYPE(ALeffectState); /* All delay lines are allocated as a single buffer to reduce memory @@ -287,6 +283,15 @@ typedef struct ALreverbState { ALfloat *SampleBuffer; ALuint TotalSamples; + struct { + /* Calculated parameters which indicate if cross-fading is needed after + * an update. + */ + ALfloat Density, Diffusion; + ALfloat DecayTime, HFDecayTime, LFDecayTime; + ALfloat HFReference, LFReference; + } Params; + /* Master effect filters */ struct { BiquadFilter Lp; @@ -298,15 +303,12 @@ typedef struct ALreverbState { /* Tap points for early reflection delay. */ ALsizei EarlyDelayTap[NUM_LINES][2]; - ALfloat EarlyDelayCoeff[NUM_LINES]; + ALfloat EarlyDelayCoeff[NUM_LINES][2]; /* Tap points for late reverb feed and delay. */ ALsizei LateFeedTap; ALsizei LateDelayTap[NUM_LINES][2]; - /* The feed-back and feed-forward all-pass coefficient. */ - ALfloat ApFeedCoeff; - /* Coefficients for the all-pass and line scattering matrices. */ ALfloat MixX; ALfloat MixY; @@ -318,33 +320,43 @@ typedef struct ALreverbState { /* Indicates the cross-fade point for delay line reads [0,FADE_SAMPLES]. */ ALsizei FadeCount; + /* Maximum number of samples to process at once. */ + ALsizei MaxUpdate[2]; + /* The current write offset for all delay lines. */ ALsizei Offset; /* Temporary storage used when processing. */ - alignas(16) ALfloat AFormatSamples[NUM_LINES][MAX_UPDATE_SAMPLES]; - alignas(16) ALfloat ReverbSamples[NUM_LINES][MAX_UPDATE_SAMPLES]; - alignas(16) ALfloat EarlySamples[NUM_LINES][MAX_UPDATE_SAMPLES]; -} ALreverbState; + alignas(16) ALfloat TempSamples[NUM_LINES][MAX_UPDATE_SAMPLES]; + alignas(16) ALfloat MixSamples[NUM_LINES][MAX_UPDATE_SAMPLES]; +} ReverbState; -static ALvoid ALreverbState_Destruct(ALreverbState *State); -static ALboolean ALreverbState_deviceUpdate(ALreverbState *State, ALCdevice *Device); -static ALvoid ALreverbState_update(ALreverbState *State, const ALCcontext *Context, const ALeffectslot *Slot, const ALeffectProps *props); -static ALvoid ALreverbState_process(ALreverbState *State, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels); -DECLARE_DEFAULT_ALLOCATORS(ALreverbState) +static ALvoid ReverbState_Destruct(ReverbState *State); +static ALboolean ReverbState_deviceUpdate(ReverbState *State, ALCdevice *Device); +static ALvoid ReverbState_update(ReverbState *State, const ALCcontext *Context, const ALeffectslot *Slot, const ALeffectProps *props); +static ALvoid ReverbState_process(ReverbState *State, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels); +DECLARE_DEFAULT_ALLOCATORS(ReverbState) -DEFINE_ALEFFECTSTATE_VTABLE(ALreverbState); +DEFINE_ALEFFECTSTATE_VTABLE(ReverbState); -static void ALreverbState_Construct(ALreverbState *state) +static void ReverbState_Construct(ReverbState *state) { ALsizei i, j; ALeffectState_Construct(STATIC_CAST(ALeffectState, state)); - SET_VTABLE2(ALreverbState, ALeffectState, state); + SET_VTABLE2(ReverbState, ALeffectState, state); state->TotalSamples = 0; state->SampleBuffer = NULL; + state->Params.Density = AL_EAXREVERB_DEFAULT_DENSITY; + state->Params.Diffusion = AL_EAXREVERB_DEFAULT_DIFFUSION; + state->Params.DecayTime = AL_EAXREVERB_DEFAULT_DECAY_TIME; + state->Params.HFDecayTime = AL_EAXREVERB_DEFAULT_DECAY_TIME*AL_EAXREVERB_DEFAULT_DECAY_HFRATIO; + state->Params.LFDecayTime = AL_EAXREVERB_DEFAULT_DECAY_TIME*AL_EAXREVERB_DEFAULT_DECAY_LFRATIO; + state->Params.HFReference = AL_EAXREVERB_DEFAULT_HFREFERENCE; + state->Params.LFReference = AL_EAXREVERB_DEFAULT_LFREFERENCE; + for(i = 0;i < NUM_LINES;i++) { BiquadFilter_clear(&state->Filter[i].Lp); @@ -358,7 +370,8 @@ static void ALreverbState_Construct(ALreverbState *state) { state->EarlyDelayTap[i][0] = 0; state->EarlyDelayTap[i][1] = 0; - state->EarlyDelayCoeff[i] = 0.0f; + state->EarlyDelayCoeff[i][0] = 0.0f; + state->EarlyDelayCoeff[i][1] = 0.0f; } state->LateFeedTap = 0; @@ -369,12 +382,12 @@ static void ALreverbState_Construct(ALreverbState *state) state->LateDelayTap[i][1] = 0; } - state->ApFeedCoeff = 0.0f; state->MixX = 0.0f; state->MixY = 0.0f; state->Early.VecAp.Delay.Mask = 0; state->Early.VecAp.Delay.Line = NULL; + state->Early.VecAp.Coeff = 0.0f; state->Early.Delay.Mask = 0; state->Early.Delay.Line = NULL; for(i = 0;i < NUM_LINES;i++) @@ -383,15 +396,17 @@ static void ALreverbState_Construct(ALreverbState *state) state->Early.VecAp.Offset[i][1] = 0; state->Early.Offset[i][0] = 0; state->Early.Offset[i][1] = 0; - state->Early.Coeff[i] = 0.0f; + state->Early.Coeff[i][0] = 0.0f; + state->Early.Coeff[i][1] = 0.0f; } - state->Late.DensityGain = 0.0f; - + state->Late.DensityGain[0] = 0.0f; + state->Late.DensityGain[1] = 0.0f; state->Late.Delay.Mask = 0; state->Late.Delay.Line = NULL; state->Late.VecAp.Delay.Mask = 0; state->Late.VecAp.Delay.Line = NULL; + state->Late.VecAp.Coeff = 0.0f; for(i = 0;i < NUM_LINES;i++) { state->Late.Offset[i][0] = 0; @@ -400,13 +415,10 @@ static void ALreverbState_Construct(ALreverbState *state) state->Late.VecAp.Offset[i][0] = 0; state->Late.VecAp.Offset[i][1] = 0; - for(j = 0;j < 3;j++) - { - state->Late.T60[i].HFCoeffs[j] = 0.0f; - state->Late.T60[i].LFCoeffs[j] = 0.0f; - } - state->Late.T60[i].HFState = 0.0f; - state->Late.T60[i].LFState = 0.0f; + state->Late.T60[i].MidGain[0] = 0.0f; + state->Late.T60[i].MidGain[1] = 0.0f; + BiquadFilter_clear(&state->Late.T60[i].HFFilter); + BiquadFilter_clear(&state->Late.T60[i].LFFilter); } for(i = 0;i < NUM_LINES;i++) @@ -421,10 +433,12 @@ static void ALreverbState_Construct(ALreverbState *state) } state->FadeCount = 0; + state->MaxUpdate[0] = MAX_UPDATE_SAMPLES; + state->MaxUpdate[1] = MAX_UPDATE_SAMPLES; state->Offset = 0; } -static ALvoid ALreverbState_Destruct(ALreverbState *State) +static ALvoid ReverbState_Destruct(ReverbState *State) { al_free(State->SampleBuffer); State->SampleBuffer = NULL; @@ -478,7 +492,7 @@ static ALuint CalcLineLength(const ALfloat length, const ptrdiff_t offset, const * for all lines given the sample rate (frequency). If an allocation failure * occurs, it returns AL_FALSE. */ -static ALboolean AllocLines(const ALuint frequency, ALreverbState *State) +static ALboolean AllocLines(const ALuint frequency, ReverbState *State) { ALuint totalSamples, i; ALfloat multiplier, length; @@ -519,10 +533,10 @@ static ALboolean AllocLines(const ALuint frequency, ALreverbState *State) totalSamples += CalcLineLength(length, totalSamples, frequency, 0, &State->Late.VecAp.Delay); - /* The late delay lines are calculated from the larger of the maximum - * density line length or the maximum echo time. + /* The late delay lines are calculated from the largest maximum density + * line length. */ - length = maxf(AL_EAXREVERB_MAX_ECHO_TIME, LATE_LINE_LENGTHS[NUM_LINES-1]*multiplier); + length = LATE_LINE_LENGTHS[NUM_LINES-1] * multiplier; totalSamples += CalcLineLength(length, totalSamples, frequency, 0, &State->Late.Delay); @@ -553,10 +567,11 @@ static ALboolean AllocLines(const ALuint frequency, ALreverbState *State) return AL_TRUE; } -static ALboolean ALreverbState_deviceUpdate(ALreverbState *State, ALCdevice *Device) +static ALboolean ReverbState_deviceUpdate(ReverbState *State, ALCdevice *Device) { ALuint frequency = Device->Frequency; ALfloat multiplier; + ALsizei i, j; /* Allocate the delay lines. */ if(!AllocLines(frequency, State)) @@ -569,6 +584,54 @@ static ALboolean ALreverbState_deviceUpdate(ALreverbState *State, ALCdevice *Dev EARLY_TAP_LENGTHS[NUM_LINES-1]*multiplier) * frequency); + /* Clear filters and gain coefficients since the delay lines were all just + * cleared (if not reallocated). + */ + for(i = 0;i < NUM_LINES;i++) + { + BiquadFilter_clear(&State->Filter[i].Lp); + BiquadFilter_clear(&State->Filter[i].Hp); + } + + for(i = 0;i < NUM_LINES;i++) + { + State->EarlyDelayCoeff[i][0] = 0.0f; + State->EarlyDelayCoeff[i][1] = 0.0f; + } + + for(i = 0;i < NUM_LINES;i++) + { + State->Early.Coeff[i][0] = 0.0f; + State->Early.Coeff[i][1] = 0.0f; + } + + State->Late.DensityGain[0] = 0.0f; + State->Late.DensityGain[1] = 0.0f; + for(i = 0;i < NUM_LINES;i++) + { + State->Late.T60[i].MidGain[0] = 0.0f; + State->Late.T60[i].MidGain[1] = 0.0f; + BiquadFilter_clear(&State->Late.T60[i].HFFilter); + BiquadFilter_clear(&State->Late.T60[i].LFFilter); + } + + for(i = 0;i < NUM_LINES;i++) + { + for(j = 0;j < MAX_OUTPUT_CHANNELS;j++) + { + State->Early.CurrentGain[i][j] = 0.0f; + State->Early.PanGain[i][j] = 0.0f; + State->Late.CurrentGain[i][j] = 0.0f; + State->Late.PanGain[i][j] = 0.0f; + } + } + + /* Reset counters and offset base. */ + State->FadeCount = 0; + State->MaxUpdate[0] = MAX_UPDATE_SAMPLES; + State->MaxUpdate[1] = MAX_UPDATE_SAMPLES; + State->Offset = 0; + return AL_TRUE; } @@ -648,288 +711,29 @@ static ALfloat CalcLimitedHfRatio(const ALfloat hfRatio, const ALfloat airAbsorp return minf(limitRatio, hfRatio); } -/* Calculates the first-order high-pass coefficients following the I3DL2 - * reference model. This is the transfer function: - * - * 1 - z^-1 - * H(z) = p ------------ - * 1 - p z^-1 - * - * And this is the I3DL2 coefficient calculation given gain (g) and reference - * angular frequency (w): - * - * g - * p = ------------------------------------------------------ - * g cos(w) + sqrt((cos(w) - 1) (g^2 cos(w) + g^2 - 2)) - * - * The coefficient is applied to the partial differential filter equation as: - * - * c_0 = p - * c_1 = -p - * c_2 = p - * y_i = c_0 x_i + c_1 x_(i-1) + c_2 y_(i-1) - * - */ -static inline void CalcHighpassCoeffs(const ALfloat gain, const ALfloat w, ALfloat coeffs[3]) -{ - ALfloat g, g2, cw, p; - - if(gain >= 1.0f) - { - coeffs[0] = 1.0f; - coeffs[1] = 0.0f; - coeffs[2] = 0.0f; - return; - } - - g = maxf(0.001f, gain); - g2 = g * g; - cw = cosf(w); - p = g / (g*cw + sqrtf((cw - 1.0f) * (g2*cw + g2 - 2.0f))); - - coeffs[0] = p; - coeffs[1] = -p; - coeffs[2] = p; -} - -/* Calculates the first-order low-pass coefficients following the I3DL2 - * reference model. This is the transfer function: - * - * (1 - a) z^0 - * H(z) = ---------------- - * 1 z^0 - a z^-1 - * - * And this is the I3DL2 coefficient calculation given gain (g) and reference - * angular frequency (w): - * - * 1 - g^2 cos(w) - sqrt(2 g^2 (1 - cos(w)) - g^4 (1 - cos(w)^2)) - * a = ---------------------------------------------------------------- - * 1 - g^2 - * - * The coefficient is applied to the partial differential filter equation as: - * - * c_0 = 1 - a - * c_1 = 0 - * c_2 = a - * y_i = c_0 x_i + c_1 x_(i-1) + c_2 y_(i-1) - * - */ -static inline void CalcLowpassCoeffs(const ALfloat gain, const ALfloat w, ALfloat coeffs[3]) -{ - ALfloat g, g2, cw, a; - - if(gain >= 1.0f) - { - coeffs[0] = 1.0f; - coeffs[1] = 0.0f; - coeffs[2] = 0.0f; - return; - } - - /* Be careful with gains < 0.001, as that causes the coefficient - * to head towards 1, which will flatten the signal. */ - g = maxf(0.001f, gain); - g2 = g * g; - cw = cosf(w); - a = (1.0f - g2*cw - sqrtf((2.0f*g2*(1.0f - cw)) - g2*g2*(1.0f - cw*cw))) / - (1.0f - g2); - - coeffs[0] = 1.0f - a; - coeffs[1] = 0.0f; - coeffs[2] = a; -} - -/* Calculates the first-order low-shelf coefficients. The shelf filters are - * used in place of low/high-pass filters to preserve the mid-band. This is - * the transfer function: - * - * a_0 + a_1 z^-1 - * H(z) = ---------------- - * 1 + b_1 z^-1 - * - * And these are the coefficient calculations given cut gain (g) and a center - * angular frequency (w): - * - * sin(0.5 (pi - w) - 0.25 pi) - * p = ----------------------------- - * sin(0.5 (pi - w) + 0.25 pi) - * - * g + 1 g + 1 - * a = ------- + sqrt((-------)^2 - 1) - * g - 1 g - 1 - * - * 1 + g + (1 - g) a - * b_0 = ------------------- - * 2 - * - * 1 - g + (1 + g) a - * b_1 = ------------------- - * 2 - * - * The coefficients are applied to the partial differential filter equation - * as: - * - * b_0 + p b_1 - * c_0 = ------------- - * 1 + p a - * - * -(b_1 + p b_0) - * c_1 = ---------------- - * 1 + p a - * - * p + a - * c_2 = --------- - * 1 + p a - * - * y_i = c_0 x_i + c_1 x_(i-1) + c_2 y_(i-1) - * - */ -static inline void CalcLowShelfCoeffs(const ALfloat gain, const ALfloat w, ALfloat coeffs[3]) -{ - ALfloat g, rw, p, n; - ALfloat alpha, beta0, beta1; - - if(gain >= 1.0f) - { - coeffs[0] = 1.0f; - coeffs[1] = 0.0f; - coeffs[2] = 0.0f; - return; - } - - g = maxf(0.001f, gain); - rw = F_PI - w; - p = sinf(0.5f*rw - 0.25f*F_PI) / sinf(0.5f*rw + 0.25f*F_PI); - n = (g + 1.0f) / (g - 1.0f); - alpha = n + sqrtf(n*n - 1.0f); - beta0 = (1.0f + g + (1.0f - g)*alpha) / 2.0f; - beta1 = (1.0f - g + (1.0f + g)*alpha) / 2.0f; - - coeffs[0] = (beta0 + p*beta1) / (1.0f + p*alpha); - coeffs[1] = -(beta1 + p*beta0) / (1.0f + p*alpha); - coeffs[2] = (p + alpha) / (1.0f + p*alpha); -} - -/* Calculates the first-order high-shelf coefficients. The shelf filters are - * used in place of low/high-pass filters to preserve the mid-band. This is - * the transfer function: - * - * a_0 + a_1 z^-1 - * H(z) = ---------------- - * 1 + b_1 z^-1 - * - * And these are the coefficient calculations given cut gain (g) and a center - * angular frequency (w): - * - * sin(0.5 w - 0.25 pi) - * p = ---------------------- - * sin(0.5 w + 0.25 pi) - * - * g + 1 g + 1 - * a = ------- + sqrt((-------)^2 - 1) - * g - 1 g - 1 - * - * 1 + g + (1 - g) a - * b_0 = ------------------- - * 2 - * - * 1 - g + (1 + g) a - * b_1 = ------------------- - * 2 - * - * The coefficients are applied to the partial differential filter equation - * as: - * - * b_0 + p b_1 - * c_0 = ------------- - * 1 + p a - * - * b_1 + p b_0 - * c_1 = ------------- - * 1 + p a - * - * -(p + a) - * c_2 = ---------- - * 1 + p a - * - * y_i = c_0 x_i + c_1 x_(i-1) + c_2 y_(i-1) - * - */ -static inline void CalcHighShelfCoeffs(const ALfloat gain, const ALfloat w, ALfloat coeffs[3]) -{ - ALfloat g, p, n; - ALfloat alpha, beta0, beta1; - - if(gain >= 1.0f) - { - coeffs[0] = 1.0f; - coeffs[1] = 0.0f; - coeffs[2] = 0.0f; - return; - } - - g = maxf(0.001f, gain); - p = sinf(0.5f*w - 0.25f*F_PI) / sinf(0.5f*w + 0.25f*F_PI); - n = (g + 1.0f) / (g - 1.0f); - alpha = n + sqrtf(n*n - 1.0f); - beta0 = (1.0f + g + (1.0f - g)*alpha) / 2.0f; - beta1 = (1.0f - g + (1.0f + g)*alpha) / 2.0f; - - coeffs[0] = (beta0 + p*beta1) / (1.0f + p*alpha); - coeffs[1] = (beta1 + p*beta0) / (1.0f + p*alpha); - coeffs[2] = -(p + alpha) / (1.0f + p*alpha); -} /* Calculates the 3-band T60 damping coefficients for a particular delay line - * of specified length using a combination of two low/high-pass/shelf or - * pass-through filter sections (producing 3 coefficients each) given decay - * times for each band split at two (LF/HF) reference frequencies (w). + * of specified length, using a combination of two shelf filter sections given + * decay times for each band split at two reference frequencies. */ static void CalcT60DampingCoeffs(const ALfloat length, const ALfloat lfDecayTime, const ALfloat mfDecayTime, const ALfloat hfDecayTime, - const ALfloat lfW, const ALfloat hfW, ALfloat lfcoeffs[3], - ALfloat hfcoeffs[3]) + const ALfloat lf0norm, const ALfloat hf0norm, + T60Filter *filter) { ALfloat lfGain = CalcDecayCoeff(length, lfDecayTime); ALfloat mfGain = CalcDecayCoeff(length, mfDecayTime); ALfloat hfGain = CalcDecayCoeff(length, hfDecayTime); - if(lfGain <= mfGain) - { - CalcHighpassCoeffs(lfGain / mfGain, lfW, lfcoeffs); - if(mfGain >= hfGain) - { - CalcLowpassCoeffs(hfGain / mfGain, hfW, hfcoeffs); - hfcoeffs[0] *= mfGain; hfcoeffs[1] *= mfGain; - } - else - { - CalcLowShelfCoeffs(mfGain / hfGain, hfW, hfcoeffs); - hfcoeffs[0] *= hfGain; hfcoeffs[1] *= hfGain; - } - } - else - { - CalcHighShelfCoeffs(mfGain / lfGain, lfW, lfcoeffs); - if(mfGain >= hfGain) - { - CalcLowpassCoeffs(hfGain / mfGain, hfW, hfcoeffs); - hfcoeffs[0] *= lfGain; hfcoeffs[1] *= lfGain; - } - else - { - ALfloat hg = mfGain / lfGain; - ALfloat lg = mfGain / hfGain; - ALfloat mg = maxf(lfGain, hfGain) / maxf(hg, lg); - - CalcLowShelfCoeffs(lg, hfW, hfcoeffs); - hfcoeffs[0] *= mg; hfcoeffs[1] *= mg; - } - } + filter->MidGain[1] = mfGain; + BiquadFilter_setParams(&filter->LFFilter, BiquadType_LowShelf, lfGain/mfGain, lf0norm, + calc_rcpQ_from_slope(lfGain/mfGain, 1.0f)); + BiquadFilter_setParams(&filter->HFFilter, BiquadType_HighShelf, hfGain/mfGain, hf0norm, + calc_rcpQ_from_slope(hfGain/mfGain, 1.0f)); } /* Update the offsets for the main effect delay line. */ -static ALvoid UpdateDelayLine(const ALfloat earlyDelay, const ALfloat lateDelay, const ALfloat density, const ALfloat decayTime, const ALuint frequency, ALreverbState *State) +static ALvoid UpdateDelayLine(const ALfloat earlyDelay, const ALfloat lateDelay, const ALfloat density, const ALfloat decayTime, const ALuint frequency, ReverbState *State) { ALfloat multiplier, length; ALuint i; @@ -952,7 +756,7 @@ static ALvoid UpdateDelayLine(const ALfloat earlyDelay, const ALfloat lateDelay, State->EarlyDelayTap[i][1] = float2int(length * frequency); length = EARLY_TAP_LENGTHS[i]*multiplier; - State->EarlyDelayCoeff[i] = CalcDecayCoeff(length, decayTime); + State->EarlyDelayCoeff[i][1] = CalcDecayCoeff(length, decayTime); length = lateDelay + (LATE_LINE_LENGTHS[i] - LATE_LINE_LENGTHS[0])*0.25f*multiplier; State->LateDelayTap[i][1] = State->LateFeedTap + float2int(length * frequency); @@ -960,13 +764,16 @@ static ALvoid UpdateDelayLine(const ALfloat earlyDelay, const ALfloat lateDelay, } /* Update the early reflection line lengths and gain coefficients. */ -static ALvoid UpdateEarlyLines(const ALfloat density, const ALfloat decayTime, const ALuint frequency, EarlyReflections *Early) +static ALvoid UpdateEarlyLines(const ALfloat density, const ALfloat diffusion, const ALfloat decayTime, const ALuint frequency, EarlyReflections *Early) { ALfloat multiplier, length; ALsizei i; multiplier = CalcDelayLengthMult(density); + /* Calculate the all-pass feed-back/forward coefficient. */ + Early->VecAp.Coeff = sqrtf(0.5f) * powf(diffusion, 2.0f); + for(i = 0;i < NUM_LINES;i++) { /* Calculate the length (in seconds) of each all-pass line. */ @@ -982,13 +789,17 @@ static ALvoid UpdateEarlyLines(const ALfloat density, const ALfloat decayTime, c Early->Offset[i][1] = float2int(length * frequency); /* Calculate the gain (coefficient) for each line. */ - Early->Coeff[i] = CalcDecayCoeff(length, decayTime); + Early->Coeff[i][1] = CalcDecayCoeff(length, decayTime); } } /* Update the late reverb line lengths and T60 coefficients. */ -static ALvoid UpdateLateLines(const ALfloat density, const ALfloat diffusion, const ALfloat lfDecayTime, const ALfloat mfDecayTime, const ALfloat hfDecayTime, const ALfloat lfW, const ALfloat hfW, const ALfloat echoTime, const ALfloat echoDepth, const ALuint frequency, LateReverb *Late) +static ALvoid UpdateLateLines(const ALfloat density, const ALfloat diffusion, const ALfloat lfDecayTime, const ALfloat mfDecayTime, const ALfloat hfDecayTime, const ALfloat lf0norm, const ALfloat hf0norm, const ALuint frequency, LateReverb *Late) { + /* Scaling factor to convert the normalized reference frequencies from + * representing 0...freq to 0...max_reference. + */ + const ALfloat norm_weight_factor = (ALfloat)frequency / AL_EAXREVERB_MAX_HFREFERENCE; ALfloat multiplier, length, bandWeights[3]; ALsizei i; @@ -1003,23 +814,24 @@ static ALvoid UpdateLateLines(const ALfloat density, const ALfloat diffusion, co multiplier = CalcDelayLengthMult(density); length = (LATE_LINE_LENGTHS[0] + LATE_LINE_LENGTHS[1] + LATE_LINE_LENGTHS[2] + LATE_LINE_LENGTHS[3]) / 4.0f * multiplier; - /* Include the echo transformation (see below). */ - length = lerp(length, echoTime, echoDepth); length += (LATE_ALLPASS_LENGTHS[0] + LATE_ALLPASS_LENGTHS[1] + LATE_ALLPASS_LENGTHS[2] + LATE_ALLPASS_LENGTHS[3]) / 4.0f * multiplier; /* The density gain calculation uses an average decay time weighted by - * approximate bandwidth. This attempts to compensate for losses of - * energy that reduce decay time due to scattering into highly attenuated - * bands. + * approximate bandwidth. This attempts to compensate for losses of energy + * that reduce decay time due to scattering into highly attenuated bands. */ - bandWeights[0] = lfW; - bandWeights[1] = hfW - lfW; - bandWeights[2] = F_TAU - hfW; - Late->DensityGain = CalcDensityGain( - CalcDecayCoeff(length, (bandWeights[0]*lfDecayTime + bandWeights[1]*mfDecayTime + - bandWeights[2]*hfDecayTime) / F_TAU) + bandWeights[0] = lf0norm*norm_weight_factor; + bandWeights[1] = hf0norm*norm_weight_factor - lf0norm*norm_weight_factor; + bandWeights[2] = 1.0f - hf0norm*norm_weight_factor; + Late->DensityGain[1] = CalcDensityGain( + CalcDecayCoeff(length, + bandWeights[0]*lfDecayTime + bandWeights[1]*mfDecayTime + bandWeights[2]*hfDecayTime + ) ); + /* Calculate the all-pass feed-back/forward coefficient. */ + Late->VecAp.Coeff = sqrtf(0.5f) * powf(diffusion, 2.0f); + for(i = 0;i < NUM_LINES;i++) { /* Calculate the length (in seconds) of each all-pass line. */ @@ -1028,12 +840,8 @@ static ALvoid UpdateLateLines(const ALfloat density, const ALfloat diffusion, co /* Calculate the delay offset for each all-pass line. */ Late->VecAp.Offset[i][1] = float2int(length * frequency); - /* Calculate the length (in seconds) of each delay line. This also - * applies the echo transformation. As the EAX echo depth approaches - * 1, the line lengths approach a length equal to the echoTime. This - * helps to produce distinct echoes along the tail. - */ - length = lerp(LATE_LINE_LENGTHS[i] * multiplier, echoTime, echoDepth); + /* Calculate the length (in seconds) of each delay line. */ + length = LATE_LINE_LENGTHS[i] * multiplier; /* Calculate the delay offset for each delay line. */ Late->Offset[i][1] = float2int(length*frequency + 0.5f); @@ -1049,8 +857,7 @@ static ALvoid UpdateLateLines(const ALfloat density, const ALfloat diffusion, co /* Calculate the T60 damping coefficients for each line. */ CalcT60DampingCoeffs(length, lfDecayTime, mfDecayTime, hfDecayTime, - lfW, hfW, Late->T60[i].LFCoeffs, - Late->T60[i].HFCoeffs); + lf0norm, hf0norm, &Late->T60[i]); } } @@ -1061,7 +868,6 @@ static ALvoid UpdateLateLines(const ALfloat density, const ALfloat diffusion, co */ static aluMatrixf GetTransformFromVector(const ALfloat *vec) { - const ALfloat sqrt_3 = 1.732050808f; aluMatrixf focus; ALfloat norm[3]; ALfloat mag; @@ -1076,9 +882,9 @@ static aluMatrixf GetTransformFromVector(const ALfloat *vec) mag = sqrtf(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]); if(mag > 1.0f) { - norm[0] = vec[0] / mag * -sqrt_3; - norm[1] = vec[1] / mag * sqrt_3; - norm[2] = vec[2] / mag * sqrt_3; + norm[0] = vec[0] / mag * -SQRTF_3; + norm[1] = vec[1] / mag * SQRTF_3; + norm[2] = vec[2] / mag * SQRTF_3; mag = 1.0f; } else @@ -1087,9 +893,9 @@ static aluMatrixf GetTransformFromVector(const ALfloat *vec) * term. There's no need to renormalize the magnitude since it would * just be reapplied in the matrix. */ - norm[0] = vec[0] * -sqrt_3; - norm[1] = vec[1] * sqrt_3; - norm[2] = vec[2] * sqrt_3; + norm[0] = vec[0] * -SQRTF_3; + norm[1] = vec[1] * SQRTF_3; + norm[2] = vec[2] * SQRTF_3; } aluMatrixfSet(&focus, @@ -1103,7 +909,7 @@ static aluMatrixf GetTransformFromVector(const ALfloat *vec) } /* Update the early and late 3D panning gains. */ -static ALvoid Update3DPanning(const ALCdevice *Device, const ALfloat *ReflectionsPan, const ALfloat *LateReverbPan, const ALfloat gain, const ALfloat earlyGain, const ALfloat lateGain, ALreverbState *State) +static ALvoid Update3DPanning(const ALCdevice *Device, const ALfloat *ReflectionsPan, const ALfloat *LateReverbPan, const ALfloat earlyGain, const ALfloat lateGain, ReverbState *State) { aluMatrixf transform, rot; ALsizei i; @@ -1128,19 +934,19 @@ static ALvoid Update3DPanning(const ALCdevice *Device, const ALfloat *Reflection MATRIX_MULT(transform, rot, A2B); memset(&State->Early.PanGain, 0, sizeof(State->Early.PanGain)); for(i = 0;i < MAX_EFFECT_CHANNELS;i++) - ComputeFirstOrderGains(&Device->FOAOut, transform.m[i], gain*earlyGain, - State->Early.PanGain[i]); + ComputePanGains(&Device->FOAOut, transform.m[i], earlyGain, + State->Early.PanGain[i]); rot = GetTransformFromVector(LateReverbPan); MATRIX_MULT(transform, rot, A2B); memset(&State->Late.PanGain, 0, sizeof(State->Late.PanGain)); for(i = 0;i < MAX_EFFECT_CHANNELS;i++) - ComputeFirstOrderGains(&Device->FOAOut, transform.m[i], gain*lateGain, - State->Late.PanGain[i]); + ComputePanGains(&Device->FOAOut, transform.m[i], lateGain, + State->Late.PanGain[i]); #undef MATRIX_MULT } -static ALvoid ALreverbState_update(ALreverbState *State, const ALCcontext *Context, const ALeffectslot *Slot, const ALeffectProps *props) +static void ReverbState_update(ReverbState *State, const ALCcontext *Context, const ALeffectslot *Slot, const ALeffectProps *props) { const ALCdevice *Device = Context->Device; const ALlistener *Listener = Context->Listener; @@ -1151,14 +957,14 @@ static ALvoid ALreverbState_update(ALreverbState *State, const ALCcontext *Conte ALsizei i; /* Calculate the master filters */ - hf0norm = props->Reverb.HFReference / frequency; + hf0norm = minf(props->Reverb.HFReference / frequency, 0.49f); /* Restrict the filter gains from going below -60dB to keep the filter from * killing most of the signal. */ gainhf = maxf(props->Reverb.GainHF, 0.001f); BiquadFilter_setParams(&State->Filter[0].Lp, BiquadType_HighShelf, gainhf, hf0norm, calc_rcpQ_from_slope(gainhf, 1.0f)); - lf0norm = props->Reverb.LFReference / frequency; + lf0norm = minf(props->Reverb.LFReference / frequency, 0.49f); gainlf = maxf(props->Reverb.GainLF, 0.001f); BiquadFilter_setParams(&State->Filter[0].Hp, BiquadType_LowShelf, gainlf, lf0norm, calc_rcpQ_from_slope(gainlf, 1.0f)); @@ -1173,12 +979,9 @@ static ALvoid ALreverbState_update(ALreverbState *State, const ALCcontext *Conte props->Reverb.Density, props->Reverb.DecayTime, frequency, State); - /* Calculate the all-pass feed-back/forward coefficient. */ - State->ApFeedCoeff = sqrtf(0.5f) * powf(props->Reverb.Diffusion, 2.0f); - /* Update the early lines. */ - UpdateEarlyLines(props->Reverb.Density, props->Reverb.DecayTime, - frequency, &State->Early); + UpdateEarlyLines(props->Reverb.Density, props->Reverb.Diffusion, + props->Reverb.DecayTime, frequency, &State->Early); /* Get the mixing matrix coefficients. */ CalcMatrixCoeffs(props->Reverb.Diffusion, &State->MixX, &State->MixY); @@ -1200,32 +1003,46 @@ static ALvoid ALreverbState_update(ALreverbState *State, const ALCcontext *Conte /* Update the late lines. */ UpdateLateLines(props->Reverb.Density, props->Reverb.Diffusion, - lfDecayTime, props->Reverb.DecayTime, hfDecayTime, - F_TAU * lf0norm, F_TAU * hf0norm, - props->Reverb.EchoTime, props->Reverb.EchoDepth, - frequency, &State->Late); + lfDecayTime, props->Reverb.DecayTime, hfDecayTime, lf0norm, hf0norm, + frequency, &State->Late + ); /* Update early and late 3D panning. */ gain = props->Reverb.Gain * Slot->Params.Gain * ReverbBoost; - Update3DPanning(Device, props->Reverb.ReflectionsPan, - props->Reverb.LateReverbPan, gain, - props->Reverb.ReflectionsGain, - props->Reverb.LateReverbGain, State); + Update3DPanning(Device, props->Reverb.ReflectionsPan, props->Reverb.LateReverbPan, + props->Reverb.ReflectionsGain*gain, props->Reverb.LateReverbGain*gain, + State); - /* Determine if delay-line cross-fading is required. */ - for(i = 0;i < NUM_LINES;i++) - { - if(State->EarlyDelayTap[i][1] != State->EarlyDelayTap[i][0] || - State->Early.VecAp.Offset[i][1] != State->Early.VecAp.Offset[i][0] || - State->Early.Offset[i][1] != State->Early.Offset[i][0] || - State->LateDelayTap[i][1] != State->LateDelayTap[i][0] || - State->Late.VecAp.Offset[i][1] != State->Late.VecAp.Offset[i][0] || - State->Late.Offset[i][1] != State->Late.Offset[i][0]) - { - State->FadeCount = 0; - break; - } - } + /* Calculate the max update size from the smallest relevant delay. */ + State->MaxUpdate[1] = mini(MAX_UPDATE_SAMPLES, + mini(State->Early.Offset[0][1], State->Late.Offset[0][1]) + ); + + /* Determine if delay-line cross-fading is required. Density is essentially + * a master control for the feedback delays, so changes the offsets of many + * delay lines. + */ + if(State->Params.Density != props->Reverb.Density || + /* Diffusion and decay times influences the decay rate (gain) of the + * late reverb T60 filter. + */ + State->Params.Diffusion != props->Reverb.Diffusion || + State->Params.DecayTime != props->Reverb.DecayTime || + State->Params.HFDecayTime != hfDecayTime || + State->Params.LFDecayTime != lfDecayTime || + /* HF/LF References control the weighting used to calculate the density + * gain. + */ + State->Params.HFReference != props->Reverb.HFReference || + State->Params.LFReference != props->Reverb.LFReference) + State->FadeCount = 0; + State->Params.Density = props->Reverb.Density; + State->Params.Diffusion = props->Reverb.Diffusion; + State->Params.DecayTime = props->Reverb.DecayTime; + State->Params.HFDecayTime = hfDecayTime; + State->Params.LFDecayTime = lfDecayTime; + State->Params.HFReference = props->Reverb.HFReference; + State->Params.LFReference = props->Reverb.LFReference; } @@ -1243,37 +1060,22 @@ static inline ALfloat DelayLineOut(const DelayLineI *Delay, const ALsizei offset * offsets, this interpolates (cross-fades) the outputs at each offset. */ static inline ALfloat FadedDelayLineOut(const DelayLineI *Delay, const ALsizei off0, - const ALsizei off1, const ALsizei c, const ALfloat mu) + const ALsizei off1, const ALsizei c, + const ALfloat sc0, const ALfloat sc1) { - return Delay->Line[off0&Delay->Mask][c]*(1.0f-mu) + - Delay->Line[off1&Delay->Mask][c]*( mu); + return Delay->Line[off0&Delay->Mask][c]*sc0 + + Delay->Line[off1&Delay->Mask][c]*sc1; } -#define UnfadedDelayLineOut(d, o0, o1, c, mu) DelayLineOut(d, o0, c) -static inline ALvoid DelayLineIn(DelayLineI *Delay, ALsizei offset, const ALsizei c, - const ALfloat *restrict in, ALsizei count) + +static inline void DelayLineIn(const DelayLineI *Delay, ALsizei offset, const ALsizei c, + const ALfloat *restrict in, ALsizei count) { ALsizei i; for(i = 0;i < count;i++) Delay->Line[(offset++)&Delay->Mask][c] = *(in++); } -static inline ALvoid DelayLineIn4(DelayLineI *Delay, ALsizei offset, const ALfloat in[NUM_LINES]) -{ - ALsizei i; - offset &= Delay->Mask; - for(i = 0;i < NUM_LINES;i++) - Delay->Line[offset][i] = in[i]; -} - -static inline ALvoid DelayLineIn4Rev(DelayLineI *Delay, ALsizei offset, const ALfloat in[NUM_LINES]) -{ - ALsizei i; - offset &= Delay->Mask; - for(i = 0;i < NUM_LINES;i++) - Delay->Line[offset][i] = in[NUM_LINES-1-i]; -} - /* Applies a scattering matrix to the 4-line (vector) input. This is used * for both the below vector all-pass model and to perform modal feed-back * delay network (FDN) mixing. @@ -1320,15 +1122,26 @@ static inline void VectorPartialScatter(ALfloat *restrict out, const ALfloat *re out[2] = xCoeff*in[2] + yCoeff*( in[0] + -in[1] + in[3]); out[3] = xCoeff*in[3] + yCoeff*(-in[0] + -in[1] + -in[2] ); } +#define VectorScatterDelayIn(delay, o, in, xcoeff, ycoeff) \ + VectorPartialScatter((delay)->Line[(o)&(delay)->Mask], in, xcoeff, ycoeff) -/* Same as above, but reverses the input. */ -static inline void VectorPartialScatterRev(ALfloat *restrict out, const ALfloat *restrict in, - const ALfloat xCoeff, const ALfloat yCoeff) +/* Utilizes the above, but reverses the input channels. */ +static inline void VectorScatterRevDelayIn(const DelayLineI *Delay, ALint offset, + const ALfloat xCoeff, const ALfloat yCoeff, + const ALfloat (*restrict in)[MAX_UPDATE_SAMPLES], + const ALsizei count) { - out[0] = xCoeff*in[3] + yCoeff*(in[0] + -in[1] + in[2] ); - out[1] = xCoeff*in[2] + yCoeff*(in[0] + in[1] + -in[3]); - out[2] = xCoeff*in[1] + yCoeff*(in[0] + -in[2] + in[3]); - out[3] = xCoeff*in[0] + yCoeff*( -in[1] + -in[2] + -in[3]); + const DelayLineI delay = *Delay; + ALsizei i, j; + + for(i = 0;i < count;++i) + { + ALfloat f[NUM_LINES]; + for(j = 0;j < NUM_LINES;j++) + f[NUM_LINES-1-j] = in[j][i]; + + VectorScatterDelayIn(&delay, offset++, f, xCoeff, yCoeff); + } } /* This applies a Gerzon multiple-in/multiple-out (MIMO) vector all-pass @@ -1341,34 +1154,74 @@ static inline void VectorPartialScatterRev(ALfloat *restrict out, const ALfloat * Two static specializations are used for transitional (cross-faded) delay * line processing and non-transitional processing. */ -#define DECL_TEMPLATE(T) \ -static void VectorAllpass_##T(ALfloat *restrict out, \ - const ALfloat *restrict in, \ - const ALsizei offset, const ALfloat feedCoeff, \ - const ALfloat xCoeff, const ALfloat yCoeff, \ - const ALfloat mu, VecAllpass *Vap) \ -{ \ - ALfloat f[NUM_LINES], fs[NUM_LINES]; \ - ALfloat input; \ - ALsizei i; \ - \ - (void)mu; /* Ignore for Unfaded. */ \ - \ - for(i = 0;i < NUM_LINES;i++) \ - { \ - input = in[i]; \ - out[i] = T##DelayLineOut(&Vap->Delay, offset-Vap->Offset[i][0], \ - offset-Vap->Offset[i][1], i, mu) - \ - feedCoeff*input; \ - f[i] = input + feedCoeff*out[i]; \ - } \ - VectorPartialScatter(fs, f, xCoeff, yCoeff); \ - \ - DelayLineIn4(&Vap->Delay, offset, fs); \ +static void VectorAllpass_Unfaded(ALfloat (*restrict samples)[MAX_UPDATE_SAMPLES], ALsizei offset, + const ALfloat xCoeff, const ALfloat yCoeff, ALsizei todo, + VecAllpass *Vap) +{ + const DelayLineI delay = Vap->Delay; + const ALfloat feedCoeff = Vap->Coeff; + ALsizei vap_offset[NUM_LINES]; + ALsizei i, j; + + ASSUME(todo > 0); + + for(j = 0;j < NUM_LINES;j++) + vap_offset[j] = offset-Vap->Offset[j][0]; + for(i = 0;i < todo;i++) + { + ALfloat f[NUM_LINES]; + + for(j = 0;j < NUM_LINES;j++) + { + ALfloat input = samples[j][i]; + ALfloat out = DelayLineOut(&delay, vap_offset[j]++, j) - feedCoeff*input; + f[j] = input + feedCoeff*out; + + samples[j][i] = out; + } + + VectorScatterDelayIn(&delay, offset, f, xCoeff, yCoeff); + ++offset; + } +} +static void VectorAllpass_Faded(ALfloat (*restrict samples)[MAX_UPDATE_SAMPLES], ALsizei offset, + const ALfloat xCoeff, const ALfloat yCoeff, ALfloat fade, + ALsizei todo, VecAllpass *Vap) +{ + const DelayLineI delay = Vap->Delay; + const ALfloat feedCoeff = Vap->Coeff; + ALsizei vap_offset[NUM_LINES][2]; + ALsizei i, j; + + ASSUME(todo > 0); + + fade *= 1.0f/FADE_SAMPLES; + for(j = 0;j < NUM_LINES;j++) + { + vap_offset[j][0] = offset-Vap->Offset[j][0]; + vap_offset[j][1] = offset-Vap->Offset[j][1]; + } + for(i = 0;i < todo;i++) + { + ALfloat f[NUM_LINES]; + + for(j = 0;j < NUM_LINES;j++) + { + ALfloat input = samples[j][i]; + ALfloat out = + FadedDelayLineOut(&delay, vap_offset[j][0]++, vap_offset[j][1]++, j, + 1.0f-fade, fade + ) - feedCoeff*input; + f[j] = input + feedCoeff*out; + + samples[j][i] = out; + } + fade += FadeStep; + + VectorScatterDelayIn(&delay, offset, f, xCoeff, yCoeff); + ++offset; + } } -DECL_TEMPLATE(Unfaded) -DECL_TEMPLATE(Faded) -#undef DECL_TEMPLATE /* This generates early reflections. * @@ -1389,78 +1242,131 @@ DECL_TEMPLATE(Faded) * Two static specializations are used for transitional (cross-faded) delay * line processing and non-transitional processing. */ -#define DECL_TEMPLATE(T) \ -static void EarlyReflection_##T(ALreverbState *State, const ALsizei todo, \ - ALfloat fade, \ - ALfloat (*restrict out)[MAX_UPDATE_SAMPLES]) \ -{ \ - ALsizei offset = State->Offset; \ - const ALfloat apFeedCoeff = State->ApFeedCoeff; \ - const ALfloat mixX = State->MixX; \ - const ALfloat mixY = State->MixY; \ - ALfloat f[NUM_LINES], fr[NUM_LINES]; \ - ALsizei i, j; \ - \ - for(i = 0;i < todo;i++) \ - { \ - for(j = 0;j < NUM_LINES;j++) \ - fr[j] = T##DelayLineOut(&State->Delay, \ - offset-State->EarlyDelayTap[j][0], \ - offset-State->EarlyDelayTap[j][1], j, fade \ - ) * State->EarlyDelayCoeff[j]; \ - \ - VectorAllpass_##T(f, fr, offset, apFeedCoeff, mixX, mixY, fade, \ - &State->Early.VecAp); \ - \ - DelayLineIn4Rev(&State->Early.Delay, offset, f); \ - \ - for(j = 0;j < NUM_LINES;j++) \ - f[j] += T##DelayLineOut(&State->Early.Delay, \ - offset-State->Early.Offset[j][0], \ - offset-State->Early.Offset[j][1], j, fade \ - ) * State->Early.Coeff[j]; \ - \ - for(j = 0;j < NUM_LINES;j++) \ - out[j][i] = f[j]; \ - \ - VectorPartialScatterRev(fr, f, mixX, mixY); \ - \ - DelayLineIn4(&State->Delay, offset-State->LateFeedTap, fr); \ - \ - offset++; \ - fade += FadeStep; \ - } \ -} -DECL_TEMPLATE(Unfaded) -DECL_TEMPLATE(Faded) -#undef DECL_TEMPLATE - -/* Applies a first order filter section. */ -static inline ALfloat FirstOrderFilter(const ALfloat in, const ALfloat *restrict coeffs, - ALfloat *restrict state) +static void EarlyReflection_Unfaded(ReverbState *State, ALsizei offset, const ALsizei todo, + ALfloat (*restrict out)[MAX_UPDATE_SAMPLES]) { - ALfloat out = coeffs[0]*in + *state; - *state = coeffs[1]*in + coeffs[2]*out; - return out; + ALfloat (*restrict temps)[MAX_UPDATE_SAMPLES] = State->TempSamples; + const DelayLineI early_delay = State->Early.Delay; + const DelayLineI main_delay = State->Delay; + const ALfloat mixX = State->MixX; + const ALfloat mixY = State->MixY; + ALsizei late_feed_tap; + ALsizei i, j; + + ASSUME(todo > 0); + + /* First, load decorrelated samples from the main delay line as the primary + * reflections. + */ + for(j = 0;j < NUM_LINES;j++) + { + ALsizei early_delay_tap = offset - State->EarlyDelayTap[j][0]; + ALfloat coeff = State->EarlyDelayCoeff[j][0]; + for(i = 0;i < todo;i++) + temps[j][i] = DelayLineOut(&main_delay, early_delay_tap++, j) * coeff; + } + + /* Apply a vector all-pass, to help color the initial reflections based on + * the diffusion strength. + */ + VectorAllpass_Unfaded(temps, offset, mixX, mixY, todo, &State->Early.VecAp); + + /* Apply a delay and bounce to generate secondary reflections, combine with + * the primary reflections and write out the result for mixing. + */ + for(j = 0;j < NUM_LINES;j++) + { + ALint early_feedb_tap = offset - State->Early.Offset[j][0]; + ALfloat early_feedb_coeff = State->Early.Coeff[j][0]; + + for(i = 0;i < todo;i++) + out[j][i] = DelayLineOut(&early_delay, early_feedb_tap++, j)*early_feedb_coeff + + temps[j][i]; + } + for(j = 0;j < NUM_LINES;j++) + DelayLineIn(&early_delay, offset, NUM_LINES-1-j, temps[j], todo); + + /* Also write the result back to the main delay line for the late reverb + * stage to pick up at the appropriate time, appplying a scatter and + * bounce to improve the initial diffusion in the late reverb. + */ + late_feed_tap = offset - State->LateFeedTap; + VectorScatterRevDelayIn(&main_delay, late_feed_tap, mixX, mixY, out, todo); +} +static void EarlyReflection_Faded(ReverbState *State, ALsizei offset, const ALsizei todo, + const ALfloat fade, ALfloat (*restrict out)[MAX_UPDATE_SAMPLES]) +{ + ALfloat (*restrict temps)[MAX_UPDATE_SAMPLES] = State->TempSamples; + const DelayLineI early_delay = State->Early.Delay; + const DelayLineI main_delay = State->Delay; + const ALfloat mixX = State->MixX; + const ALfloat mixY = State->MixY; + ALsizei late_feed_tap; + ALsizei i, j; + + ASSUME(todo > 0); + + for(j = 0;j < NUM_LINES;j++) + { + ALsizei early_delay_tap0 = offset - State->EarlyDelayTap[j][0]; + ALsizei early_delay_tap1 = offset - State->EarlyDelayTap[j][1]; + ALfloat oldCoeff = State->EarlyDelayCoeff[j][0]; + ALfloat oldCoeffStep = -oldCoeff / FADE_SAMPLES; + ALfloat newCoeffStep = State->EarlyDelayCoeff[j][1] / FADE_SAMPLES; + ALfloat fadeCount = fade; + + for(i = 0;i < todo;i++) + { + const ALfloat fade0 = oldCoeff + oldCoeffStep*fadeCount; + const ALfloat fade1 = newCoeffStep*fadeCount; + temps[j][i] = FadedDelayLineOut(&main_delay, + early_delay_tap0++, early_delay_tap1++, j, fade0, fade1 + ); + fadeCount += 1.0f; + } + } + + VectorAllpass_Faded(temps, offset, mixX, mixY, fade, todo, &State->Early.VecAp); + + for(j = 0;j < NUM_LINES;j++) + { + ALint feedb_tap0 = offset - State->Early.Offset[j][0]; + ALint feedb_tap1 = offset - State->Early.Offset[j][1]; + ALfloat feedb_oldCoeff = State->Early.Coeff[j][0]; + ALfloat feedb_oldCoeffStep = -feedb_oldCoeff / FADE_SAMPLES; + ALfloat feedb_newCoeffStep = State->Early.Coeff[j][1] / FADE_SAMPLES; + ALfloat fadeCount = fade; + + for(i = 0;i < todo;i++) + { + const ALfloat fade0 = feedb_oldCoeff + feedb_oldCoeffStep*fadeCount; + const ALfloat fade1 = feedb_newCoeffStep*fadeCount; + out[j][i] = FadedDelayLineOut(&early_delay, + feedb_tap0++, feedb_tap1++, j, fade0, fade1 + ) + temps[j][i]; + fadeCount += 1.0f; + } + } + for(j = 0;j < NUM_LINES;j++) + DelayLineIn(&early_delay, offset, NUM_LINES-1-j, temps[j], todo); + + late_feed_tap = offset - State->LateFeedTap; + VectorScatterRevDelayIn(&main_delay, late_feed_tap, mixX, mixY, out, todo); } /* Applies the two T60 damping filter sections. */ -static inline void LateT60Filter(ALfloat *restrict out, const ALfloat *restrict in, - T60Filter *filter) +static inline void LateT60Filter(ALfloat *restrict samples, const ALsizei todo, T60Filter *filter) { - ALsizei i; - for(i = 0;i < NUM_LINES;i++) - out[i] = FirstOrderFilter( - FirstOrderFilter(in[i], filter[i].HFCoeffs, &filter[i].HFState), - filter[i].LFCoeffs, &filter[i].LFState - ); + ALfloat temp[MAX_UPDATE_SAMPLES]; + BiquadFilter_process(&filter->HFFilter, temp, samples, todo); + BiquadFilter_process(&filter->LFFilter, samples, temp, todo); } /* This generates the reverb tail using a modified feed-back delay network * (FDN). * - * Results from the early reflections are attenuated by the density gain and - * mixed with the output from the late delay lines. + * Results from the early reflections are mixed with the output from the late + * delay lines. * * The late response is then completed by T60 and all-pass filtering the mix. * @@ -1470,69 +1376,120 @@ static inline void LateT60Filter(ALfloat *restrict out, const ALfloat *restrict * Two variations are made, one for for transitional (cross-faded) delay line * processing and one for non-transitional processing. */ -#define DECL_TEMPLATE(T) \ -static void LateReverb_##T(ALreverbState *State, const ALsizei todo, \ - ALfloat fade, \ - ALfloat (*restrict out)[MAX_UPDATE_SAMPLES]) \ -{ \ - const ALfloat apFeedCoeff = State->ApFeedCoeff; \ - const ALfloat mixX = State->MixX; \ - const ALfloat mixY = State->MixY; \ - ALsizei offset; \ - ALsizei i, j; \ - \ - offset = State->Offset; \ - for(i = 0;i < todo;i++) \ - { \ - ALfloat f[NUM_LINES], fr[NUM_LINES]; \ - \ - for(j = 0;j < NUM_LINES;j++) \ - f[j] = T##DelayLineOut(&State->Delay, \ - offset - State->LateDelayTap[j][0], \ - offset - State->LateDelayTap[j][1], j, fade \ - ) * State->Late.DensityGain; \ - \ - for(j = 0;j < NUM_LINES;j++) \ - f[j] += T##DelayLineOut(&State->Late.Delay, \ - offset - State->Late.Offset[j][0], \ - offset - State->Late.Offset[j][1], j, fade \ - ); \ - \ - LateT60Filter(fr, f, State->Late.T60); \ - VectorAllpass_##T(f, fr, offset, apFeedCoeff, mixX, mixY, fade, \ - &State->Late.VecAp); \ - \ - for(j = 0;j < NUM_LINES;j++) \ - out[j][i] = f[j]; \ - \ - VectorPartialScatterRev(fr, f, mixX, mixY); \ - \ - DelayLineIn4(&State->Late.Delay, offset, fr); \ - \ - offset++; \ - fade += FadeStep; \ - } \ -} -DECL_TEMPLATE(Unfaded) -DECL_TEMPLATE(Faded) -#undef DECL_TEMPLATE - -static ALvoid ALreverbState_process(ALreverbState *State, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels) +static void LateReverb_Unfaded(ReverbState *State, ALsizei offset, const ALsizei todo, + ALfloat (*restrict out)[MAX_UPDATE_SAMPLES]) { - ALfloat (*restrict afmt)[MAX_UPDATE_SAMPLES] = State->AFormatSamples; - ALfloat (*restrict early)[MAX_UPDATE_SAMPLES] = State->EarlySamples; - ALfloat (*restrict late)[MAX_UPDATE_SAMPLES] = State->ReverbSamples; + ALfloat (*restrict temps)[MAX_UPDATE_SAMPLES] = State->TempSamples; + const DelayLineI late_delay = State->Late.Delay; + const DelayLineI main_delay = State->Delay; + const ALfloat mixX = State->MixX; + const ALfloat mixY = State->MixY; + ALsizei i, j; + + ASSUME(todo > 0); + + /* First, load decorrelated samples from the main and feedback delay lines. + * Filter the signal to apply its frequency-dependent decay. + */ + for(j = 0;j < NUM_LINES;j++) + { + ALsizei late_delay_tap = offset - State->LateDelayTap[j][0]; + ALsizei late_feedb_tap = offset - State->Late.Offset[j][0]; + ALfloat midGain = State->Late.T60[j].MidGain[0]; + const ALfloat densityGain = State->Late.DensityGain[0] * midGain; + for(i = 0;i < todo;i++) + temps[j][i] = DelayLineOut(&main_delay, late_delay_tap++, j)*densityGain + + DelayLineOut(&late_delay, late_feedb_tap++, j)*midGain; + LateT60Filter(temps[j], todo, &State->Late.T60[j]); + } + + /* Apply a vector all-pass to improve micro-surface diffusion, and write + * out the results for mixing. + */ + VectorAllpass_Unfaded(temps, offset, mixX, mixY, todo, &State->Late.VecAp); + + for(j = 0;j < NUM_LINES;j++) + memcpy(out[j], temps[j], todo*sizeof(ALfloat)); + + /* Finally, scatter and bounce the results to refeed the feedback buffer. */ + VectorScatterRevDelayIn(&late_delay, offset, mixX, mixY, out, todo); +} +static void LateReverb_Faded(ReverbState *State, ALsizei offset, const ALsizei todo, + const ALfloat fade, ALfloat (*restrict out)[MAX_UPDATE_SAMPLES]) +{ + ALfloat (*restrict temps)[MAX_UPDATE_SAMPLES] = State->TempSamples; + const DelayLineI late_delay = State->Late.Delay; + const DelayLineI main_delay = State->Delay; + const ALfloat mixX = State->MixX; + const ALfloat mixY = State->MixY; + ALsizei i, j; + + ASSUME(todo > 0); + + for(j = 0;j < NUM_LINES;j++) + { + const ALfloat oldMidGain = State->Late.T60[j].MidGain[0]; + const ALfloat midGain = State->Late.T60[j].MidGain[1]; + const ALfloat oldMidStep = -oldMidGain / FADE_SAMPLES; + const ALfloat midStep = midGain / FADE_SAMPLES; + const ALfloat oldDensityGain = State->Late.DensityGain[0] * oldMidGain; + const ALfloat densityGain = State->Late.DensityGain[1] * midGain; + const ALfloat oldDensityStep = -oldDensityGain / FADE_SAMPLES; + const ALfloat densityStep = densityGain / FADE_SAMPLES; + ALsizei late_delay_tap0 = offset - State->LateDelayTap[j][0]; + ALsizei late_delay_tap1 = offset - State->LateDelayTap[j][1]; + ALsizei late_feedb_tap0 = offset - State->Late.Offset[j][0]; + ALsizei late_feedb_tap1 = offset - State->Late.Offset[j][1]; + ALfloat fadeCount = fade; + + for(i = 0;i < todo;i++) + { + const ALfloat fade0 = oldDensityGain + oldDensityStep*fadeCount; + const ALfloat fade1 = densityStep*fadeCount; + const ALfloat gfade0 = oldMidGain + oldMidStep*fadeCount; + const ALfloat gfade1 = midStep*fadeCount; + temps[j][i] = + FadedDelayLineOut(&main_delay, late_delay_tap0++, late_delay_tap1++, j, + fade0, fade1) + + FadedDelayLineOut(&late_delay, late_feedb_tap0++, late_feedb_tap1++, j, + gfade0, gfade1); + fadeCount += 1.0f; + } + LateT60Filter(temps[j], todo, &State->Late.T60[j]); + } + + VectorAllpass_Faded(temps, offset, mixX, mixY, fade, todo, &State->Late.VecAp); + + for(j = 0;j < NUM_LINES;j++) + memcpy(out[j], temps[j], todo*sizeof(ALfloat)); + + VectorScatterRevDelayIn(&late_delay, offset, mixX, mixY, temps, todo); +} + +static ALvoid ReverbState_process(ReverbState *State, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels) +{ + ALfloat (*restrict afmt)[MAX_UPDATE_SAMPLES] = State->TempSamples; + ALfloat (*restrict samples)[MAX_UPDATE_SAMPLES] = State->MixSamples; ALsizei fadeCount = State->FadeCount; - ALfloat fade = (ALfloat)fadeCount / FADE_SAMPLES; + ALsizei offset = State->Offset; ALsizei base, c; /* Process reverb for these samples. */ for(base = 0;base < SamplesToDo;) { - ALsizei todo = mini(SamplesToDo-base, MAX_UPDATE_SAMPLES); + ALsizei todo = SamplesToDo - base; /* If cross-fading, don't do more samples than there are to fade. */ if(FADE_SAMPLES-fadeCount > 0) + { todo = mini(todo, FADE_SAMPLES-fadeCount); + todo = mini(todo, State->MaxUpdate[0]); + } + todo = mini(todo, State->MaxUpdate[1]); + /* If this is not the final update, ensure the update size is a + * multiple of 4 for the SIMD mixers. + */ + if(todo < SamplesToDo-base) + todo &= ~3; /* Convert B-Format to A-Format for processing. */ memset(afmt, 0, sizeof(*afmt)*NUM_LINES); @@ -1544,69 +1501,84 @@ static ALvoid ALreverbState_process(ALreverbState *State, ALsizei SamplesToDo, c /* Process the samples for reverb. */ for(c = 0;c < NUM_LINES;c++) { - /* Band-pass the incoming samples. Use the early output lines for - * temp storage. - */ - BiquadFilter_process(&State->Filter[c].Lp, early[0], afmt[c], todo); - BiquadFilter_process(&State->Filter[c].Hp, early[1], early[0], todo); + /* Band-pass the incoming samples. */ + BiquadFilter_process(&State->Filter[c].Lp, samples[0], afmt[c], todo); + BiquadFilter_process(&State->Filter[c].Hp, samples[1], samples[0], todo); /* Feed the initial delay line. */ - DelayLineIn(&State->Delay, State->Offset, c, early[1], todo); + DelayLineIn(&State->Delay, offset, c, samples[1], todo); } if(UNLIKELY(fadeCount < FADE_SAMPLES)) { - /* Generate early reflections. */ - EarlyReflection_Faded(State, todo, fade, early); + ALfloat fade = (ALfloat)fadeCount; - /* Generate late reverb. */ - LateReverb_Faded(State, todo, fade, late); - fade = minf(1.0f, fade + todo*FadeStep); + /* Generate early reflections. */ + EarlyReflection_Faded(State, offset, todo, fade, samples); + /* Mix the A-Format results to output, implicitly converting back + * to B-Format. + */ + for(c = 0;c < NUM_LINES;c++) + MixSamples(samples[c], NumChannels, SamplesOut, + State->Early.CurrentGain[c], State->Early.PanGain[c], + SamplesToDo-base, base, todo + ); + + /* Generate and mix late reverb. */ + LateReverb_Faded(State, offset, todo, fade, samples); + for(c = 0;c < NUM_LINES;c++) + MixSamples(samples[c], NumChannels, SamplesOut, + State->Late.CurrentGain[c], State->Late.PanGain[c], + SamplesToDo-base, base, todo + ); + + /* Step fading forward. */ + fadeCount += todo; + if(LIKELY(fadeCount >= FADE_SAMPLES)) + { + /* Update the cross-fading delay line taps. */ + fadeCount = FADE_SAMPLES; + for(c = 0;c < NUM_LINES;c++) + { + State->EarlyDelayTap[c][0] = State->EarlyDelayTap[c][1]; + State->EarlyDelayCoeff[c][0] = State->EarlyDelayCoeff[c][1]; + State->Early.VecAp.Offset[c][0] = State->Early.VecAp.Offset[c][1]; + State->Early.Offset[c][0] = State->Early.Offset[c][1]; + State->Early.Coeff[c][0] = State->Early.Coeff[c][1]; + State->LateDelayTap[c][0] = State->LateDelayTap[c][1]; + State->Late.VecAp.Offset[c][0] = State->Late.VecAp.Offset[c][1]; + State->Late.Offset[c][0] = State->Late.Offset[c][1]; + State->Late.T60[c].MidGain[0] = State->Late.T60[c].MidGain[1]; + } + State->Late.DensityGain[0] = State->Late.DensityGain[1]; + State->MaxUpdate[0] = State->MaxUpdate[1]; + } } else { - /* Generate early reflections. */ - EarlyReflection_Unfaded(State, todo, fade, early); + /* Generate and mix early reflections. */ + EarlyReflection_Unfaded(State, offset, todo, samples); + for(c = 0;c < NUM_LINES;c++) + MixSamples(samples[c], NumChannels, SamplesOut, + State->Early.CurrentGain[c], State->Early.PanGain[c], + SamplesToDo-base, base, todo + ); - /* Generate late reverb. */ - LateReverb_Unfaded(State, todo, fade, late); + /* Generate and mix late reverb. */ + LateReverb_Unfaded(State, offset, todo, samples); + for(c = 0;c < NUM_LINES;c++) + MixSamples(samples[c], NumChannels, SamplesOut, + State->Late.CurrentGain[c], State->Late.PanGain[c], + SamplesToDo-base, base, todo + ); } /* Step all delays forward. */ - State->Offset += todo; - - if(UNLIKELY(fadeCount < FADE_SAMPLES) && (fadeCount += todo) >= FADE_SAMPLES) - { - /* Update the cross-fading delay line taps. */ - fadeCount = FADE_SAMPLES; - fade = 1.0f; - for(c = 0;c < NUM_LINES;c++) - { - State->EarlyDelayTap[c][0] = State->EarlyDelayTap[c][1]; - State->Early.VecAp.Offset[c][0] = State->Early.VecAp.Offset[c][1]; - State->Early.Offset[c][0] = State->Early.Offset[c][1]; - State->LateDelayTap[c][0] = State->LateDelayTap[c][1]; - State->Late.VecAp.Offset[c][0] = State->Late.VecAp.Offset[c][1]; - State->Late.Offset[c][0] = State->Late.Offset[c][1]; - } - } - - /* Mix the A-Format results to output, implicitly converting back to - * B-Format. - */ - for(c = 0;c < NUM_LINES;c++) - MixSamples(early[c], NumChannels, SamplesOut, - State->Early.CurrentGain[c], State->Early.PanGain[c], - SamplesToDo-base, base, todo - ); - for(c = 0;c < NUM_LINES;c++) - MixSamples(late[c], NumChannels, SamplesOut, - State->Late.CurrentGain[c], State->Late.PanGain[c], - SamplesToDo-base, base, todo - ); + offset += todo; base += todo; } + State->Offset = offset; State->FadeCount = fadeCount; } @@ -1617,9 +1589,9 @@ typedef struct ReverbStateFactory { static ALeffectState *ReverbStateFactory_create(ReverbStateFactory* UNUSED(factory)) { - ALreverbState *state; + ReverbState *state; - NEW_OBJ0(state, ALreverbState)(); + NEW_OBJ0(state, ReverbState)(); if(!state) return NULL; return STATIC_CAST(ALeffectState, state); diff --git a/Engine/lib/openal-soft/Alc/helpers.c b/Engine/lib/openal-soft/Alc/helpers.c index 7bcb3f4ab..d2cb62536 100644 --- a/Engine/lib/openal-soft/Alc/helpers.c +++ b/Engine/lib/openal-soft/Alc/helpers.c @@ -125,6 +125,7 @@ extern inline ALuint NextPowerOf2(ALuint value); extern inline size_t RoundUp(size_t value, size_t r); extern inline ALint fastf2i(ALfloat f); extern inline int float2int(float f); +extern inline float fast_roundf(float f); #ifndef __GNUC__ #if defined(HAVE_BITSCANFORWARD64_INTRINSIC) extern inline int msvc64_ctz64(ALuint64 v); @@ -222,22 +223,32 @@ void FillCPUCaps(int capfilter) ERR("Failed to open /proc/cpuinfo, cannot check for NEON support\n"); else { + al_string features = AL_STRING_INIT_STATIC(); char buf[256]; + while(fgets(buf, sizeof(buf), file) != NULL) { - size_t len; - char *str; - if(strncmp(buf, "Features\t:", 10) != 0) continue; - len = strlen(buf); - while(len > 0 && isspace(buf[len-1])) - buf[--len] = 0; + alstr_copy_cstr(&features, buf+10); + while(VECTOR_BACK(features) != '\n') + { + if(fgets(buf, sizeof(buf), file) == NULL) + break; + alstr_append_cstr(&features, buf); + } + break; + } + fclose(file); + file = NULL; - TRACE("Got features string:%s\n", buf+10); + if(!alstr_empty(features)) + { + const char *str = alstr_get_cstr(features); + while(isspace(str[0])) ++str; - str = buf; + TRACE("Got features string:%s\n", str); while((str=strstr(str, "neon")) != NULL) { if(isspace(*(str-1)) && (str[4] == 0 || isspace(str[4]))) @@ -245,13 +256,11 @@ void FillCPUCaps(int capfilter) caps |= CPU_CAP_NEON; break; } - str++; + ++str; } - break; } - fclose(file); - file = NULL; + alstr_reset(&features); } #endif @@ -675,13 +684,13 @@ void GetProcBinary(al_string *path, al_string *fname) size_t pathlen; #ifdef __FreeBSD__ - int mib[4] = { CTL_KERN, KERN_PROC_ARGS, getpid() }; - if(sysctl(mib, 3, NULL, &pathlen, NULL, 0) == -1) - WARN("Failed to sysctl kern.procargs.%d: %s\n", mib[2], strerror(errno)); + int mib[4] = { CTL_KERN, KERN_PROC, KERN_PROC_PATHNAME, -1 }; + if(sysctl(mib, 4, NULL, &pathlen, NULL, 0) == -1) + WARN("Failed to sysctl kern.proc.pathname: %s\n", strerror(errno)); else { pathname = malloc(pathlen + 1); - sysctl(mib, 3, (void*)pathname, &pathlen, NULL, 0); + sysctl(mib, 4, (void*)pathname, &pathlen, NULL, 0); pathname[pathlen] = 0; } #endif @@ -1096,8 +1105,8 @@ void alstr_copy_range(al_string *str, const al_string_char_type *from, const al_ void alstr_append_char(al_string *str, const al_string_char_type c) { size_t len = alstr_length(*str); - VECTOR_RESIZE(*str, len, len+2); - VECTOR_PUSH_BACK(*str, c); + VECTOR_RESIZE(*str, len+1, len+2); + VECTOR_BACK(*str) = c; VECTOR_ELEM(*str, len+1) = 0; } diff --git a/Engine/lib/openal-soft/Alc/hrtf.c b/Engine/lib/openal-soft/Alc/hrtf.c index 810530e5c..ddbd3a28c 100644 --- a/Engine/lib/openal-soft/Alc/hrtf.c +++ b/Engine/lib/openal-soft/Alc/hrtf.c @@ -156,13 +156,13 @@ void GetHrtfCoeffs(const struct Hrtf *Hrtf, ALfloat elevation, ALfloat azimuth, blend[3] = ( emu) * ( amu[1]) * dirfact; /* Calculate the blended HRIR delays. */ - delays[0] = float2int( + delays[0] = fastf2i( Hrtf->delays[idx[0]][0]*blend[0] + Hrtf->delays[idx[1]][0]*blend[1] + - Hrtf->delays[idx[2]][0]*blend[2] + Hrtf->delays[idx[3]][0]*blend[3] + 0.5f + Hrtf->delays[idx[2]][0]*blend[2] + Hrtf->delays[idx[3]][0]*blend[3] ); - delays[1] = float2int( + delays[1] = fastf2i( Hrtf->delays[idx[0]][1]*blend[0] + Hrtf->delays[idx[1]][1]*blend[1] + - Hrtf->delays[idx[2]][1]*blend[2] + Hrtf->delays[idx[3]][1]*blend[3] + 0.5f + Hrtf->delays[idx[2]][1]*blend[2] + Hrtf->delays[idx[3]][1]*blend[3] ); /* Calculate the sample offsets for the HRIR indices. */ @@ -202,12 +202,15 @@ void BuildBFormatHrtf(const struct Hrtf *Hrtf, DirectHrtfState *state, ALsizei N #define NUM_BANDS 2 BandSplitter splitter; ALdouble (*tmpres)[HRIR_LENGTH][2]; - ALsizei idx[HRTF_AMBI_MAX_CHANNELS]; + ALsizei *restrict idx; ALsizei min_delay = HRTF_HISTORY_LENGTH; + ALsizei max_delay = 0; ALfloat temps[3][HRIR_LENGTH]; - ALsizei max_length = 0; + ALsizei max_length; ALsizei i, c, b; + idx = al_calloc(DEF_ALIGN, AmbiCount*sizeof(*idx)); + for(c = 0;c < AmbiCount;c++) { ALuint evidx, azidx; @@ -228,6 +231,7 @@ void BuildBFormatHrtf(const struct Hrtf *Hrtf, DirectHrtfState *state, ALsizei N idx[c] = evoffset + azidx; min_delay = mini(min_delay, mini(Hrtf->delays[idx[c]][0], Hrtf->delays[idx[c]][1])); + max_delay = maxi(max_delay, maxi(Hrtf->delays[idx[c]][0], Hrtf->delays[idx[c]][1])); } tmpres = al_calloc(16, NumChannels * sizeof(*tmpres)); @@ -242,10 +246,6 @@ void BuildBFormatHrtf(const struct Hrtf *Hrtf, DirectHrtfState *state, ALsizei N if(NUM_BANDS == 1) { - max_length = maxi(max_length, - mini(maxi(ldelay, rdelay) + Hrtf->irSize, HRIR_LENGTH) - ); - for(i = 0;i < NumChannels;++i) { ALdouble mult = (ALdouble)AmbiOrderHFGain[(ALsizei)sqrt(i)] * AmbiMatrix[c][i]; @@ -261,15 +261,6 @@ void BuildBFormatHrtf(const struct Hrtf *Hrtf, DirectHrtfState *state, ALsizei N } else { - /* Increase the IR size by 2/3rds to account for the tail generated - * by the band-split filter. - */ - const ALsizei irsize = mini(Hrtf->irSize*5/3, HRIR_LENGTH); - - max_length = maxi(max_length, - mini(maxi(ldelay, rdelay) + irsize, HRIR_LENGTH) - ); - /* Band-split left HRIR into low and high frequency responses. */ bandsplit_clear(&splitter); for(i = 0;i < Hrtf->irSize;i++) @@ -311,9 +302,6 @@ void BuildBFormatHrtf(const struct Hrtf *Hrtf, DirectHrtfState *state, ALsizei N } } } - /* Round up to the next IR size multiple. */ - max_length += MOD_IR_SIZE-1; - max_length -= max_length%MOD_IR_SIZE; for(i = 0;i < NumChannels;++i) { @@ -324,11 +312,27 @@ void BuildBFormatHrtf(const struct Hrtf *Hrtf, DirectHrtfState *state, ALsizei N state->Chan[i].Coeffs[idx][1] = (ALfloat)tmpres[i][idx][1]; } } - al_free(tmpres); tmpres = NULL; + al_free(idx); + idx = NULL; - TRACE("Skipped delay: %d, new FIR length: %d\n", min_delay, max_length); + if(NUM_BANDS == 1) + max_length = mini(max_delay-min_delay + Hrtf->irSize, HRIR_LENGTH); + else + { + /* Increase the IR size by 2/3rds to account for the tail generated by + * the band-split filter. + */ + const ALsizei irsize = mini(Hrtf->irSize*5/3, HRIR_LENGTH); + max_length = mini(max_delay-min_delay + irsize, HRIR_LENGTH); + } + /* Round up to the next IR size multiple. */ + max_length += MOD_IR_SIZE-1; + max_length -= max_length%MOD_IR_SIZE; + + TRACE("Skipped delay: %d, max delay: %d, new FIR length: %d\n", + min_delay, max_delay-min_delay, max_length); state->IrSize = max_length; #undef NUM_BANDS } @@ -1030,12 +1034,12 @@ static void AddFileEntry(vector_EnumeratedHrtf *list, const_al_string filename) /* Check if this entry has already been added to the list. */ #define MATCH_ENTRY(i) (loaded_entry == (i)->hrtf) VECTOR_FIND_IF(iter, const EnumeratedHrtf, *list, MATCH_ENTRY); +#undef MATCH_ENTRY if(iter != VECTOR_END(*list)) { TRACE("Skipping duplicate file entry %s\n", alstr_get_cstr(filename)); return; } -#undef MATCH_FNAME break; } @@ -1109,12 +1113,12 @@ static void AddBuiltInEntry(vector_EnumeratedHrtf *list, const_al_string filenam { #define MATCH_ENTRY(i) (loaded_entry == (i)->hrtf) VECTOR_FIND_IF(iter, const EnumeratedHrtf, *list, MATCH_ENTRY); +#undef MATCH_ENTRY if(iter != VECTOR_END(*list)) { TRACE("Skipping duplicate file entry %s\n", alstr_get_cstr(filename)); return; } -#undef MATCH_FNAME break; } diff --git a/Engine/lib/openal-soft/Alc/hrtf.h b/Engine/lib/openal-soft/Alc/hrtf.h index cb6dfddc8..ab68929b4 100644 --- a/Engine/lib/openal-soft/Alc/hrtf.h +++ b/Engine/lib/openal-soft/Alc/hrtf.h @@ -9,12 +9,6 @@ #include "atomic.h" -/* The maximum number of virtual speakers used to generate HRTF coefficients - * for decoding B-Format. - */ -#define HRTF_AMBI_MAX_CHANNELS 18 - - #define HRTF_HISTORY_BITS (6) #define HRTF_HISTORY_LENGTH (1<Envelope[i] = 0.0f; - - for(c = 0;c < NumChans;c++) - { - for(i = 0;i < SamplesToDo;i++) - Comp->Envelope[i] += OutBuffer[c][i]; - } - - for(i = 0;i < SamplesToDo;i++) - Comp->Envelope[i] = fabsf(Comp->Envelope[i]); -} - -static void MaxChannels(Compressor *Comp, const ALsizei NumChans, const ALsizei SamplesToDo, - ALfloat (*restrict OutBuffer)[BUFFERSIZE]) -{ - ALsizei c, i; - - for(i = 0;i < SamplesToDo;i++) - Comp->Envelope[i] = 0.0f; - - for(c = 0;c < NumChans;c++) - { - for(i = 0;i < SamplesToDo;i++) - Comp->Envelope[i] = maxf(Comp->Envelope[i], fabsf(OutBuffer[c][i])); - } -} - -/* Envelope detection/sensing can be done via: + * D. Giannoulis, M. Massberg and J. D. Reiss, + * "Parameter Automation in a Dynamic Range Compressor," + * Journal of the Audio Engineering Society, v61 (10), Oct. 2013 * - * RMS - Rectangular windowed root mean square of linking stage. - * Peak - Implicit output from linking stage. + * Available (along with supplemental reading) at: + * + * http://c4dm.eecs.qmul.ac.uk/audioengineering/compressors/ */ -static void RmsDetection(Compressor *Comp, const ALsizei SamplesToDo) -{ - ALuint sum = Comp->RmsSum; - ALuint *window = Comp->RmsWindow; - ALsizei index = Comp->RmsIndex; - ALsizei i; +typedef struct Compressor { + ALsizei NumChans; + ALuint SampleRate; - for(i = 0;i < SamplesToDo;i++) - { - ALfloat sig = Comp->Envelope[i]; + struct { + ALuint Knee : 1; + ALuint Attack : 1; + ALuint Release : 1; + ALuint PostGain : 1; + ALuint Declip : 1; + } Auto; - sum -= window[index]; - window[index] = fastf2i(minf(sig * sig * 65536.0f, RMS_VALUE_MAX)); - sum += window[index]; - index = (index + 1) & RMS_WINDOW_MASK; + ALsizei LookAhead; - Comp->Envelope[i] = sqrtf(sum / 65536.0f / RMS_WINDOW_SIZE); - } + ALfloat PreGain; + ALfloat PostGain; - Comp->RmsSum = sum; - Comp->RmsIndex = index; -} + ALfloat Threshold; + ALfloat Slope; + ALfloat Knee; -/* This isn't a very sophisticated envelope follower, but it gets the job - * done. First, it operates at logarithmic scales to keep transitions - * appropriate for human hearing. Second, it can apply adaptive (automated) - * attack/release adjustments based on the signal. + ALfloat Attack; + ALfloat Release; + + alignas(16) ALfloat SideChain[2*BUFFERSIZE]; + alignas(16) ALfloat CrestFactor[BUFFERSIZE]; + + SlidingHold *Hold; + ALfloat (*Delay)[BUFFERSIZE]; + ALsizei DelayIndex; + + ALfloat CrestCoeff; + ALfloat GainEstimate; + ALfloat AdaptCoeff; + + ALfloat LastPeakSq; + ALfloat LastRmsSq; + ALfloat LastRelease; + ALfloat LastAttack; + ALfloat LastGainDev; +} Compressor; + + +/* This sliding hold follows the input level with an instant attack and a + * fixed duration hold before an instant release to the next highest level. + * It is a sliding window maximum (descending maxima) implementation based on + * Richard Harter's ascending minima algorithm available at: + * + * http://www.richardhartersworld.com/cri/2001/slidingmin.html */ -static void FollowEnvelope(Compressor *Comp, const ALsizei SamplesToDo) +static ALfloat UpdateSlidingHold(SlidingHold *Hold, const ALsizei i, const ALfloat in) { - ALfloat attackMin = Comp->AttackMin; - ALfloat attackMax = Comp->AttackMax; - ALfloat releaseMin = Comp->ReleaseMin; - ALfloat releaseMax = Comp->ReleaseMax; - ALfloat last = Comp->EnvLast; - ALsizei i; + const ALsizei mask = BUFFERSIZE - 1; + const ALsizei length = Hold->Length; + ALfloat *restrict values = Hold->Values; + ALsizei *restrict expiries = Hold->Expiries; + ALsizei lowerIndex = Hold->LowerIndex; + ALsizei upperIndex = Hold->UpperIndex; - for(i = 0;i < SamplesToDo;i++) + if(i >= expiries[upperIndex]) + upperIndex = (upperIndex + 1) & mask; + + if(in >= values[upperIndex]) { - ALfloat env = log10f(maxf(Comp->Envelope[i], 0.000001f)); - ALfloat slope = minf(1.0f, fabsf(env - last) / 4.5f); - - if(env > last) - last = minf(env, last + lerp(attackMin, attackMax, 1.0f - (slope * slope))); - else - last = maxf(env, last + lerp(releaseMin, releaseMax, 1.0f - (slope * slope))); - - Comp->Envelope[i] = last; - } - - Comp->EnvLast = last; -} - -/* The envelope is converted to control gain with an optional soft knee. */ -static void EnvelopeGain(Compressor *Comp, const ALsizei SamplesToDo, const ALfloat Slope) -{ - const ALfloat threshold = Comp->Threshold; - const ALfloat knee = Comp->Knee; - ALsizei i; - - if(!(knee > 0.0f)) - { - for(i = 0;i < SamplesToDo;i++) - { - ALfloat gain = Slope * (threshold - Comp->Envelope[i]); - Comp->Envelope[i] = powf(10.0f, minf(0.0f, gain)); - } + values[upperIndex] = in; + expiries[upperIndex] = i + length; + lowerIndex = upperIndex; } else { - const ALfloat lower = threshold - (0.5f * knee); - const ALfloat upper = threshold + (0.5f * knee); - const ALfloat m = 0.5f * Slope / knee; + do { + do { + if(!(in >= values[lowerIndex])) + goto found_place; + } while(lowerIndex--); + lowerIndex = mask; + } while(1); + found_place: + lowerIndex = (lowerIndex + 1) & mask; + values[lowerIndex] = in; + expiries[lowerIndex] = i + length; + } + + Hold->LowerIndex = lowerIndex; + Hold->UpperIndex = upperIndex; + + return values[upperIndex]; +} + +static void ShiftSlidingHold(SlidingHold *Hold, const ALsizei n) +{ + const ALsizei lowerIndex = Hold->LowerIndex; + ALsizei *restrict expiries = Hold->Expiries; + ALsizei i = Hold->UpperIndex; + + if(lowerIndex < i) + { + for(;i < BUFFERSIZE;i++) + expiries[i] -= n; + i = 0; + } + for(;i < lowerIndex;i++) + expiries[i] -= n; + + expiries[i] -= n; +} + +/* Multichannel compression is linked via the absolute maximum of all + * channels. + */ +static void LinkChannels(Compressor *Comp, const ALsizei SamplesToDo, ALfloat (*restrict OutBuffer)[BUFFERSIZE]) +{ + const ALsizei index = Comp->LookAhead; + const ALsizei numChans = Comp->NumChans; + ALfloat *restrict sideChain = Comp->SideChain; + ALsizei c, i; + + ASSUME(SamplesToDo > 0); + ASSUME(numChans > 0); + + for(i = 0;i < SamplesToDo;i++) + sideChain[index + i] = 0.0f; + + for(c = 0;c < numChans;c++) + { + ALsizei offset = index; for(i = 0;i < SamplesToDo;i++) { - ALfloat env = Comp->Envelope[i]; - ALfloat gain; - - if(env > lower && env < upper) - gain = m * (env - lower) * (lower - env); - else - gain = Slope * (threshold - env); - - Comp->Envelope[i] = powf(10.0f, minf(0.0f, gain)); + sideChain[offset] = maxf(sideChain[offset], fabsf(OutBuffer[c][i])); + ++offset; } } } +/* This calculates the squared crest factor of the control signal for the + * basic automation of the attack/release times. As suggested by the paper, + * it uses an instantaneous squared peak detector and a squared RMS detector + * both with 200ms release times. + */ +static void CrestDetector(Compressor *Comp, const ALsizei SamplesToDo) +{ + const ALfloat a_crest = Comp->CrestCoeff; + const ALsizei index = Comp->LookAhead; + const ALfloat *restrict sideChain = Comp->SideChain; + ALfloat *restrict crestFactor = Comp->CrestFactor; + ALfloat y2_peak = Comp->LastPeakSq; + ALfloat y2_rms = Comp->LastRmsSq; + ALsizei i; -Compressor *CompressorInit(const ALfloat PreGainDb, const ALfloat PostGainDb, - const ALboolean SummedLink, const ALboolean RmsSensing, - const ALfloat AttackTimeMin, const ALfloat AttackTimeMax, - const ALfloat ReleaseTimeMin, const ALfloat ReleaseTimeMax, - const ALfloat Ratio, const ALfloat ThresholdDb, - const ALfloat KneeDb, const ALuint SampleRate) + ASSUME(SamplesToDo > 0); + + for(i = 0;i < SamplesToDo;i++) + { + ALfloat x_abs = sideChain[index + i]; + ALfloat x2 = maxf(0.000001f, x_abs * x_abs); + + y2_peak = maxf(x2, lerp(x2, y2_peak, a_crest)); + y2_rms = lerp(x2, y2_rms, a_crest); + crestFactor[i] = y2_peak / y2_rms; + } + + Comp->LastPeakSq = y2_peak; + Comp->LastRmsSq = y2_rms; +} + +/* The side-chain starts with a simple peak detector (based on the absolute + * value of the incoming signal) and performs most of its operations in the + * log domain. + */ +static void PeakDetector(Compressor *Comp, const ALsizei SamplesToDo) +{ + const ALsizei index = Comp->LookAhead; + ALfloat *restrict sideChain = Comp->SideChain; + ALsizei i; + + ASSUME(SamplesToDo > 0); + + for(i = 0;i < SamplesToDo;i++) + { + const ALuint offset = index + i; + const ALfloat x_abs = sideChain[offset]; + + sideChain[offset] = logf(maxf(0.000001f, x_abs)); + } +} + +/* An optional hold can be used to extend the peak detector so it can more + * solidly detect fast transients. This is best used when operating as a + * limiter. + */ +static void PeakHoldDetector(Compressor *Comp, const ALsizei SamplesToDo) +{ + const ALsizei index = Comp->LookAhead; + ALfloat *restrict sideChain = Comp->SideChain; + SlidingHold *hold = Comp->Hold; + ALsizei i; + + ASSUME(SamplesToDo > 0); + + for(i = 0;i < SamplesToDo;i++) + { + const ALsizei offset = index + i; + const ALfloat x_abs = sideChain[offset]; + const ALfloat x_G = logf(maxf(0.000001f, x_abs)); + + sideChain[offset] = UpdateSlidingHold(hold, i, x_G); + } + + ShiftSlidingHold(hold, SamplesToDo); +} + +/* This is the heart of the feed-forward compressor. It operates in the log + * domain (to better match human hearing) and can apply some basic automation + * to knee width, attack/release times, make-up/post gain, and clipping + * reduction. + */ +static void GainCompressor(Compressor *Comp, const ALsizei SamplesToDo) +{ + const bool autoKnee = Comp->Auto.Knee; + const bool autoAttack = Comp->Auto.Attack; + const bool autoRelease = Comp->Auto.Release; + const bool autoPostGain = Comp->Auto.PostGain; + const bool autoDeclip = Comp->Auto.Declip; + const ALsizei lookAhead = Comp->LookAhead; + const ALfloat threshold = Comp->Threshold; + const ALfloat slope = Comp->Slope; + const ALfloat attack = Comp->Attack; + const ALfloat release = Comp->Release; + const ALfloat c_est = Comp->GainEstimate; + const ALfloat a_adp = Comp->AdaptCoeff; + const ALfloat *restrict crestFactor = Comp->CrestFactor; + ALfloat *restrict sideChain = Comp->SideChain; + ALfloat postGain = Comp->PostGain; + ALfloat knee = Comp->Knee; + ALfloat t_att = attack; + ALfloat t_rel = release - attack; + ALfloat a_att = expf(-1.0f / t_att); + ALfloat a_rel = expf(-1.0f / t_rel); + ALfloat y_1 = Comp->LastRelease; + ALfloat y_L = Comp->LastAttack; + ALfloat c_dev = Comp->LastGainDev; + ALsizei i; + + ASSUME(SamplesToDo > 0); + + for(i = 0;i < SamplesToDo;i++) + { + const ALfloat y2_crest = crestFactor[i]; + const ALfloat x_G = sideChain[lookAhead + i]; + const ALfloat x_over = x_G - threshold; + ALfloat knee_h; + ALfloat y_G; + ALfloat x_L; + + if(autoKnee) + knee = maxf(0.0f, 2.5f * (c_dev + c_est)); + knee_h = 0.5f * knee; + + /* This is the gain computer. It applies a static compression curve + * to the control signal. + */ + if(x_over <= -knee_h) + y_G = 0.0f; + else if(fabsf(x_over) < knee_h) + y_G = (x_over + knee_h) * (x_over + knee_h) / (2.0f * knee); + else + y_G = x_over; + + x_L = -slope * y_G; + + if(autoAttack) + { + t_att = 2.0f * attack / y2_crest; + a_att = expf(-1.0f / t_att); + } + + if(autoRelease) + { + t_rel = 2.0f * release / y2_crest - t_att; + a_rel = expf(-1.0f / t_rel); + } + + /* Gain smoothing (ballistics) is done via a smooth decoupled peak + * detector. The attack time is subtracted from the release time + * above to compensate for the chained operating mode. + */ + y_1 = maxf(x_L, lerp(x_L, y_1, a_rel)); + y_L = lerp(y_1, y_L, a_att); + + /* Knee width and make-up gain automation make use of a smoothed + * measurement of deviation between the control signal and estimate. + * The estimate is also used to bias the measurement to hot-start its + * average. + */ + c_dev = lerp(-y_L - c_est, c_dev, a_adp); + + if(autoPostGain) + { + /* Clipping reduction is only viable when make-up gain is being + * automated. It modifies the deviation to further attenuate the + * control signal when clipping is detected. The adaptation + * time is sufficiently long enough to suppress further clipping + * at the same output level. + */ + if(autoDeclip) + c_dev = maxf(c_dev, sideChain[i] - y_L - threshold - c_est); + + postGain = -(c_dev + c_est); + } + + sideChain[i] = expf(postGain - y_L); + } + + Comp->LastRelease = y_1; + Comp->LastAttack = y_L; + Comp->LastGainDev = c_dev; +} + +/* Combined with the hold time, a look-ahead delay can improve handling of + * fast transients by allowing the envelope time to converge prior to + * reaching the offending impulse. This is best used when operating as a + * limiter. + */ +static void SignalDelay(Compressor *Comp, const ALsizei SamplesToDo, ALfloat (*restrict OutBuffer)[BUFFERSIZE]) +{ + const ALsizei mask = BUFFERSIZE - 1; + const ALsizei numChans = Comp->NumChans; + const ALsizei indexIn = Comp->DelayIndex; + const ALsizei indexOut = Comp->DelayIndex - Comp->LookAhead; + ALfloat (*restrict delay)[BUFFERSIZE] = Comp->Delay; + ALsizei c, i; + + ASSUME(SamplesToDo > 0); + ASSUME(numChans > 0); + + for(c = 0;c < numChans;c++) + { + for(i = 0;i < SamplesToDo;i++) + { + ALfloat sig = OutBuffer[c][i]; + + OutBuffer[c][i] = delay[c][(indexOut + i) & mask]; + delay[c][(indexIn + i) & mask] = sig; + } + } + + Comp->DelayIndex = (indexIn + SamplesToDo) & mask; +} + +/* The compressor is initialized with the following settings: + * + * NumChans - Number of channels to process. + * SampleRate - Sample rate to process. + * AutoKnee - Whether to automate the knee width parameter. + * AutoAttack - Whether to automate the attack time parameter. + * AutoRelease - Whether to automate the release time parameter. + * AutoPostGain - Whether to automate the make-up (post) gain parameter. + * AutoDeclip - Whether to automate clipping reduction. Ignored when + * not automating make-up gain. + * LookAheadTime - Look-ahead time (in seconds). + * HoldTime - Peak hold-time (in seconds). + * PreGainDb - Gain applied before detection (in dB). + * PostGainDb - Make-up gain applied after compression (in dB). + * ThresholdDb - Triggering threshold (in dB). + * Ratio - Compression ratio (x:1). Set to INFINITY for true + * limiting. Ignored when automating knee width. + * KneeDb - Knee width (in dB). Ignored when automating knee + * width. + * AttackTimeMin - Attack time (in seconds). Acts as a maximum when + * automating attack time. + * ReleaseTimeMin - Release time (in seconds). Acts as a maximum when + * automating release time. + */ +Compressor* CompressorInit(const ALsizei NumChans, const ALuint SampleRate, + const ALboolean AutoKnee, const ALboolean AutoAttack, + const ALboolean AutoRelease, const ALboolean AutoPostGain, + const ALboolean AutoDeclip, const ALfloat LookAheadTime, + const ALfloat HoldTime, const ALfloat PreGainDb, + const ALfloat PostGainDb, const ALfloat ThresholdDb, + const ALfloat Ratio, const ALfloat KneeDb, + const ALfloat AttackTime, const ALfloat ReleaseTime) { Compressor *Comp; + ALsizei lookAhead; + ALsizei hold; size_t size; - ALsizei i; + + lookAhead = (ALsizei)clampf(roundf(LookAheadTime*SampleRate), 0.0f, BUFFERSIZE-1); + hold = (ALsizei)clampf(roundf(HoldTime*SampleRate), 0.0f, BUFFERSIZE-1); + /* The sliding hold implementation doesn't handle a length of 1. A 1-sample + * hold is useless anyway, it would only ever give back what was just given + * to it. + */ + if(hold == 1) + hold = 0; size = sizeof(*Comp); - if(RmsSensing) - size += sizeof(Comp->RmsWindow[0]) * RMS_WINDOW_SIZE; + if(lookAhead > 0) + { + size += sizeof(*Comp->Delay) * NumChans; + if(hold > 0) + size += sizeof(*Comp->Hold); + } + Comp = al_calloc(16, size); - - Comp->PreGain = powf(10.0f, PreGainDb / 20.0f); - Comp->PostGain = powf(10.0f, PostGainDb / 20.0f); - Comp->SummedLink = SummedLink; - Comp->AttackMin = 1.0f / maxf(0.000001f, AttackTimeMin * SampleRate * logf(10.0f)); - Comp->AttackMax = 1.0f / maxf(0.000001f, AttackTimeMax * SampleRate * logf(10.0f)); - Comp->ReleaseMin = -1.0f / maxf(0.000001f, ReleaseTimeMin * SampleRate * logf(10.0f)); - Comp->ReleaseMax = -1.0f / maxf(0.000001f, ReleaseTimeMax * SampleRate * logf(10.0f)); - Comp->Ratio = Ratio; - Comp->Threshold = ThresholdDb / 20.0f; - Comp->Knee = maxf(0.0f, KneeDb / 20.0f); + Comp->NumChans = NumChans; Comp->SampleRate = SampleRate; + Comp->Auto.Knee = AutoKnee; + Comp->Auto.Attack = AutoAttack; + Comp->Auto.Release = AutoRelease; + Comp->Auto.PostGain = AutoPostGain; + Comp->Auto.Declip = AutoPostGain && AutoDeclip; + Comp->LookAhead = lookAhead; + Comp->PreGain = powf(10.0f, PreGainDb / 20.0f); + Comp->PostGain = PostGainDb * logf(10.0f) / 20.0f; + Comp->Threshold = ThresholdDb * logf(10.0f) / 20.0f; + Comp->Slope = 1.0f / maxf(1.0f, Ratio) - 1.0f; + Comp->Knee = maxf(0.0f, KneeDb * logf(10.0f) / 20.0f); + Comp->Attack = maxf(1.0f, AttackTime * SampleRate); + Comp->Release = maxf(1.0f, ReleaseTime * SampleRate); - Comp->RmsSum = 0; - if(RmsSensing) - Comp->RmsWindow = (ALuint*)(Comp+1); - else - Comp->RmsWindow = NULL; - Comp->RmsIndex = 0; + /* Knee width automation actually treats the compressor as a limiter. By + * varying the knee width, it can effectively be seen as applying + * compression over a wide range of ratios. + */ + if(AutoKnee) + Comp->Slope = -1.0f; - for(i = 0;i < BUFFERSIZE;i++) - Comp->Envelope[i] = 0.0f; - Comp->EnvLast = -6.0f; + if(lookAhead > 0) + { + if(hold > 0) + { + Comp->Hold = (SlidingHold*)(Comp + 1); + Comp->Hold->Values[0] = -INFINITY; + Comp->Hold->Expiries[0] = hold; + Comp->Hold->Length = hold; + Comp->Delay = (ALfloat(*)[])(Comp->Hold + 1); + } + else + { + Comp->Delay = (ALfloat(*)[])(Comp + 1); + } + } + + Comp->CrestCoeff = expf(-1.0f / (0.200f * SampleRate)); // 200ms + Comp->GainEstimate = Comp->Threshold * -0.5f * Comp->Slope; + Comp->AdaptCoeff = expf(-1.0f / (2.0f * SampleRate)); // 2s return Comp; } -void ApplyCompression(Compressor *Comp, const ALsizei NumChans, const ALsizei SamplesToDo, - ALfloat (*restrict OutBuffer)[BUFFERSIZE]) +void ApplyCompression(Compressor *Comp, const ALsizei SamplesToDo, ALfloat (*restrict OutBuffer)[BUFFERSIZE]) { + const ALsizei numChans = Comp->NumChans; + const ALfloat preGain = Comp->PreGain; + ALfloat *restrict sideChain; ALsizei c, i; - if(Comp->PreGain != 1.0f) + ASSUME(SamplesToDo > 0); + ASSUME(numChans > 0); + + if(preGain != 1.0f) { - for(c = 0;c < NumChans;c++) + for(c = 0;c < numChans;c++) { for(i = 0;i < SamplesToDo;i++) - OutBuffer[c][i] *= Comp->PreGain; + OutBuffer[c][i] *= preGain; } } - if(Comp->SummedLink) - SumChannels(Comp, NumChans, SamplesToDo, OutBuffer); + LinkChannels(Comp, SamplesToDo, OutBuffer); + + if(Comp->Auto.Attack || Comp->Auto.Release) + CrestDetector(Comp, SamplesToDo); + + if(Comp->Hold) + PeakHoldDetector(Comp, SamplesToDo); else - MaxChannels(Comp, NumChans, SamplesToDo, OutBuffer); + PeakDetector(Comp, SamplesToDo); - if(Comp->RmsWindow) - RmsDetection(Comp, SamplesToDo); - FollowEnvelope(Comp, SamplesToDo); + GainCompressor(Comp, SamplesToDo); - if(Comp->Ratio > 0.0f) - EnvelopeGain(Comp, SamplesToDo, 1.0f - (1.0f / Comp->Ratio)); - else - EnvelopeGain(Comp, SamplesToDo, 1.0f); + if(Comp->Delay) + SignalDelay(Comp, SamplesToDo, OutBuffer); - if(Comp->PostGain != 1.0f) + sideChain = Comp->SideChain; + for(c = 0;c < numChans;c++) { for(i = 0;i < SamplesToDo;i++) - Comp->Envelope[i] *= Comp->PostGain; - } - for(c = 0;c < NumChans;c++) - { - for(i = 0;i < SamplesToDo;i++) - OutBuffer[c][i] *= Comp->Envelope[i]; + OutBuffer[c][i] *= sideChain[i]; } + + memmove(sideChain, sideChain+SamplesToDo, Comp->LookAhead*sizeof(ALfloat)); } + + +ALsizei GetCompressorLookAhead(const Compressor *Comp) +{ return Comp->LookAhead; } diff --git a/Engine/lib/openal-soft/Alc/mastering.h b/Engine/lib/openal-soft/Alc/mastering.h index 0a7b49017..b68b0de17 100644 --- a/Engine/lib/openal-soft/Alc/mastering.h +++ b/Engine/lib/openal-soft/Alc/mastering.h @@ -6,52 +6,44 @@ /* For BUFFERSIZE. */ #include "alMain.h" -typedef struct Compressor { - ALfloat PreGain; - ALfloat PostGain; - ALboolean SummedLink; - ALfloat AttackMin; - ALfloat AttackMax; - ALfloat ReleaseMin; - ALfloat ReleaseMax; - ALfloat Ratio; - ALfloat Threshold; - ALfloat Knee; - ALuint SampleRate; +struct Compressor; - ALuint RmsSum; - ALuint *RmsWindow; - ALsizei RmsIndex; - ALfloat Envelope[BUFFERSIZE]; - ALfloat EnvLast; -} Compressor; - -/* The compressor requires the following information for proper - * initialization: +/* The compressor is initialized with the following settings: * - * PreGainDb - Gain applied before detection (in dB). - * PostGainDb - Gain applied after compression (in dB). - * SummedLink - Whether to use summed (true) or maxed (false) linking. - * RmsSensing - Whether to use RMS (true) or Peak (false) sensing. - * AttackTimeMin - Minimum attack time (in seconds). - * AttackTimeMax - Maximum attack time. Automates when min != max. - * ReleaseTimeMin - Minimum release time (in seconds). - * ReleaseTimeMax - Maximum release time. Automates when min != max. - * Ratio - Compression ratio (x:1). Set to 0 for true limiter. - * ThresholdDb - Triggering threshold (in dB). - * KneeDb - Knee width (below threshold; in dB). + * NumChans - Number of channels to process. * SampleRate - Sample rate to process. + * AutoKnee - Whether to automate the knee width parameter. + * AutoAttack - Whether to automate the attack time parameter. + * AutoRelease - Whether to automate the release time parameter. + * AutoPostGain - Whether to automate the make-up (post) gain parameter. + * AutoDeclip - Whether to automate clipping reduction. Ignored when + * not automating make-up gain. + * LookAheadTime - Look-ahead time (in seconds). + * HoldTime - Peak hold-time (in seconds). + * PreGainDb - Gain applied before detection (in dB). + * PostGainDb - Make-up gain applied after compression (in dB). + * ThresholdDb - Triggering threshold (in dB). + * Ratio - Compression ratio (x:1). Set to INFINIFTY for true + * limiting. Ignored when automating knee width. + * KneeDb - Knee width (in dB). Ignored when automating knee + * width. + * AttackTimeMin - Attack time (in seconds). Acts as a maximum when + * automating attack time. + * ReleaseTimeMin - Release time (in seconds). Acts as a maximum when + * automating release time. */ -Compressor *CompressorInit(const ALfloat PreGainDb, const ALfloat PostGainDb, - const ALboolean SummedLink, const ALboolean RmsSensing, const ALfloat AttackTimeMin, - const ALfloat AttackTimeMax, const ALfloat ReleaseTimeMin, const ALfloat ReleaseTimeMax, - const ALfloat Ratio, const ALfloat ThresholdDb, const ALfloat KneeDb, - const ALuint SampleRate); +struct Compressor* CompressorInit(const ALsizei NumChans, const ALuint SampleRate, + const ALboolean AutoKnee, const ALboolean AutoAttack, + const ALboolean AutoRelease, const ALboolean AutoPostGain, + const ALboolean AutoDeclip, const ALfloat LookAheadTime, + const ALfloat HoldTime, const ALfloat PreGainDb, + const ALfloat PostGainDb, const ALfloat ThresholdDb, + const ALfloat Ratio, const ALfloat KneeDb, + const ALfloat AttackTime, const ALfloat ReleaseTime); -void ApplyCompression(struct Compressor *Comp, const ALsizei NumChans, const ALsizei SamplesToDo, +void ApplyCompression(struct Compressor *Comp, const ALsizei SamplesToDo, ALfloat (*restrict OutBuffer)[BUFFERSIZE]); -inline ALuint GetCompressorSampleRate(const Compressor *Comp) -{ return Comp->SampleRate; } +ALsizei GetCompressorLookAhead(const struct Compressor *Comp); #endif /* MASTERING_H */ diff --git a/Engine/lib/openal-soft/Alc/mixer/defs.h b/Engine/lib/openal-soft/Alc/mixer/defs.h index fe19cef4f..8f6e3755e 100644 --- a/Engine/lib/openal-soft/Alc/mixer/defs.h +++ b/Engine/lib/openal-soft/Alc/mixer/defs.h @@ -62,7 +62,7 @@ void MixRow_SSE(ALfloat *OutBuffer, const ALfloat *Gains, ALsizei InPos, ALsizei BufferSize); /* SSE resamplers */ -inline void InitiatePositionArrays(ALsizei frac, ALint increment, ALsizei *restrict frac_arr, ALint *restrict pos_arr, ALsizei size) +inline void InitiatePositionArrays(ALsizei frac, ALint increment, ALsizei *restrict frac_arr, ALsizei *restrict pos_arr, ALsizei size) { ALsizei i; diff --git a/Engine/lib/openal-soft/Alc/mixer/hrtf_inc.c b/Engine/lib/openal-soft/Alc/mixer/hrtf_inc.c index d6bd8042c..3ef22f240 100644 --- a/Engine/lib/openal-soft/Alc/mixer/hrtf_inc.c +++ b/Engine/lib/openal-soft/Alc/mixer/hrtf_inc.c @@ -22,8 +22,9 @@ void MixHrtf(ALfloat *restrict LeftOut, ALfloat *restrict RightOut, { const ALfloat (*Coeffs)[2] = ASSUME_ALIGNED(hrtfparams->Coeffs, 16); const ALsizei Delay[2] = { hrtfparams->Delay[0], hrtfparams->Delay[1] }; - ALfloat gainstep = hrtfparams->GainStep; - ALfloat gain = hrtfparams->Gain; + const ALfloat gainstep = hrtfparams->GainStep; + const ALfloat gain = hrtfparams->Gain; + ALfloat g, stepcount = 0.0f; ALfloat left, right; ALsizei i; @@ -35,8 +36,10 @@ void MixHrtf(ALfloat *restrict LeftOut, ALfloat *restrict RightOut, for(i = 0;i < BufferSize;i++) { hrtfstate->History[Offset&HRTF_HISTORY_MASK] = *(data++); - left = hrtfstate->History[(Offset-Delay[0])&HRTF_HISTORY_MASK]*gain; - right = hrtfstate->History[(Offset-Delay[1])&HRTF_HISTORY_MASK]*gain; + + g = gain + gainstep*stepcount; + left = hrtfstate->History[(Offset-Delay[0])&HRTF_HISTORY_MASK]*g; + right = hrtfstate->History[(Offset-Delay[1])&HRTF_HISTORY_MASK]*g; hrtfstate->Values[(Offset+IrSize-1)&HRIR_MASK][0] = 0.0f; hrtfstate->Values[(Offset+IrSize-1)&HRIR_MASK][1] = 0.0f; @@ -45,10 +48,10 @@ void MixHrtf(ALfloat *restrict LeftOut, ALfloat *restrict RightOut, *(LeftOut++) += hrtfstate->Values[Offset&HRIR_MASK][0]; *(RightOut++) += hrtfstate->Values[Offset&HRIR_MASK][1]; - gain += gainstep; + stepcount += 1.0f; Offset++; } - hrtfparams->Gain = gain; + hrtfparams->Gain = gain + gainstep*stepcount; } void MixHrtfBlend(ALfloat *restrict LeftOut, ALfloat *restrict RightOut, @@ -59,12 +62,13 @@ void MixHrtfBlend(ALfloat *restrict LeftOut, ALfloat *restrict RightOut, { const ALfloat (*OldCoeffs)[2] = ASSUME_ALIGNED(oldparams->Coeffs, 16); const ALsizei OldDelay[2] = { oldparams->Delay[0], oldparams->Delay[1] }; - ALfloat oldGain = oldparams->Gain; - ALfloat oldGainStep = -oldGain / (ALfloat)BufferSize; + const ALfloat oldGain = oldparams->Gain; + const ALfloat oldGainStep = -oldGain / (ALfloat)BufferSize; const ALfloat (*NewCoeffs)[2] = ASSUME_ALIGNED(newparams->Coeffs, 16); const ALsizei NewDelay[2] = { newparams->Delay[0], newparams->Delay[1] }; - ALfloat newGain = newparams->Gain; - ALfloat newGainStep = newparams->GainStep; + const ALfloat newGain = newparams->Gain; + const ALfloat newGainStep = newparams->GainStep; + ALfloat g, stepcount = 0.0f; ALfloat left, right; ALsizei i; @@ -80,22 +84,23 @@ void MixHrtfBlend(ALfloat *restrict LeftOut, ALfloat *restrict RightOut, hrtfstate->History[Offset&HRTF_HISTORY_MASK] = *(data++); - left = hrtfstate->History[(Offset-OldDelay[0])&HRTF_HISTORY_MASK]*oldGain; - right = hrtfstate->History[(Offset-OldDelay[1])&HRTF_HISTORY_MASK]*oldGain; + g = oldGain + oldGainStep*stepcount; + left = hrtfstate->History[(Offset-OldDelay[0])&HRTF_HISTORY_MASK]*g; + right = hrtfstate->History[(Offset-OldDelay[1])&HRTF_HISTORY_MASK]*g; ApplyCoeffs(Offset, hrtfstate->Values, IrSize, OldCoeffs, left, right); - left = hrtfstate->History[(Offset-NewDelay[0])&HRTF_HISTORY_MASK]*newGain; - right = hrtfstate->History[(Offset-NewDelay[1])&HRTF_HISTORY_MASK]*newGain; + g = newGain + newGainStep*stepcount; + left = hrtfstate->History[(Offset-NewDelay[0])&HRTF_HISTORY_MASK]*g; + right = hrtfstate->History[(Offset-NewDelay[1])&HRTF_HISTORY_MASK]*g; ApplyCoeffs(Offset, hrtfstate->Values, IrSize, NewCoeffs, left, right); *(LeftOut++) += hrtfstate->Values[Offset&HRIR_MASK][0]; *(RightOut++) += hrtfstate->Values[Offset&HRIR_MASK][1]; - oldGain += oldGainStep; - newGain += newGainStep; + stepcount += 1.0f; Offset++; } - newparams->Gain = newGain; + newparams->Gain = newGain + newGainStep*stepcount; } void MixDirectHrtf(ALfloat *restrict LeftOut, ALfloat *restrict RightOut, diff --git a/Engine/lib/openal-soft/Alc/mixer/mixer_c.c b/Engine/lib/openal-soft/Alc/mixer/mixer_c.c index 844852060..14d7c6693 100644 --- a/Engine/lib/openal-soft/Alc/mixer/mixer_c.c +++ b/Engine/lib/openal-soft/Alc/mixer/mixer_c.c @@ -9,12 +9,37 @@ #include "defs.h" -static inline ALfloat do_point(const ALfloat *restrict vals, ALsizei UNUSED(frac)) +static inline ALfloat do_point(const InterpState* UNUSED(state), const ALfloat *restrict vals, ALsizei UNUSED(frac)) { return vals[0]; } -static inline ALfloat do_lerp(const ALfloat *restrict vals, ALsizei frac) +static inline ALfloat do_lerp(const InterpState* UNUSED(state), const ALfloat *restrict vals, ALsizei frac) { return lerp(vals[0], vals[1], frac * (1.0f/FRACTIONONE)); } -static inline ALfloat do_cubic(const ALfloat *restrict vals, ALsizei frac) +static inline ALfloat do_cubic(const InterpState* UNUSED(state), const ALfloat *restrict vals, ALsizei frac) { return cubic(vals[0], vals[1], vals[2], vals[3], frac * (1.0f/FRACTIONONE)); } +static inline ALfloat do_bsinc(const InterpState *state, const ALfloat *restrict vals, ALsizei frac) +{ + const ALfloat *fil, *scd, *phd, *spd; + ALsizei j_f, pi; + ALfloat pf, r; + + ASSUME(state->bsinc.m > 0); + + // Calculate the phase index and factor. +#define FRAC_PHASE_BITDIFF (FRACTIONBITS-BSINC_PHASE_BITS) + pi = frac >> FRAC_PHASE_BITDIFF; + pf = (frac & ((1<bsinc.filter + state->bsinc.m*pi*4, 16); + scd = ASSUME_ALIGNED(fil + state->bsinc.m, 16); + phd = ASSUME_ALIGNED(scd + state->bsinc.m, 16); + spd = ASSUME_ALIGNED(phd + state->bsinc.m, 16); + + // Apply the scale and phase interpolated filter. + r = 0.0f; + for(j_f = 0;j_f < state->bsinc.m;j_f++) + r += (fil[j_f] + state->bsinc.sf*scd[j_f] + pf*(phd[j_f] + state->bsinc.sf*spd[j_f])) * vals[j_f]; + return r; +} const ALfloat *Resample_copy_C(const InterpState* UNUSED(state), const ALfloat *restrict src, ALsizei UNUSED(frac), ALint UNUSED(increment), @@ -30,16 +55,19 @@ const ALfloat *Resample_copy_C(const InterpState* UNUSED(state), } #define DECL_TEMPLATE(Tag, Sampler, O) \ -const ALfloat *Resample_##Tag##_C(const InterpState* UNUSED(state), \ +const ALfloat *Resample_##Tag##_C(const InterpState *state, \ const ALfloat *restrict src, ALsizei frac, ALint increment, \ ALfloat *restrict dst, ALsizei numsamples) \ { \ + const InterpState istate = *state; \ ALsizei i; \ \ + ASSUME(numsamples > 0); \ + \ src -= O; \ for(i = 0;i < numsamples;i++) \ { \ - dst[i] = Sampler(src, frac); \ + dst[i] = Sampler(&istate, src, frac); \ \ frac += increment; \ src += frac>>FRACTIONBITS; \ @@ -51,49 +79,10 @@ const ALfloat *Resample_##Tag##_C(const InterpState* UNUSED(state), \ DECL_TEMPLATE(point, do_point, 0) DECL_TEMPLATE(lerp, do_lerp, 0) DECL_TEMPLATE(cubic, do_cubic, 1) +DECL_TEMPLATE(bsinc, do_bsinc, istate.bsinc.l) #undef DECL_TEMPLATE -const ALfloat *Resample_bsinc_C(const InterpState *state, const ALfloat *restrict src, - ALsizei frac, ALint increment, ALfloat *restrict dst, - ALsizei dstlen) -{ - const ALfloat *fil, *scd, *phd, *spd; - const ALfloat *const filter = state->bsinc.filter; - const ALfloat sf = state->bsinc.sf; - const ALsizei m = state->bsinc.m; - ALsizei j_f, pi, i; - ALfloat pf, r; - - ASSUME(m > 0); - - src += state->bsinc.l; - for(i = 0;i < dstlen;i++) - { - // Calculate the phase index and factor. -#define FRAC_PHASE_BITDIFF (FRACTIONBITS-BSINC_PHASE_BITS) - pi = frac >> FRAC_PHASE_BITDIFF; - pf = (frac & ((1<>FRACTIONBITS; - frac &= FRACTIONMASK; - } - return dst; -} - static inline void ApplyCoeffs(ALsizei Offset, ALfloat (*restrict Values)[2], const ALsizei IrSize, @@ -119,28 +108,32 @@ void Mix_C(const ALfloat *data, ALsizei OutChans, ALfloat (*restrict OutBuffer)[ ALfloat *CurrentGains, const ALfloat *TargetGains, ALsizei Counter, ALsizei OutPos, ALsizei BufferSize) { - ALfloat gain, delta, step; + const ALfloat delta = (Counter > 0) ? 1.0f/(ALfloat)Counter : 0.0f; ALsizei c; ASSUME(OutChans > 0); ASSUME(BufferSize > 0); - delta = (Counter > 0) ? 1.0f/(ALfloat)Counter : 0.0f; for(c = 0;c < OutChans;c++) { ALsizei pos = 0; - gain = CurrentGains[c]; - step = (TargetGains[c] - gain) * delta; - if(fabsf(step) > FLT_EPSILON) + ALfloat gain = CurrentGains[c]; + const ALfloat diff = TargetGains[c] - gain; + + if(fabsf(diff) > FLT_EPSILON) { ALsizei minsize = mini(BufferSize, Counter); + const ALfloat step = diff * delta; + ALfloat step_count = 0.0f; for(;pos < minsize;pos++) { - OutBuffer[c][OutPos+pos] += data[pos]*gain; - gain += step; + OutBuffer[c][OutPos+pos] += data[pos] * (gain + step*step_count); + step_count += 1.0f; } if(pos == Counter) gain = TargetGains[c]; + else + gain += step*step_count; CurrentGains[c] = gain; } @@ -166,7 +159,7 @@ void MixRow_C(ALfloat *OutBuffer, const ALfloat *Gains, const ALfloat (*restrict for(c = 0;c < InChans;c++) { - ALfloat gain = Gains[c]; + const ALfloat gain = Gains[c]; if(!(fabsf(gain) > GAIN_SILENCE_THRESHOLD)) continue; diff --git a/Engine/lib/openal-soft/Alc/mixer/mixer_neon.c b/Engine/lib/openal-soft/Alc/mixer/mixer_neon.c index 1a5e8ee77..9bf5521a7 100644 --- a/Engine/lib/openal-soft/Alc/mixer/mixer_neon.c +++ b/Engine/lib/openal-soft/Alc/mixer/mixer_neon.c @@ -17,23 +17,25 @@ const ALfloat *Resample_lerp_Neon(const InterpState* UNUSED(state), const int32x4_t increment4 = vdupq_n_s32(increment*4); const float32x4_t fracOne4 = vdupq_n_f32(1.0f/FRACTIONONE); const int32x4_t fracMask4 = vdupq_n_s32(FRACTIONMASK); - alignas(16) ALint pos_[4]; - alignas(16) ALsizei frac_[4]; - int32x4_t pos4; - int32x4_t frac4; - ALsizei i; + alignas(16) ALsizei pos_[4], frac_[4]; + int32x4_t pos4, frac4; + ALsizei todo, pos, i; ASSUME(numsamples > 0); InitiatePositionArrays(frac, increment, frac_, pos_, 4); - frac4 = vld1q_s32(frac_); pos4 = vld1q_s32(pos_); - for(i = 0;numsamples-i > 3;i += 4) + todo = numsamples & ~3; + for(i = 0;i < todo;i += 4) { - const float32x4_t val1 = (float32x4_t){src[pos_[0]], src[pos_[1]], src[pos_[2]], src[pos_[3]]}; - const float32x4_t val2 = (float32x4_t){src[pos_[0]+1], src[pos_[1]+1], src[pos_[2]+1], src[pos_[3]+1]}; + const int pos0 = vgetq_lane_s32(pos4, 0); + const int pos1 = vgetq_lane_s32(pos4, 1); + const int pos2 = vgetq_lane_s32(pos4, 2); + const int pos3 = vgetq_lane_s32(pos4, 3); + const float32x4_t val1 = (float32x4_t){src[pos0], src[pos1], src[pos2], src[pos3]}; + const float32x4_t val2 = (float32x4_t){src[pos0+1], src[pos1+1], src[pos2+1], src[pos3+1]}; /* val1 + (val2-val1)*mu */ const float32x4_t r0 = vsubq_f32(val2, val1); @@ -45,25 +47,21 @@ const ALfloat *Resample_lerp_Neon(const InterpState* UNUSED(state), frac4 = vaddq_s32(frac4, increment4); pos4 = vaddq_s32(pos4, vshrq_n_s32(frac4, FRACTIONBITS)); frac4 = vandq_s32(frac4, fracMask4); - - vst1q_s32(pos_, pos4); } - if(i < numsamples) - { - /* NOTE: These four elements represent the position *after* the last - * four samples, so the lowest element is the next position to - * resample. - */ - ALint pos = pos_[0]; - frac = vgetq_lane_s32(frac4, 0); - do { - dst[i] = lerp(src[pos], src[pos+1], frac * (1.0f/FRACTIONONE)); + /* NOTE: These four elements represent the position *after* the last four + * samples, so the lowest element is the next position to resample. + */ + pos = vgetq_lane_s32(pos4, 0); + frac = vgetq_lane_s32(frac4, 0); - frac += increment; - pos += frac>>FRACTIONBITS; - frac &= FRACTIONMASK; - } while(++i < numsamples); + for(;i < numsamples;++i) + { + dst[i] = lerp(src[pos], src[pos+1], frac * (1.0f/FRACTIONONE)); + + frac += increment; + pos += frac>>FRACTIONBITS; + frac &= FRACTIONMASK; } return dst; } @@ -83,7 +81,7 @@ const ALfloat *Resample_bsinc_Neon(const InterpState *state, ASSUME(m > 0); ASSUME(dstlen > 0); - src += state->bsinc.l; + src -= state->bsinc.l; for(i = 0;i < dstlen;i++) { // Calculate the phase index and factor. @@ -101,16 +99,20 @@ const ALfloat *Resample_bsinc_Neon(const InterpState *state, // Apply the scale and phase interpolated filter. r4 = vdupq_n_f32(0.0f); { + const ALsizei count = m >> 2; const float32x4_t pf4 = vdupq_n_f32(pf); - for(j = 0;j < m;j+=4,fil++,scd++,phd++,spd++) + + ASSUME(count > 0); + + for(j = 0;j < count;j++) { /* f = ((fil + sf*scd) + pf*(phd + sf*spd)) */ const float32x4_t f4 = vmlaq_f32( - vmlaq_f32(*fil, sf4, *scd), - pf4, vmlaq_f32(*phd, sf4, *spd) + vmlaq_f32(fil[j], sf4, scd[j]), + pf4, vmlaq_f32(phd[j], sf4, spd[j]) ); /* r += f*src */ - r4 = vmlaq_f32(r4, f4, vld1q_f32(&src[j])); + r4 = vmlaq_f32(r4, f4, vld1q_f32(&src[j*4])); } } r4 = vaddq_f32(r4, vcombine_f32(vrev64_f32(vget_high_f32(r4)), @@ -165,8 +167,7 @@ void Mix_Neon(const ALfloat *data, ALsizei OutChans, ALfloat (*restrict OutBuffe ALfloat *CurrentGains, const ALfloat *TargetGains, ALsizei Counter, ALsizei OutPos, ALsizei BufferSize) { - ALfloat gain, delta, step; - float32x4_t gain4; + const ALfloat delta = (Counter > 0) ? 1.0f/(ALfloat)Counter : 0.0f; ALsizei c; ASSUME(OutChans > 0); @@ -174,47 +175,55 @@ void Mix_Neon(const ALfloat *data, ALsizei OutChans, ALfloat (*restrict OutBuffe data = ASSUME_ALIGNED(data, 16); OutBuffer = ASSUME_ALIGNED(OutBuffer, 16); - delta = (Counter > 0) ? 1.0f/(ALfloat)Counter : 0.0f; - for(c = 0;c < OutChans;c++) { ALsizei pos = 0; - gain = CurrentGains[c]; - step = (TargetGains[c] - gain) * delta; - if(fabsf(step) > FLT_EPSILON) + ALfloat gain = CurrentGains[c]; + const ALfloat diff = TargetGains[c] - gain; + + if(fabsf(diff) > FLT_EPSILON) { ALsizei minsize = mini(BufferSize, Counter); + const ALfloat step = diff * delta; + ALfloat step_count = 0.0f; /* Mix with applying gain steps in aligned multiples of 4. */ - if(minsize-pos > 3) + if(LIKELY(minsize > 3)) { - float32x4_t step4; - gain4 = vsetq_lane_f32(gain, gain4, 0); - gain4 = vsetq_lane_f32(gain + step, gain4, 1); - gain4 = vsetq_lane_f32(gain + step + step, gain4, 2); - gain4 = vsetq_lane_f32(gain + step + step + step, gain4, 3); - step4 = vdupq_n_f32(step + step + step + step); + const float32x4_t four4 = vdupq_n_f32(4.0f); + const float32x4_t step4 = vdupq_n_f32(step); + const float32x4_t gain4 = vdupq_n_f32(gain); + float32x4_t step_count4 = vsetq_lane_f32(0.0f, + vsetq_lane_f32(1.0f, + vsetq_lane_f32(2.0f, + vsetq_lane_f32(3.0f, vdupq_n_f32(0.0f), 3), + 2), 1), 0 + ); + ALsizei todo = minsize >> 2; + do { const float32x4_t val4 = vld1q_f32(&data[pos]); float32x4_t dry4 = vld1q_f32(&OutBuffer[c][OutPos+pos]); - dry4 = vmlaq_f32(dry4, val4, gain4); - gain4 = vaddq_f32(gain4, step4); + dry4 = vmlaq_f32(dry4, val4, vmlaq_f32(gain4, step4, step_count4)); + step_count4 = vaddq_f32(step_count4, four4); vst1q_f32(&OutBuffer[c][OutPos+pos], dry4); pos += 4; - } while(minsize-pos > 3); - /* NOTE: gain4 now represents the next four gains after the - * last four mixed samples, so the lowest element represents - * the next gain to apply. + } while(--todo); + /* NOTE: step_count4 now represents the next four counts after + * the last four mixed samples, so the lowest element + * represents the next step count to apply. */ - gain = vgetq_lane_f32(gain4, 0); + step_count = vgetq_lane_f32(step_count4, 0); } /* Mix with applying left over gain steps that aren't aligned multiples of 4. */ for(;pos < minsize;pos++) { - OutBuffer[c][OutPos+pos] += data[pos]*gain; - gain += step; + OutBuffer[c][OutPos+pos] += data[pos]*(gain + step*step_count); + step_count += 1.0f; } if(pos == Counter) gain = TargetGains[c]; + else + gain += step*step_count; CurrentGains[c] = gain; /* Mix until pos is aligned with 4 or the mix is done. */ @@ -225,13 +234,17 @@ void Mix_Neon(const ALfloat *data, ALsizei OutChans, ALfloat (*restrict OutBuffe if(!(fabsf(gain) > GAIN_SILENCE_THRESHOLD)) continue; - gain4 = vdupq_n_f32(gain); - for(;BufferSize-pos > 3;pos += 4) + if(LIKELY(BufferSize-pos > 3)) { - const float32x4_t val4 = vld1q_f32(&data[pos]); - float32x4_t dry4 = vld1q_f32(&OutBuffer[c][OutPos+pos]); - dry4 = vmlaq_f32(dry4, val4, gain4); - vst1q_f32(&OutBuffer[c][OutPos+pos], dry4); + ALsizei todo = (BufferSize-pos) >> 2; + const float32x4_t gain4 = vdupq_n_f32(gain); + do { + const float32x4_t val4 = vld1q_f32(&data[pos]); + float32x4_t dry4 = vld1q_f32(&OutBuffer[c][OutPos+pos]); + dry4 = vmlaq_f32(dry4, val4, gain4); + vst1q_f32(&OutBuffer[c][OutPos+pos], dry4); + pos += 4; + } while(--todo); } for(;pos < BufferSize;pos++) OutBuffer[c][OutPos+pos] += data[pos]*gain; @@ -240,28 +253,29 @@ void Mix_Neon(const ALfloat *data, ALsizei OutChans, ALfloat (*restrict OutBuffe void MixRow_Neon(ALfloat *OutBuffer, const ALfloat *Gains, const ALfloat (*restrict data)[BUFFERSIZE], ALsizei InChans, ALsizei InPos, ALsizei BufferSize) { - float32x4_t gain4; ALsizei c; ASSUME(InChans > 0); ASSUME(BufferSize > 0); - data = ASSUME_ALIGNED(data, 16); - OutBuffer = ASSUME_ALIGNED(OutBuffer, 16); for(c = 0;c < InChans;c++) { ALsizei pos = 0; - ALfloat gain = Gains[c]; + const ALfloat gain = Gains[c]; if(!(fabsf(gain) > GAIN_SILENCE_THRESHOLD)) continue; - gain4 = vdupq_n_f32(gain); - for(;BufferSize-pos > 3;pos += 4) + if(LIKELY(BufferSize > 3)) { - const float32x4_t val4 = vld1q_f32(&data[c][InPos+pos]); - float32x4_t dry4 = vld1q_f32(&OutBuffer[pos]); - dry4 = vmlaq_f32(dry4, val4, gain4); - vst1q_f32(&OutBuffer[pos], dry4); + ALsizei todo = BufferSize >> 2; + float32x4_t gain4 = vdupq_n_f32(gain); + do { + const float32x4_t val4 = vld1q_f32(&data[c][InPos+pos]); + float32x4_t dry4 = vld1q_f32(&OutBuffer[pos]); + dry4 = vmlaq_f32(dry4, val4, gain4); + vst1q_f32(&OutBuffer[pos], dry4); + pos += 4; + } while(--todo); } for(;pos < BufferSize;pos++) OutBuffer[pos] += data[c][InPos+pos]*gain; diff --git a/Engine/lib/openal-soft/Alc/mixer/mixer_sse.c b/Engine/lib/openal-soft/Alc/mixer/mixer_sse.c index a178477f8..725a5ebc9 100644 --- a/Engine/lib/openal-soft/Alc/mixer/mixer_sse.c +++ b/Engine/lib/openal-soft/Alc/mixer/mixer_sse.c @@ -27,7 +27,7 @@ const ALfloat *Resample_bsinc_SSE(const InterpState *state, const ALfloat *restr ASSUME(m > 0); ASSUME(dstlen > 0); - src += state->bsinc.l; + src -= state->bsinc.l; for(i = 0;i < dstlen;i++) { // Calculate the phase index and factor. @@ -45,17 +45,21 @@ const ALfloat *Resample_bsinc_SSE(const InterpState *state, const ALfloat *restr // Apply the scale and phase interpolated filter. r4 = _mm_setzero_ps(); { + const ALsizei count = m >> 2; const __m128 pf4 = _mm_set1_ps(pf); + + ASSUME(count > 0); + #define MLA4(x, y, z) _mm_add_ps(x, _mm_mul_ps(y, z)) - for(j = 0;j < m;j+=4,fil++,scd++,phd++,spd++) + for(j = 0;j < count;j++) { /* f = ((fil + sf*scd) + pf*(phd + sf*spd)) */ const __m128 f4 = MLA4( - MLA4(*fil, sf4, *scd), - pf4, MLA4(*phd, sf4, *spd) + MLA4(fil[j], sf4, scd[j]), + pf4, MLA4(phd[j], sf4, spd[j]) ); /* r += f*src */ - r4 = MLA4(r4, f4, _mm_loadu_ps(&src[j])); + r4 = MLA4(r4, f4, _mm_loadu_ps(&src[j*4])); } #undef MLA4 } @@ -135,55 +139,58 @@ void Mix_SSE(const ALfloat *data, ALsizei OutChans, ALfloat (*restrict OutBuffer ALfloat *CurrentGains, const ALfloat *TargetGains, ALsizei Counter, ALsizei OutPos, ALsizei BufferSize) { - ALfloat gain, delta, step; - __m128 gain4; + const ALfloat delta = (Counter > 0) ? 1.0f/(ALfloat)Counter : 0.0f; ALsizei c; ASSUME(OutChans > 0); ASSUME(BufferSize > 0); - delta = (Counter > 0) ? 1.0f/(ALfloat)Counter : 0.0f; for(c = 0;c < OutChans;c++) { ALsizei pos = 0; - gain = CurrentGains[c]; - step = (TargetGains[c] - gain) * delta; - if(fabsf(step) > FLT_EPSILON) + ALfloat gain = CurrentGains[c]; + const ALfloat diff = TargetGains[c] - gain; + + if(fabsf(diff) > FLT_EPSILON) { ALsizei minsize = mini(BufferSize, Counter); + const ALfloat step = diff * delta; + ALfloat step_count = 0.0f; /* Mix with applying gain steps in aligned multiples of 4. */ - if(minsize-pos > 3) + if(LIKELY(minsize > 3)) { - __m128 step4; - gain4 = _mm_setr_ps( - gain, - gain + step, - gain + step + step, - gain + step + step + step - ); - step4 = _mm_set1_ps(step + step + step + step); + const __m128 four4 = _mm_set1_ps(4.0f); + const __m128 step4 = _mm_set1_ps(step); + const __m128 gain4 = _mm_set1_ps(gain); + __m128 step_count4 = _mm_setr_ps(0.0f, 1.0f, 2.0f, 3.0f); + ALsizei todo = minsize >> 2; do { const __m128 val4 = _mm_load_ps(&data[pos]); __m128 dry4 = _mm_load_ps(&OutBuffer[c][OutPos+pos]); - dry4 = _mm_add_ps(dry4, _mm_mul_ps(val4, gain4)); - gain4 = _mm_add_ps(gain4, step4); +#define MLA4(x, y, z) _mm_add_ps(x, _mm_mul_ps(y, z)) + /* dry += val * (gain + step*step_count) */ + dry4 = MLA4(dry4, val4, MLA4(gain4, step4, step_count4)); +#undef MLA4 _mm_store_ps(&OutBuffer[c][OutPos+pos], dry4); + step_count4 = _mm_add_ps(step_count4, four4); pos += 4; - } while(minsize-pos > 3); - /* NOTE: gain4 now represents the next four gains after the - * last four mixed samples, so the lowest element represents - * the next gain to apply. + } while(--todo); + /* NOTE: step_count4 now represents the next four counts after + * the last four mixed samples, so the lowest element + * represents the next step count to apply. */ - gain = _mm_cvtss_f32(gain4); + step_count = _mm_cvtss_f32(step_count4); } /* Mix with applying left over gain steps that aren't aligned multiples of 4. */ for(;pos < minsize;pos++) { - OutBuffer[c][OutPos+pos] += data[pos]*gain; - gain += step; + OutBuffer[c][OutPos+pos] += data[pos]*(gain + step*step_count); + step_count += 1.0f; } if(pos == Counter) gain = TargetGains[c]; + else + gain += step*step_count; CurrentGains[c] = gain; /* Mix until pos is aligned with 4 or the mix is done. */ @@ -194,13 +201,17 @@ void Mix_SSE(const ALfloat *data, ALsizei OutChans, ALfloat (*restrict OutBuffer if(!(fabsf(gain) > GAIN_SILENCE_THRESHOLD)) continue; - gain4 = _mm_set1_ps(gain); - for(;BufferSize-pos > 3;pos += 4) + if(LIKELY(BufferSize-pos > 3)) { - const __m128 val4 = _mm_load_ps(&data[pos]); - __m128 dry4 = _mm_load_ps(&OutBuffer[c][OutPos+pos]); - dry4 = _mm_add_ps(dry4, _mm_mul_ps(val4, gain4)); - _mm_store_ps(&OutBuffer[c][OutPos+pos], dry4); + ALsizei todo = (BufferSize-pos) >> 2; + const __m128 gain4 = _mm_set1_ps(gain); + do { + const __m128 val4 = _mm_load_ps(&data[pos]); + __m128 dry4 = _mm_load_ps(&OutBuffer[c][OutPos+pos]); + dry4 = _mm_add_ps(dry4, _mm_mul_ps(val4, gain4)); + _mm_store_ps(&OutBuffer[c][OutPos+pos], dry4); + pos += 4; + } while(--todo); } for(;pos < BufferSize;pos++) OutBuffer[c][OutPos+pos] += data[pos]*gain; @@ -209,7 +220,6 @@ void Mix_SSE(const ALfloat *data, ALsizei OutChans, ALfloat (*restrict OutBuffer void MixRow_SSE(ALfloat *OutBuffer, const ALfloat *Gains, const ALfloat (*restrict data)[BUFFERSIZE], ALsizei InChans, ALsizei InPos, ALsizei BufferSize) { - __m128 gain4; ALsizei c; ASSUME(InChans > 0); @@ -218,17 +228,21 @@ void MixRow_SSE(ALfloat *OutBuffer, const ALfloat *Gains, const ALfloat (*restri for(c = 0;c < InChans;c++) { ALsizei pos = 0; - ALfloat gain = Gains[c]; + const ALfloat gain = Gains[c]; if(!(fabsf(gain) > GAIN_SILENCE_THRESHOLD)) continue; - gain4 = _mm_set1_ps(gain); - for(;BufferSize-pos > 3;pos += 4) + if(LIKELY(BufferSize > 3)) { - const __m128 val4 = _mm_load_ps(&data[c][InPos+pos]); - __m128 dry4 = _mm_load_ps(&OutBuffer[pos]); - dry4 = _mm_add_ps(dry4, _mm_mul_ps(val4, gain4)); - _mm_store_ps(&OutBuffer[pos], dry4); + ALsizei todo = BufferSize >> 2; + const __m128 gain4 = _mm_set1_ps(gain); + do { + const __m128 val4 = _mm_load_ps(&data[c][InPos+pos]); + __m128 dry4 = _mm_load_ps(&OutBuffer[pos]); + dry4 = _mm_add_ps(dry4, _mm_mul_ps(val4, gain4)); + _mm_store_ps(&OutBuffer[pos], dry4); + pos += 4; + } while(--todo); } for(;pos < BufferSize;pos++) OutBuffer[pos] += data[c][InPos+pos]*gain; diff --git a/Engine/lib/openal-soft/Alc/mixer/mixer_sse2.c b/Engine/lib/openal-soft/Alc/mixer/mixer_sse2.c index 4aeb6fc4c..9cbaeb0ad 100644 --- a/Engine/lib/openal-soft/Alc/mixer/mixer_sse2.c +++ b/Engine/lib/openal-soft/Alc/mixer/mixer_sse2.c @@ -34,23 +34,25 @@ const ALfloat *Resample_lerp_SSE2(const InterpState* UNUSED(state), const __m128i increment4 = _mm_set1_epi32(increment*4); const __m128 fracOne4 = _mm_set1_ps(1.0f/FRACTIONONE); const __m128i fracMask4 = _mm_set1_epi32(FRACTIONMASK); - union { alignas(16) ALint i[4]; float f[4]; } pos_; - union { alignas(16) ALsizei i[4]; float f[4]; } frac_; + alignas(16) ALsizei pos_[4], frac_[4]; __m128i frac4, pos4; - ALint pos; - ALsizei i; + ALsizei todo, pos, i; ASSUME(numsamples > 0); - InitiatePositionArrays(frac, increment, frac_.i, pos_.i, 4); + InitiatePositionArrays(frac, increment, frac_, pos_, 4); + frac4 = _mm_setr_epi32(frac_[0], frac_[1], frac_[2], frac_[3]); + pos4 = _mm_setr_epi32(pos_[0], pos_[1], pos_[2], pos_[3]); - frac4 = _mm_castps_si128(_mm_load_ps(frac_.f)); - pos4 = _mm_castps_si128(_mm_load_ps(pos_.f)); - - for(i = 0;numsamples-i > 3;i += 4) + todo = numsamples & ~3; + for(i = 0;i < todo;i += 4) { - const __m128 val1 = _mm_setr_ps(src[pos_.i[0]], src[pos_.i[1]], src[pos_.i[2]], src[pos_.i[3]]); - const __m128 val2 = _mm_setr_ps(src[pos_.i[0]+1], src[pos_.i[1]+1], src[pos_.i[2]+1], src[pos_.i[3]+1]); + const int pos0 = _mm_cvtsi128_si32(_mm_shuffle_epi32(pos4, _MM_SHUFFLE(0, 0, 0, 0))); + const int pos1 = _mm_cvtsi128_si32(_mm_shuffle_epi32(pos4, _MM_SHUFFLE(1, 1, 1, 1))); + const int pos2 = _mm_cvtsi128_si32(_mm_shuffle_epi32(pos4, _MM_SHUFFLE(2, 2, 2, 2))); + const int pos3 = _mm_cvtsi128_si32(_mm_shuffle_epi32(pos4, _MM_SHUFFLE(3, 3, 3, 3))); + const __m128 val1 = _mm_setr_ps(src[pos0 ], src[pos1 ], src[pos2 ], src[pos3 ]); + const __m128 val2 = _mm_setr_ps(src[pos0+1], src[pos1+1], src[pos2+1], src[pos3+1]); /* val1 + (val2-val1)*mu */ const __m128 r0 = _mm_sub_ps(val2, val1); @@ -62,17 +64,15 @@ const ALfloat *Resample_lerp_SSE2(const InterpState* UNUSED(state), frac4 = _mm_add_epi32(frac4, increment4); pos4 = _mm_add_epi32(pos4, _mm_srli_epi32(frac4, FRACTIONBITS)); frac4 = _mm_and_si128(frac4, fracMask4); - - _mm_store_ps(pos_.f, _mm_castsi128_ps(pos4)); } /* NOTE: These four elements represent the position *after* the last four * samples, so the lowest element is the next position to resample. */ - pos = pos_.i[0]; + pos = _mm_cvtsi128_si32(pos4); frac = _mm_cvtsi128_si32(frac4); - for(;i < numsamples;i++) + for(;i < numsamples;++i) { dst[i] = lerp(src[pos], src[pos+1], frac * (1.0f/FRACTIONONE)); diff --git a/Engine/lib/openal-soft/Alc/mixer/mixer_sse41.c b/Engine/lib/openal-soft/Alc/mixer/mixer_sse41.c index 98a3ef74b..e92a3dd00 100644 --- a/Engine/lib/openal-soft/Alc/mixer/mixer_sse41.c +++ b/Engine/lib/openal-soft/Alc/mixer/mixer_sse41.c @@ -35,23 +35,25 @@ const ALfloat *Resample_lerp_SSE41(const InterpState* UNUSED(state), const __m128i increment4 = _mm_set1_epi32(increment*4); const __m128 fracOne4 = _mm_set1_ps(1.0f/FRACTIONONE); const __m128i fracMask4 = _mm_set1_epi32(FRACTIONMASK); - union { alignas(16) ALint i[4]; float f[4]; } pos_; - union { alignas(16) ALsizei i[4]; float f[4]; } frac_; + alignas(16) ALsizei pos_[4], frac_[4]; __m128i frac4, pos4; - ALint pos; - ALsizei i; + ALsizei todo, pos, i; ASSUME(numsamples > 0); - InitiatePositionArrays(frac, increment, frac_.i, pos_.i, 4); + InitiatePositionArrays(frac, increment, frac_, pos_, 4); + frac4 = _mm_setr_epi32(frac_[0], frac_[1], frac_[2], frac_[3]); + pos4 = _mm_setr_epi32(pos_[0], pos_[1], pos_[2], pos_[3]); - frac4 = _mm_castps_si128(_mm_load_ps(frac_.f)); - pos4 = _mm_castps_si128(_mm_load_ps(pos_.f)); - - for(i = 0;numsamples-i > 3;i += 4) + todo = numsamples & ~3; + for(i = 0;i < todo;i += 4) { - const __m128 val1 = _mm_setr_ps(src[pos_.i[0]], src[pos_.i[1]], src[pos_.i[2]], src[pos_.i[3]]); - const __m128 val2 = _mm_setr_ps(src[pos_.i[0]+1], src[pos_.i[1]+1], src[pos_.i[2]+1], src[pos_.i[3]+1]); + const int pos0 = _mm_extract_epi32(pos4, 0); + const int pos1 = _mm_extract_epi32(pos4, 1); + const int pos2 = _mm_extract_epi32(pos4, 2); + const int pos3 = _mm_extract_epi32(pos4, 3); + const __m128 val1 = _mm_setr_ps(src[pos0 ], src[pos1 ], src[pos2 ], src[pos3 ]); + const __m128 val2 = _mm_setr_ps(src[pos0+1], src[pos1+1], src[pos2+1], src[pos3+1]); /* val1 + (val2-val1)*mu */ const __m128 r0 = _mm_sub_ps(val2, val1); @@ -63,20 +65,15 @@ const ALfloat *Resample_lerp_SSE41(const InterpState* UNUSED(state), frac4 = _mm_add_epi32(frac4, increment4); pos4 = _mm_add_epi32(pos4, _mm_srli_epi32(frac4, FRACTIONBITS)); frac4 = _mm_and_si128(frac4, fracMask4); - - pos_.i[0] = _mm_extract_epi32(pos4, 0); - pos_.i[1] = _mm_extract_epi32(pos4, 1); - pos_.i[2] = _mm_extract_epi32(pos4, 2); - pos_.i[3] = _mm_extract_epi32(pos4, 3); } /* NOTE: These four elements represent the position *after* the last four * samples, so the lowest element is the next position to resample. */ - pos = pos_.i[0]; + pos = _mm_cvtsi128_si32(pos4); frac = _mm_cvtsi128_si32(frac4); - for(;i < numsamples;i++) + for(;i < numsamples;++i) { dst[i] = lerp(src[pos], src[pos+1], frac * (1.0f/FRACTIONONE)); diff --git a/Engine/lib/openal-soft/Alc/mixvoice.c b/Engine/lib/openal-soft/Alc/mixvoice.c index 2d935ce5e..d019b898c 100644 --- a/Engine/lib/openal-soft/Alc/mixvoice.c +++ b/Engine/lib/openal-soft/Alc/mixvoice.c @@ -45,7 +45,7 @@ static_assert((INT_MAX>>FRACTIONBITS)/MAX_PITCH > BUFFERSIZE, "MAX_PITCH and/or BUFFERSIZE are too large for FRACTIONBITS!"); -extern inline void InitiatePositionArrays(ALsizei frac, ALint increment, ALsizei *restrict frac_arr, ALint *restrict pos_arr, ALsizei size); +extern inline void InitiatePositionArrays(ALsizei frac, ALint increment, ALsizei *restrict frac_arr, ALsizei *restrict pos_arr, ALsizei size); /* BSinc24 requires up to 23 extra samples before the current position, and 24 after. */ @@ -197,12 +197,11 @@ void aluInitMixer(void) static void SendAsyncEvent(ALCcontext *context, ALuint enumtype, ALenum type, ALuint objid, ALuint param, const char *msg) { - AsyncEvent evt; - evt.EnumType = enumtype; - evt.Type = type; - evt.ObjectId = objid; - evt.Param = param; - strcpy(evt.Message, msg); + AsyncEvent evt = ASYNC_EVENT(enumtype); + evt.u.user.type = type; + evt.u.user.id = objid; + evt.u.user.param = param; + strcpy(evt.u.user.msg, msg); if(ll_ringbuffer_write(context->AsyncEvents, (const char*)&evt, 1) == 1) alsem_post(&context->EventSem); } @@ -487,6 +486,7 @@ ALboolean MixSource(ALvoice *voice, ALuint SourceID, ALCcontext *Context, ALsize while(tmpiter && SrcBufferSize > FilledAmt) { ALsizei SizeToDo = SrcBufferSize - FilledAmt; + ALsizei CompLen = 0; ALsizei i; for(i = 0;i < tmpiter->num_buffers;i++) @@ -499,23 +499,24 @@ ALboolean MixSource(ALvoice *voice, ALuint SourceID, ALCcontext *Context, ALsize const ALubyte *Data = ALBuffer->data; Data += (pos*NumChannels + chan)*SampleSize; - DataSize = minu(SizeToDo, DataSize - pos); + DataSize = mini(SizeToDo, DataSize - pos); + CompLen = maxi(CompLen, DataSize); + LoadSamples(&SrcData[FilledAmt], Data, NumChannels, ALBuffer->FmtType, DataSize); } } - if(pos > tmpiter->max_samples) + if(UNLIKELY(!CompLen)) pos -= tmpiter->max_samples; else { - FilledAmt += tmpiter->max_samples - pos; + FilledAmt += CompLen; + if(SrcBufferSize <= FilledAmt) + break; pos = 0; } - if(SrcBufferSize > FilledAmt) - { - tmpiter = ATOMIC_LOAD(&tmpiter->next, almemory_order_acquire); - if(!tmpiter) tmpiter = BufferLoopItem; - } + tmpiter = ATOMIC_LOAD(&tmpiter->next, almemory_order_acquire); + if(!tmpiter) tmpiter = BufferLoopItem; } } @@ -729,8 +730,10 @@ ALboolean MixSource(ALvoice *voice, ALuint SourceID, ALCcontext *Context, ALsize if(BufferListItem->max_samples > DataPosInt) break; + DataPosInt -= BufferListItem->max_samples; + buffers_done += BufferListItem->num_buffers; - BufferListItem = ATOMIC_LOAD(&BufferListItem->next, almemory_order_acquire); + BufferListItem = ATOMIC_LOAD(&BufferListItem->next, almemory_order_relaxed); if(!BufferListItem && !(BufferListItem=BufferLoopItem)) { isplaying = false; @@ -738,8 +741,6 @@ ALboolean MixSource(ALvoice *voice, ALuint SourceID, ALCcontext *Context, ALsize DataPosFrac = 0; break; } - - DataPosInt -= BufferListItem->max_samples; } } while(isplaying && OutPos < SamplesToDo); diff --git a/Engine/lib/openal-soft/Alc/panning.c b/Engine/lib/openal-soft/Alc/panning.c index 7f9e74e25..2c0f3bf23 100644 --- a/Engine/lib/openal-soft/Alc/panning.c +++ b/Engine/lib/openal-soft/Alc/panning.c @@ -38,9 +38,10 @@ #include "bs2b.h" +extern inline void CalcDirectionCoeffs(const ALfloat dir[3], ALfloat spread, ALfloat coeffs[MAX_AMBI_COEFFS]); extern inline void CalcAngleCoeffs(ALfloat azimuth, ALfloat elevation, ALfloat spread, ALfloat coeffs[MAX_AMBI_COEFFS]); -extern inline void ComputeDryPanGains(const DryMixParams *dry, const ALfloat coeffs[MAX_AMBI_COEFFS], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]); -extern inline void ComputeFirstOrderGains(const BFMixParams *foa, const ALfloat mtx[4], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]); +extern inline float ScaleAzimuthFront(float azimuth, float scale); +extern inline void ComputePanGains(const MixParams *dry, const ALfloat*restrict coeffs, ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]); static const ALsizei FuMa2ACN[MAX_AMBI_COEFFS] = { @@ -67,19 +68,15 @@ static const ALsizei ACN2ACN[MAX_AMBI_COEFFS] = { }; -void CalcDirectionCoeffs(const ALfloat dir[3], ALfloat spread, ALfloat coeffs[MAX_AMBI_COEFFS]) +void CalcAmbiCoeffs(const ALfloat y, const ALfloat z, const ALfloat x, const ALfloat spread, + ALfloat coeffs[MAX_AMBI_COEFFS]) { - /* Convert from OpenAL coords to Ambisonics. */ - ALfloat x = -dir[2]; - ALfloat y = -dir[0]; - ALfloat z = dir[1]; - /* Zeroth-order */ coeffs[0] = 1.0f; /* ACN 0 = 1 */ /* First-order */ - coeffs[1] = 1.732050808f * y; /* ACN 1 = sqrt(3) * Y */ - coeffs[2] = 1.732050808f * z; /* ACN 2 = sqrt(3) * Z */ - coeffs[3] = 1.732050808f * x; /* ACN 3 = sqrt(3) * X */ + coeffs[1] = SQRTF_3 * y; /* ACN 1 = sqrt(3) * Y */ + coeffs[2] = SQRTF_3 * z; /* ACN 2 = sqrt(3) * Z */ + coeffs[3] = SQRTF_3 * x; /* ACN 3 = sqrt(3) * X */ /* Second-order */ coeffs[4] = 3.872983346f * x * y; /* ACN 4 = sqrt(15) * X * Y */ coeffs[5] = 3.872983346f * y * z; /* ACN 5 = sqrt(15) * Y * Z */ @@ -153,16 +150,8 @@ void CalcDirectionCoeffs(const ALfloat dir[3], ALfloat spread, ALfloat coeffs[MA } } -void CalcAnglePairwiseCoeffs(ALfloat azimuth, ALfloat elevation, ALfloat spread, ALfloat coeffs[MAX_AMBI_COEFFS]) -{ - ALfloat sign = (azimuth < 0.0f) ? -1.0f : 1.0f; - if(!(fabsf(azimuth) > F_PI_2)) - azimuth = minf(fabsf(azimuth) * F_PI_2 / (F_PI/6.0f), F_PI_2) * sign; - CalcAngleCoeffs(azimuth, elevation, spread, coeffs); -} - -void ComputePanningGainsMC(const ChannelConfig *chancoeffs, ALsizei numchans, ALsizei numcoeffs, const ALfloat coeffs[MAX_AMBI_COEFFS], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]) +void ComputePanningGainsMC(const ChannelConfig *chancoeffs, ALsizei numchans, ALsizei numcoeffs, const ALfloat*restrict coeffs, ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]) { ALsizei i, j; @@ -177,7 +166,7 @@ void ComputePanningGainsMC(const ChannelConfig *chancoeffs, ALsizei numchans, AL gains[i] = 0.0f; } -void ComputePanningGainsBF(const BFChannelConfig *chanmap, ALsizei numchans, const ALfloat coeffs[MAX_AMBI_COEFFS], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]) +void ComputePanningGainsBF(const BFChannelConfig *chanmap, ALsizei numchans, const ALfloat*restrict coeffs, ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]) { ALsizei i; @@ -187,31 +176,6 @@ void ComputePanningGainsBF(const BFChannelConfig *chanmap, ALsizei numchans, con gains[i] = 0.0f; } -void ComputeFirstOrderGainsMC(const ChannelConfig *chancoeffs, ALsizei numchans, const ALfloat mtx[4], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]) -{ - ALsizei i, j; - - for(i = 0;i < numchans;i++) - { - float gain = 0.0f; - for(j = 0;j < 4;j++) - gain += chancoeffs[i][j] * mtx[j]; - gains[i] = clampf(gain, 0.0f, 1.0f) * ingain; - } - for(;i < MAX_OUTPUT_CHANNELS;i++) - gains[i] = 0.0f; -} - -void ComputeFirstOrderGainsBF(const BFChannelConfig *chanmap, ALsizei numchans, const ALfloat mtx[4], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]) -{ - ALsizei i; - - for(i = 0;i < numchans;i++) - gains[i] = chanmap[i].Scale * mtx[chanmap[i].Index] * ingain; - for(;i < MAX_OUTPUT_CHANNELS;i++) - gains[i] = 0.0f; -} - static inline const char *GetLabelFromChannel(enum Channel channel) { @@ -428,11 +392,12 @@ static void InitNearFieldCtrl(ALCdevice *device, ALfloat ctrl_dist, ALsizei orde * be used when rendering to an ambisonic buffer. */ device->AvgSpeakerDist = minf(ctrl_dist, 10.0f); + TRACE("Using near-field reference distance: %.2f meters\n", device->AvgSpeakerDist); for(i = 0;i < order+1;i++) - device->Dry.NumChannelsPerOrder[i] = chans_per_order[i]; + device->NumChannelsPerOrder[i] = chans_per_order[i]; for(;i < MAX_AMBI_ORDER+1;i++) - device->Dry.NumChannelsPerOrder[i] = 0; + device->NumChannelsPerOrder[i] = 0; } } @@ -816,10 +781,10 @@ static void InitHrtfPanning(ALCdevice *device) /* NOTE: azimuth goes clockwise. */ static const struct AngularPoint AmbiPoints[] = { { DEG2RAD( 90.0f), DEG2RAD( 0.0f) }, - { DEG2RAD( 35.0f), DEG2RAD( 45.0f) }, - { DEG2RAD( 35.0f), DEG2RAD( 135.0f) }, - { DEG2RAD( 35.0f), DEG2RAD(-135.0f) }, - { DEG2RAD( 35.0f), DEG2RAD( -45.0f) }, + { DEG2RAD( 35.2643897f), DEG2RAD( 45.0f) }, + { DEG2RAD( 35.2643897f), DEG2RAD( 135.0f) }, + { DEG2RAD( 35.2643897f), DEG2RAD(-135.0f) }, + { DEG2RAD( 35.2643897f), DEG2RAD( -45.0f) }, { DEG2RAD( 0.0f), DEG2RAD( 0.0f) }, { DEG2RAD( 0.0f), DEG2RAD( 45.0f) }, { DEG2RAD( 0.0f), DEG2RAD( 90.0f) }, @@ -828,10 +793,10 @@ static void InitHrtfPanning(ALCdevice *device) { DEG2RAD( 0.0f), DEG2RAD(-135.0f) }, { DEG2RAD( 0.0f), DEG2RAD( -90.0f) }, { DEG2RAD( 0.0f), DEG2RAD( -45.0f) }, - { DEG2RAD(-35.0f), DEG2RAD( 45.0f) }, - { DEG2RAD(-35.0f), DEG2RAD( 135.0f) }, - { DEG2RAD(-35.0f), DEG2RAD(-135.0f) }, - { DEG2RAD(-35.0f), DEG2RAD( -45.0f) }, + { DEG2RAD(-35.2643897f), DEG2RAD( 45.0f) }, + { DEG2RAD(-35.2643897f), DEG2RAD( 135.0f) }, + { DEG2RAD(-35.2643897f), DEG2RAD(-135.0f) }, + { DEG2RAD(-35.2643897f), DEG2RAD( -45.0f) }, { DEG2RAD(-90.0f), DEG2RAD( 0.0f) }, }; static const ALfloat AmbiMatrixFOA[][MAX_AMBI_COEFFS] = { @@ -887,7 +852,6 @@ static void InitHrtfPanning(ALCdevice *device) static_assert(COUNTOF(AmbiPoints) == COUNTOF(AmbiMatrixFOA), "FOA Ambisonic HRTF mismatch"); static_assert(COUNTOF(AmbiPoints) == COUNTOF(AmbiMatrixHOA), "HOA Ambisonic HRTF mismatch"); - static_assert(COUNTOF(AmbiPoints) <= HRTF_AMBI_MAX_CHANNELS, "HRTF_AMBI_MAX_CHANNELS is too small"); if(device->AmbiUp) { @@ -978,7 +942,7 @@ void aluInitRenderer(ALCdevice *device, ALint hrtf_id, enum HrtfRequestMode hrtf device->Dry.CoeffCount = 0; device->Dry.NumChannels = 0; for(i = 0;i < MAX_AMBI_ORDER+1;i++) - device->Dry.NumChannelsPerOrder[i] = 0; + device->NumChannelsPerOrder[i] = 0; device->AvgSpeakerDist = 0.0f; memset(device->ChannelDelay, 0, sizeof(device->ChannelDelay)); diff --git a/Engine/lib/openal-soft/CMakeLists.txt b/Engine/lib/openal-soft/CMakeLists.txt index 6a4f8ff4a..39b802508 100644 --- a/Engine/lib/openal-soft/CMakeLists.txt +++ b/Engine/lib/openal-soft/CMakeLists.txt @@ -102,8 +102,8 @@ IF(NOT LIBTYPE) ENDIF() SET(LIB_MAJOR_VERSION "1") -SET(LIB_MINOR_VERSION "18") -SET(LIB_REVISION "2") +SET(LIB_MINOR_VERSION "19") +SET(LIB_REVISION "1") SET(LIB_VERSION "${LIB_MAJOR_VERSION}.${LIB_MINOR_VERSION}.${LIB_REVISION}") SET(EXPORT_DECL "") @@ -432,6 +432,48 @@ IF(HAVE_MFPU_NEON_SWITCH) SET(FPU_NEON_SWITCH "-mfpu=neon") ENDIF() +SET(FPMATH_SET "0") +IF(CMAKE_SIZEOF_VOID_P MATCHES "4") + IF(SSE_SWITCH OR MSVC) + OPTION(ALSOFT_ENABLE_SSE_CODEGEN "Enable SSE code generation instead of x87 for 32-bit targets." TRUE) + ENDIF() + IF(SSE2_SWITCH OR MSVC) + OPTION(ALSOFT_ENABLE_SSE2_CODEGEN "Enable SSE2 code generation instead of x87 for 32-bit targets." TRUE) + ENDIF() + + IF(ALSOFT_ENABLE_SSE2_CODEGEN) + IF(SSE2_SWITCH) + CHECK_C_COMPILER_FLAG("${SSE2_SWITCH} -mfpmath=sse" HAVE_MFPMATH_SSE_2) + IF(HAVE_MFPMATH_SSE_2) + SET(C_FLAGS ${C_FLAGS} ${SSE2_SWITCH} -mfpmath=sse) + SET(FPMATH_SET 2) + ENDIF() + ELSEIF(MSVC) + CHECK_C_COMPILER_FLAG("/arch:SSE2" HAVE_ARCH_SSE2) + IF(HAVE_ARCH_SSE2) + SET(C_FLAGS ${C_FLAGS} "/arch:SSE2") + SET(FPMATH_SET 2) + ENDIF() + ENDIF() + ENDIF() + IF(ALSOFT_ENABLE_SSE_CODEGEN AND NOT FPMATH_SET) + IF(SSE_SWITCH) + CHECK_C_COMPILER_FLAG("${SSE_SWITCH} -mfpmath=sse" HAVE_MFPMATH_SSE) + IF(HAVE_MFPMATH_SSE) + SET(C_FLAGS ${C_FLAGS} ${SSE_SWITCH} -mfpmath=sse) + SET(FPMATH_SET 1) + ENDIF() + ELSEIF(MSVC) + CHECK_C_COMPILER_FLAG("/arch:SSE" HAVE_ARCH_SSE) + IF(HAVE_ARCH_SSE) + SET(C_FLAGS ${C_FLAGS} "/arch:SSE") + SET(FPMATH_SET 1) + ENDIF() + ENDIF() + ENDIF() +ENDIF() + + CHECK_C_SOURCE_COMPILES("int foo(const char *str, ...) __attribute__((format(printf, 1, 2))); int main() {return 0;}" HAVE_GCC_FORMAT) @@ -512,6 +554,7 @@ CHECK_SYMBOL_EXISTS(lrintf math.h HAVE_LRINTF) CHECK_SYMBOL_EXISTS(modff math.h HAVE_MODFF) CHECK_SYMBOL_EXISTS(log2f math.h HAVE_LOG2F) CHECK_SYMBOL_EXISTS(cbrtf math.h HAVE_CBRTF) +CHECK_SYMBOL_EXISTS(copysignf math.h HAVE_COPYSIGNF) IF(HAVE_FLOAT_H) CHECK_SYMBOL_EXISTS(_controlfp float.h HAVE__CONTROLFP) @@ -700,6 +743,8 @@ ENDIF() SET(COMMON_OBJS + common/alcomplex.c + common/alcomplex.h common/align.h common/almalloc.c common/almalloc.h @@ -753,12 +798,14 @@ SET(ALC_OBJS Alc/mastering.h Alc/ringbuffer.c Alc/ringbuffer.h + Alc/effects/autowah.c Alc/effects/chorus.c Alc/effects/compressor.c Alc/effects/dedicated.c Alc/effects/distortion.c Alc/effects/echo.c Alc/effects/equalizer.c + Alc/effects/fshifter.c Alc/effects/modulator.c Alc/effects/null.c Alc/effects/pshifter.c @@ -855,7 +902,7 @@ IF(ALSOFT_REQUIRE_SSE2 AND NOT HAVE_SSE2) MESSAGE(FATAL_ERROR "Failed to enable required SSE2 CPU extensions") ENDIF() -OPTION(ALSOFT_REQUIRE_SSE2 "Require SSE3 support" OFF) +OPTION(ALSOFT_REQUIRE_SSE3 "Require SSE3 support" OFF) CHECK_INCLUDE_FILE(pmmintrin.h HAVE_PMMINTRIN_H "${SSE3_SWITCH}") IF(HAVE_EMMINTRIN_H) OPTION(ALSOFT_CPUEXT_SSE3 "Enable SSE3 support" ON) @@ -1255,7 +1302,7 @@ ADD_CUSTOM_TARGET(native-tools VERBATIM ) -option(ALSOFT_EMBED_HRTF_DATA "Embed the HRTF data files (increases library footprint)" OFF) +option(ALSOFT_EMBED_HRTF_DATA "Embed the HRTF data files (increases library footprint)" ON) if(ALSOFT_EMBED_HRTF_DATA) MACRO(make_hrtf_header FILENAME VARNAME) SET(infile "${OpenAL_SOURCE_DIR}/hrtf/${FILENAME}") @@ -1468,6 +1515,10 @@ MESSAGE(STATUS "") MESSAGE(STATUS "Building with support for CPU extensions:") MESSAGE(STATUS " ${CPU_EXTS}") MESSAGE(STATUS "") +IF(FPMATH_SET) + MESSAGE(STATUS "Building with SSE${FPMATH_SET} codegen") + MESSAGE(STATUS "") +ENDIF() IF(WIN32) IF(NOT HAVE_DSOUND) @@ -1695,7 +1746,8 @@ IF(ALSOFT_EXAMPLES) PRIVATE ${SDL2_INCLUDE_DIR} ${FFMPEG_INCLUDE_DIRS}) TARGET_COMPILE_OPTIONS(alffplay PRIVATE ${C_FLAGS}) TARGET_LINK_LIBRARIES(alffplay - PRIVATE ${LINKER_FLAGS} ${SDL2_LIBRARY} ${FFMPEG_LIBRARIES} common OpenAL) + PRIVATE ${LINKER_FLAGS} ${SDL2_LIBRARY} ${FFMPEG_LIBRARIES} ex-common common + OpenAL) IF(ALSOFT_INSTALL) INSTALL(TARGETS alffplay diff --git a/Engine/lib/openal-soft/COPYING b/Engine/lib/openal-soft/COPYING index 5bc8fb2c8..8d5d00006 100644 --- a/Engine/lib/openal-soft/COPYING +++ b/Engine/lib/openal-soft/COPYING @@ -51,7 +51,7 @@ library. If the library is modified by someone else and passed on, we want its recipients to know that what they have is not the original version, so that any problems introduced by others will not reflect on the original authors' reputations. - + Finally, any free program is threatened constantly by software patents. We wish to avoid the danger that companies distributing free software will individually obtain patent licenses, thus in effect @@ -98,7 +98,7 @@ works together with the library. Note that it is possible for a library to be covered by the ordinary General Public License rather than by this special one. - + GNU LIBRARY GENERAL PUBLIC LICENSE TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION @@ -145,7 +145,7 @@ Library. You may charge a fee for the physical act of transferring a copy, and you may at your option offer warranty protection in exchange for a fee. - + 2. You may modify your copy or copies of the Library or any portion of it, thus forming a work based on the Library, and copy and distribute such modifications or work under the terms of Section 1 @@ -203,7 +203,7 @@ instead of to this License. (If a newer version than version 2 of the ordinary GNU General Public License has appeared, then you can specify that version instead if you wish.) Do not make any other change in these notices. - + Once this change is made in a given copy, it is irreversible for that copy, so the ordinary GNU General Public License applies to all subsequent copies and derivative works made from that copy. @@ -254,7 +254,7 @@ Library will still fall under Section 6.) distribute the object code for the work under the terms of Section 6. Any executables containing that work also fall under Section 6, whether or not they are linked directly with the Library itself. - + 6. As an exception to the Sections above, you may also compile or link a "work that uses the Library" with the Library to produce a work containing portions of the Library, and distribute that work @@ -308,7 +308,7 @@ restrictions of other proprietary libraries that do not normally accompany the operating system. Such a contradiction means you cannot use both them and the Library together in an executable that you distribute. - + 7. You may place library facilities that are a work based on the Library side-by-side in a single library together with other library facilities not covered by this License, and distribute such a combined @@ -349,7 +349,7 @@ subject to these terms and conditions. You may not impose any further restrictions on the recipients' exercise of the rights granted herein. You are not responsible for enforcing compliance by third parties to this License. - + 11. If, as a consequence of a court judgment or allegation of patent infringement or for any other reason (not limited to patent issues), conditions are imposed on you (whether by court order, agreement or @@ -401,7 +401,7 @@ conditions either of that version or of any later version published by the Free Software Foundation. If the Library does not specify a license version number, you may choose any version ever published by the Free Software Foundation. - + 14. If you wish to incorporate parts of the Library into other free programs whose distribution conditions are incompatible with these, write to the author to ask for permission. For software which is @@ -435,47 +435,3 @@ SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Libraries - - If you develop a new library, and you want it to be of the greatest -possible use to the public, we recommend making it free software that -everyone can redistribute and change. You can do so by permitting -redistribution under these terms (or, alternatively, under the terms of the -ordinary General Public License). - - To apply these terms, attach the following notices to the library. It is -safest to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least the -"copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Library General Public - License as published by the Free Software Foundation; either - version 2 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Library General Public License for more details. - - You should have received a copy of the GNU Library General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - -Also add information on how to contact you by electronic and paper mail. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the library, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the - library `Frob' (a library for tweaking knobs) written by James Random Hacker. - - , 1 April 1990 - Ty Coon, President of Vice - -That's all there is to it! diff --git a/Engine/lib/openal-soft/ChangeLog b/Engine/lib/openal-soft/ChangeLog index a7aec6388..d234c14f4 100644 --- a/Engine/lib/openal-soft/ChangeLog +++ b/Engine/lib/openal-soft/ChangeLog @@ -1,8 +1,24 @@ +openal-soft-1.19.1: + + Implemented capture support for the SoundIO backend. + + Fixed source buffer queues potentially not playing properly when a queue + entry completes. + + Fixed possible unexpected failures when generating auxiliary effect slots. + + Fixed a crash with certain reverb or device settings. + + Fixed OpenSL capture. + + Improved output limiter response, better ensuring the sample amplitude is + clamped for output. + openal-soft-1.19.0: Implemented the ALC_SOFT_device_clock extension. - Implemented the Pitch Shifter effect. + Implemented the Pitch Shifter, Frequency Shifter, and Autowah effects. Fixed compiling on FreeBSD systems that use freebsd-lib 9.1. @@ -16,11 +32,14 @@ openal-soft-1.19.0: Increased the number of virtual channels for decoding Ambisonics to HRTF output. + Changed 32-bit x86 builds to use SSE2 math by default for performance. + Build-time options are available to use just SSE1 or x87 instead. + Replaced the 4-point Sinc resampler with a more efficient cubic resampler. Renamed the MMDevAPI backend to WASAPI. - Added support fot 24-bit, dual-ear HRTF data sets. The built-in data set + Added support for 24-bit, dual-ear HRTF data sets. The built-in data set has been updated to 24-bit. Added a 24- to 48-point band-limited Sinc resampler. diff --git a/Engine/lib/openal-soft/OpenAL32/Include/alAuxEffectSlot.h b/Engine/lib/openal-soft/OpenAL32/Include/alAuxEffectSlot.h index bb9aef590..03ee97d6f 100644 --- a/Engine/lib/openal-soft/OpenAL32/Include/alAuxEffectSlot.h +++ b/Engine/lib/openal-soft/OpenAL32/Include/alAuxEffectSlot.h @@ -145,8 +145,8 @@ typedef struct ALeffectslot { * * Channel 3 is OpenAL -Z * sqrt(3) * Consequently, effects that only want to work with mono input can use * channel 0 by itself. Effects that want multichannel can process the - * ambisonics signal and make a B-Format pan (ComputeFirstOrderGains) for - * first-order device output (FOAOut). + * ambisonics signal and make a B-Format source pan for first-order device + * output (FOAOut). */ alignas(16) ALfloat WetBuffer[MAX_EFFECT_CHANNELS][BUFFERSIZE]; } ALeffectslot; @@ -160,12 +160,14 @@ ALvoid ReleaseALAuxiliaryEffectSlots(ALCcontext *Context); EffectStateFactory *NullStateFactory_getFactory(void); EffectStateFactory *ReverbStateFactory_getFactory(void); +EffectStateFactory *AutowahStateFactory_getFactory(void); EffectStateFactory *ChorusStateFactory_getFactory(void); EffectStateFactory *CompressorStateFactory_getFactory(void); EffectStateFactory *DistortionStateFactory_getFactory(void); EffectStateFactory *EchoStateFactory_getFactory(void); EffectStateFactory *EqualizerStateFactory_getFactory(void); EffectStateFactory *FlangerStateFactory_getFactory(void); +EffectStateFactory *FshifterStateFactory_getFactory(void); EffectStateFactory *ModulatorStateFactory_getFactory(void); EffectStateFactory *PshifterStateFactory_getFactory(void); diff --git a/Engine/lib/openal-soft/OpenAL32/Include/alEffect.h b/Engine/lib/openal-soft/OpenAL32/Include/alEffect.h index 50b64ee1d..7b849c0ce 100644 --- a/Engine/lib/openal-soft/OpenAL32/Include/alEffect.h +++ b/Engine/lib/openal-soft/OpenAL32/Include/alEffect.h @@ -12,12 +12,14 @@ struct ALeffect; enum { EAXREVERB_EFFECT = 0, REVERB_EFFECT, + AUTOWAH_EFFECT, CHORUS_EFFECT, COMPRESSOR_EFFECT, DISTORTION_EFFECT, ECHO_EFFECT, EQUALIZER_EFFECT, FLANGER_EFFECT, + FSHIFTER_EFFECT, MODULATOR_EFFECT, PSHIFTER_EFFECT, DEDICATED_EFFECT, @@ -33,7 +35,7 @@ struct EffectList { int type; ALenum val; }; -#define EFFECTLIST_SIZE 12 +#define EFFECTLIST_SIZE 14 extern const struct EffectList EffectList[EFFECTLIST_SIZE]; @@ -59,12 +61,14 @@ const struct ALeffectVtable T##_vtable = { \ extern const struct ALeffectVtable ALeaxreverb_vtable; extern const struct ALeffectVtable ALreverb_vtable; +extern const struct ALeffectVtable ALautowah_vtable; extern const struct ALeffectVtable ALchorus_vtable; extern const struct ALeffectVtable ALcompressor_vtable; extern const struct ALeffectVtable ALdistortion_vtable; extern const struct ALeffectVtable ALecho_vtable; extern const struct ALeffectVtable ALequalizer_vtable; extern const struct ALeffectVtable ALflanger_vtable; +extern const struct ALeffectVtable ALfshifter_vtable; extern const struct ALeffectVtable ALmodulator_vtable; extern const struct ALeffectVtable ALnull_vtable; extern const struct ALeffectVtable ALpshifter_vtable; @@ -101,6 +105,13 @@ typedef union ALeffectProps { ALfloat LFReference; } Reverb; + struct { + ALfloat AttackTime; + ALfloat ReleaseTime; + ALfloat Resonance; + ALfloat PeakGain; + } Autowah; + struct { ALint Waveform; ALint Phase; @@ -145,6 +156,12 @@ typedef union ALeffectProps { ALfloat HighGain; } Equalizer; + struct { + ALfloat Frequency; + ALint LeftDirection; + ALint RightDirection; + } Fshifter; + struct { ALfloat Frequency; ALfloat HighPassCutoff; diff --git a/Engine/lib/openal-soft/OpenAL32/Include/alMain.h b/Engine/lib/openal-soft/OpenAL32/Include/alMain.h index e29d9c270..0fd77491e 100644 --- a/Engine/lib/openal-soft/OpenAL32/Include/alMain.h +++ b/Engine/lib/openal-soft/OpenAL32/Include/alMain.h @@ -115,15 +115,25 @@ typedef ALuint64SOFT ALuint64; #endif #endif +#ifndef I64 +#if defined(_MSC_VER) +#define I64(x) ((ALint64)(x##i64)) +#elif SIZEOF_LONG == 8 +#define I64(x) ((ALint64)(x##l)) +#elif SIZEOF_LONG_LONG == 8 +#define I64(x) ((ALint64)(x##ll)) +#endif +#endif + /* Define a CTZ64 macro (count trailing zeros, for 64-bit integers). The result * is *UNDEFINED* if the value is 0. */ #ifdef __GNUC__ #if SIZEOF_LONG == 8 -#define CTZ64(x) __builtin_ctzl(x) +#define CTZ64 __builtin_ctzl #else -#define CTZ64(x) __builtin_ctzll(x) +#define CTZ64 __builtin_ctzll #endif #elif defined(HAVE_BITSCANFORWARD64_INTRINSIC) @@ -134,7 +144,7 @@ inline int msvc64_ctz64(ALuint64 v) _BitScanForward64(&idx, v); return (int)idx; } -#define CTZ64(x) msvc64_ctz64(x) +#define CTZ64 msvc64_ctz64 #elif defined(HAVE_BITSCANFORWARD_INTRINSIC) @@ -148,7 +158,7 @@ inline int msvc_ctz64(ALuint64 v) } return (int)idx; } -#define CTZ64(x) msvc_ctz64(x) +#define CTZ64 msvc_ctz64 #else @@ -171,14 +181,18 @@ inline int fallback_ctz64(ALuint64 value) { return fallback_popcnt64(~value & (value - 1)); } -#define CTZ64(x) fallback_ctz64(x) +#define CTZ64 fallback_ctz64 #endif +#if defined(__BYTE_ORDER__) && defined(__ORDER_LITTLE_ENDIAN__) +#define IS_LITTLE_ENDIAN (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) +#else static const union { ALuint u; ALubyte b[sizeof(ALuint)]; } EndianTest = { 1 }; #define IS_LITTLE_ENDIAN (EndianTest.b[0] == 1) +#endif #define COUNTOF(x) (sizeof(x) / sizeof(0[x])) @@ -249,7 +263,7 @@ inline ALint fastf2i(ALfloat f) #ifdef __SSE_MATH__ __asm__("cvtss2si %1, %0" : "=r"(i) : "x"(f)); #else - __asm__("flds %1\n fistps %0" : "=m"(i) : "m"(f)); + __asm__ __volatile__("fistpl %0" : "=m"(i) : "t"(f) : "st"); #endif return i; @@ -271,8 +285,85 @@ inline ALint fastf2i(ALfloat f) /* Converts float-to-int using standard behavior (truncation). */ inline int float2int(float f) { - /* TODO: Make a more efficient method for x87. */ +#if ((defined(__GNUC__) || defined(__clang__)) && (defined(__i386__) || defined(__x86_64__)) && \ + !defined(__SSE_MATH__)) || (defined(_MSC_VER) && defined(_M_IX86_FP) && _M_IX86_FP == 0) + ALint sign, shift, mant; + union { + ALfloat f; + ALint i; + } conv; + + conv.f = f; + sign = (conv.i>>31) | 1; + shift = ((conv.i>>23)&0xff) - (127+23); + + /* Over/underflow */ + if(UNLIKELY(shift >= 31 || shift < -23)) + return 0; + + mant = (conv.i&0x7fffff) | 0x800000; + if(LIKELY(shift < 0)) + return (mant >> -shift) * sign; + return (mant << shift) * sign; + +#else + return (ALint)f; +#endif +} + +/* Rounds a float to the nearest integral value, according to the current + * rounding mode. This is essentially an inlined version of rintf, although + * makes fewer promises (e.g. -0 or -0.25 rounded to 0 may result in +0). + */ +inline float fast_roundf(float f) +{ +#if (defined(__GNUC__) || defined(__clang__)) && (defined(__i386__) || defined(__x86_64__)) && \ + !defined(__SSE_MATH__) + + float out; + __asm__ __volatile__("frndint" : "=t"(out) : "0"(f)); + return out; + +#else + + /* Integral limit, where sub-integral precision is not available for + * floats. + */ + static const float ilim[2] = { + 8388608.0f /* 0x1.0p+23 */, + -8388608.0f /* -0x1.0p+23 */ + }; + ALuint sign, expo; + union { + ALfloat f; + ALuint i; + } conv; + + conv.f = f; + sign = (conv.i>>31)&0x01; + expo = (conv.i>>23)&0xff; + + if(UNLIKELY(expo >= 150/*+23*/)) + { + /* An exponent (base-2) of 23 or higher is incapable of sub-integral + * precision, so it's already an integral value. We don't need to worry + * about infinity or NaN here. + */ + return f; + } + /* Adding the integral limit to the value (with a matching sign) forces a + * result that has no sub-integral precision, and is consequently forced to + * round to an integral value. Removing the integral limit then restores + * the initial value rounded to the integral. The compiler should not + * optimize this out because of non-associative rules on floating-point + * math (as long as you don't use -fassociative-math, + * -funsafe-math-optimizations, -ffast-math, or -Ofast, in which case this + * may break). + */ + f += ilim[sign]; + return f - ilim[sign]; +#endif } @@ -491,7 +582,7 @@ typedef struct DistanceComp { */ #define BUFFERSIZE 2048 -typedef struct DryMixParams { +typedef struct MixParams { AmbiConfig Ambi; /* Number of coefficients in each Ambi.Coeffs to mix together (4 for first- * order, 9 for second-order, etc). If the count is 0, Ambi.Map is used @@ -501,17 +592,7 @@ typedef struct DryMixParams { ALfloat (*Buffer)[BUFFERSIZE]; ALsizei NumChannels; - ALsizei NumChannelsPerOrder[MAX_AMBI_ORDER+1]; -} DryMixParams; - -typedef struct BFMixParams { - AmbiConfig Ambi; - /* Will only be 4 or 0. */ - ALsizei CoeffCount; - - ALfloat (*Buffer)[BUFFERSIZE]; - ALsizei NumChannels; -} BFMixParams; +} MixParams; typedef struct RealMixParams { enum Channel ChannelName[MAX_OUTPUT_CHANNELS]; @@ -541,6 +622,8 @@ struct ALCdevice_struct { enum AmbiLayout AmbiLayout; enum AmbiNorm AmbiScale; + ALCenum LimiterState; + al_string DeviceName; ATOMIC(ALCenum) LastError; @@ -595,15 +678,17 @@ struct ALCdevice_struct { ALuint64 ClockBase; ALuint SamplesDone; + ALuint FixedLatency; /* Temp storage used for mixer processing. */ alignas(16) ALfloat TempBuffer[4][BUFFERSIZE]; /* The "dry" path corresponds to the main output. */ - DryMixParams Dry; + MixParams Dry; + ALsizei NumChannelsPerOrder[MAX_AMBI_ORDER+1]; /* First-order ambisonics output, to be upsampled to the dry buffer if different. */ - BFMixParams FOAOut; + MixParams FOAOut; /* "Real" output, which will be written to the device buffer. May alias the * dry buffer. @@ -668,21 +753,35 @@ struct ALCdevice_struct { enum { + /* End event thread processing. */ + EventType_KillThread = 0, + + /* User event types. */ EventType_SourceStateChange = 1<<0, EventType_BufferCompleted = 1<<1, EventType_Error = 1<<2, EventType_Performance = 1<<3, EventType_Deprecated = 1<<4, EventType_Disconnected = 1<<5, + + /* Internal events. */ + EventType_ReleaseEffectState = 65536, }; typedef struct AsyncEvent { unsigned int EnumType; - ALenum Type; - ALuint ObjectId; - ALuint Param; - ALchar Message[1008]; + union { + char dummy; + struct { + ALenum type; + ALuint id; + ALuint param; + ALchar msg[1008]; + } user; + struct ALeffectState *EffectState; + } u; } AsyncEvent; +#define ASYNC_EVENT(t) { t, { 0 } } struct ALCcontext_struct { RefCount ref; @@ -735,7 +834,6 @@ struct ALCcontext_struct { ATOMIC(struct ALeffectslotArray*) ActiveAuxSlots; - almtx_t EventThrdLock; althrd_t EventThread; alsem_t EventSem; struct ll_ringbuffer *AsyncEvents; @@ -765,9 +863,6 @@ void ALCcontext_ProcessUpdates(ALCcontext *context); void AllocateVoices(ALCcontext *context, ALsizei num_voices, ALsizei old_sends); -void AppendAllDevicesList(const ALCchar *name); -void AppendCaptureDeviceList(const ALCchar *name); - extern ALint RTPrioLevel; void SetRTPriority(void); @@ -813,6 +908,9 @@ inline void UnlockEffectSlotList(ALCcontext *context) { almtx_unlock(&context->EffectSlotLock); } +int EventThread(void *arg); + + vector_al_string SearchDataFiles(const char *match, const char *subdir); #ifdef __cplusplus diff --git a/Engine/lib/openal-soft/OpenAL32/Include/alu.h b/Engine/lib/openal-soft/OpenAL32/Include/alu.h index b977613c0..c572fd716 100644 --- a/Engine/lib/openal-soft/OpenAL32/Include/alu.h +++ b/Engine/lib/openal-soft/OpenAL32/Include/alu.h @@ -74,7 +74,7 @@ extern enum Resampler ResamplerDefault; typedef struct BsincState { ALfloat sf; /* Scale interpolation factor. */ ALsizei m; /* Coefficient count. */ - ALint l; /* Left coefficient offset. */ + ALsizei l; /* Left coefficient offset. */ /* Filter coefficients, followed by the scale, phase, and scale-phase * delta coefficients. Starting at phase index 0, each subsequent phase * index follows contiguously. @@ -430,14 +430,35 @@ void aluInitEffectPanning(struct ALeffectslot *slot); void aluSelectPostProcess(ALCdevice *device); +/** + * Calculates ambisonic encoder coefficients using the X, Y, and Z direction + * components, which must represent a normalized (unit length) vector, and the + * spread is the angular width of the sound (0...tau). + * + * NOTE: The components use ambisonic coordinates. As a result: + * + * Ambisonic Y = OpenAL -X + * Ambisonic Z = OpenAL Y + * Ambisonic X = OpenAL -Z + * + * The components are ordered such that OpenAL's X, Y, and Z are the first, + * second, and third parameters respectively -- simply negate X and Z. + */ +void CalcAmbiCoeffs(const ALfloat y, const ALfloat z, const ALfloat x, const ALfloat spread, + ALfloat coeffs[MAX_AMBI_COEFFS]); + /** * CalcDirectionCoeffs * - * Calculates ambisonic coefficients based on a direction vector. The vector - * must be normalized (unit length), and the spread is the angular width of the - * sound (0...tau). + * Calculates ambisonic coefficients based on an OpenAL direction vector. The + * vector must be normalized (unit length), and the spread is the angular width + * of the sound (0...tau). */ -void CalcDirectionCoeffs(const ALfloat dir[3], ALfloat spread, ALfloat coeffs[MAX_AMBI_COEFFS]); +inline void CalcDirectionCoeffs(const ALfloat dir[3], ALfloat spread, ALfloat coeffs[MAX_AMBI_COEFFS]) +{ + /* Convert from OpenAL coords to Ambisonics. */ + CalcAmbiCoeffs(-dir[0], dir[1], -dir[2], spread, coeffs); +} /** * CalcAngleCoeffs @@ -448,34 +469,40 @@ void CalcDirectionCoeffs(const ALfloat dir[3], ALfloat spread, ALfloat coeffs[MA */ inline void CalcAngleCoeffs(ALfloat azimuth, ALfloat elevation, ALfloat spread, ALfloat coeffs[MAX_AMBI_COEFFS]) { - ALfloat dir[3] = { - sinf(azimuth) * cosf(elevation), - sinf(elevation), - -cosf(azimuth) * cosf(elevation) - }; - CalcDirectionCoeffs(dir, spread, coeffs); + ALfloat x = -sinf(azimuth) * cosf(elevation); + ALfloat y = sinf(elevation); + ALfloat z = cosf(azimuth) * cosf(elevation); + + CalcAmbiCoeffs(x, y, z, spread, coeffs); } /** - * CalcAnglePairwiseCoeffs + * ScaleAzimuthFront * - * Calculates ambisonic coefficients based on azimuth and elevation. The - * azimuth and elevation parameters are in radians, going right and up - * respectively. This pairwise variant warps the result such that +30 azimuth - * is full right, and -30 azimuth is full left. + * Scales the given azimuth toward the side (+/- pi/2 radians) for positions in + * front. */ -void CalcAnglePairwiseCoeffs(ALfloat azimuth, ALfloat elevation, ALfloat spread, ALfloat coeffs[MAX_AMBI_COEFFS]); +inline float ScaleAzimuthFront(float azimuth, float scale) +{ + ALfloat sign = copysignf(1.0f, azimuth); + if(!(fabsf(azimuth) > F_PI_2)) + return minf(fabsf(azimuth) * scale, F_PI_2) * sign; + return azimuth; +} -void ComputePanningGainsMC(const ChannelConfig *chancoeffs, ALsizei numchans, ALsizei numcoeffs, const ALfloat coeffs[MAX_AMBI_COEFFS], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]); -void ComputePanningGainsBF(const BFChannelConfig *chanmap, ALsizei numchans, const ALfloat coeffs[MAX_AMBI_COEFFS], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]); +void ComputePanningGainsMC(const ChannelConfig *chancoeffs, ALsizei numchans, ALsizei numcoeffs, const ALfloat*restrict coeffs, ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]); +void ComputePanningGainsBF(const BFChannelConfig *chanmap, ALsizei numchans, const ALfloat*restrict coeffs, ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]); + /** - * ComputeDryPanGains + * ComputePanGains * * Computes panning gains using the given channel decoder coefficients and the - * pre-calculated direction or angle coefficients. + * pre-calculated direction or angle coefficients. For B-Format sources, the + * coeffs are a 'slice' of a transform matrix for the input channel, used to + * scale and orient the sound samples. */ -inline void ComputeDryPanGains(const DryMixParams *dry, const ALfloat coeffs[MAX_AMBI_COEFFS], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]) +inline void ComputePanGains(const MixParams *dry, const ALfloat*restrict coeffs, ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]) { if(dry->CoeffCount > 0) ComputePanningGainsMC(dry->Ambi.Coeffs, dry->NumChannels, dry->CoeffCount, @@ -484,23 +511,6 @@ inline void ComputeDryPanGains(const DryMixParams *dry, const ALfloat coeffs[MAX ComputePanningGainsBF(dry->Ambi.Map, dry->NumChannels, coeffs, ingain, gains); } -void ComputeFirstOrderGainsMC(const ChannelConfig *chancoeffs, ALsizei numchans, const ALfloat mtx[4], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]); -void ComputeFirstOrderGainsBF(const BFChannelConfig *chanmap, ALsizei numchans, const ALfloat mtx[4], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]); -/** - * ComputeFirstOrderGains - * - * Sets channel gains for a first-order ambisonics input channel. The matrix is - * a 1x4 'slice' of a transform matrix for the input channel, used to scale and - * orient the sound samples. - */ -inline void ComputeFirstOrderGains(const BFMixParams *foa, const ALfloat mtx[4], ALfloat ingain, ALfloat gains[MAX_OUTPUT_CHANNELS]) -{ - if(foa->CoeffCount > 0) - ComputeFirstOrderGainsMC(foa->Ambi.Coeffs, foa->NumChannels, mtx, ingain, gains); - else - ComputeFirstOrderGainsBF(foa->Ambi.Map, foa->NumChannels, mtx, ingain, gains); -} - ALboolean MixSource(struct ALvoice *voice, ALuint SourceID, ALCcontext *Context, ALsizei SamplesToDo); diff --git a/Engine/lib/openal-soft/OpenAL32/alAuxEffectSlot.c b/Engine/lib/openal-soft/OpenAL32/alAuxEffectSlot.c index d04fc4a77..8141e0f6b 100644 --- a/Engine/lib/openal-soft/OpenAL32/alAuxEffectSlot.c +++ b/Engine/lib/openal-soft/OpenAL32/alAuxEffectSlot.c @@ -48,12 +48,14 @@ static const struct { { AL_EFFECT_NULL, NullStateFactory_getFactory }, { AL_EFFECT_EAXREVERB, ReverbStateFactory_getFactory }, { AL_EFFECT_REVERB, ReverbStateFactory_getFactory }, + { AL_EFFECT_AUTOWAH, AutowahStateFactory_getFactory }, { AL_EFFECT_CHORUS, ChorusStateFactory_getFactory }, { AL_EFFECT_COMPRESSOR, CompressorStateFactory_getFactory }, { AL_EFFECT_DISTORTION, DistortionStateFactory_getFactory }, { AL_EFFECT_ECHO, EchoStateFactory_getFactory }, { AL_EFFECT_EQUALIZER, EqualizerStateFactory_getFactory }, { AL_EFFECT_FLANGER, FlangerStateFactory_getFactory }, + { AL_EFFECT_FREQUENCY_SHIFTER, FshifterStateFactory_getFactory }, { AL_EFFECT_RING_MODULATOR, ModulatorStateFactory_getFactory }, { AL_EFFECT_PITCH_SHIFTER, PshifterStateFactory_getFactory}, { AL_EFFECT_DEDICATED_DIALOGUE, DedicatedStateFactory_getFactory }, @@ -120,12 +122,6 @@ AL_API ALvoid AL_APIENTRY alGenAuxiliaryEffectSlots(ALsizei n, ALuint *effectslo LockEffectSlotList(context); device = context->Device; - if(device->AuxiliaryEffectSlotMax - VECTOR_SIZE(context->EffectSlotList) < (ALuint)n) - { - UnlockEffectSlotList(context); - SETERR_GOTO(context, AL_OUT_OF_MEMORY, done, "Exceeding %u auxiliary effect slot limit", - device->AuxiliaryEffectSlotMax); - } for(cur = 0;cur < n;cur++) { ALeffectslotPtr *iter = VECTOR_BEGIN(context->EffectSlotList); @@ -140,6 +136,13 @@ AL_API ALvoid AL_APIENTRY alGenAuxiliaryEffectSlots(ALsizei n, ALuint *effectslo } if(iter == end) { + if(device->AuxiliaryEffectSlotMax == VECTOR_SIZE(context->EffectSlotList)) + { + UnlockEffectSlotList(context); + alDeleteAuxiliaryEffectSlots(cur, effectslots); + SETERR_GOTO(context, AL_OUT_OF_MEMORY, done, + "Exceeding %u auxiliary effect slot limit", device->AuxiliaryEffectSlotMax); + } VECTOR_PUSH_BACK(context->EffectSlotList, NULL); iter = &VECTOR_BACK(context->EffectSlotList); } @@ -750,6 +753,9 @@ void UpdateEffectSlotProps(ALeffectslot *slot, ALCcontext *context) /* If there was an unused update container, put it back in the * freelist. */ + if(props->State) + ALeffectState_DecRef(props->State); + props->State = NULL; ATOMIC_REPLACE_HEAD(struct ALeffectslotProps*, &context->FreeEffectslotProps, props); } diff --git a/Engine/lib/openal-soft/OpenAL32/alEffect.c b/Engine/lib/openal-soft/OpenAL32/alEffect.c index e7dc6aced..c2a78a0ca 100644 --- a/Engine/lib/openal-soft/OpenAL32/alEffect.c +++ b/Engine/lib/openal-soft/OpenAL32/alEffect.c @@ -38,12 +38,14 @@ extern inline ALboolean IsReverbEffect(ALenum type); const struct EffectList EffectList[EFFECTLIST_SIZE] = { { "eaxreverb", EAXREVERB_EFFECT, AL_EFFECT_EAXREVERB }, { "reverb", REVERB_EFFECT, AL_EFFECT_REVERB }, + { "autowah", AUTOWAH_EFFECT, AL_EFFECT_AUTOWAH }, { "chorus", CHORUS_EFFECT, AL_EFFECT_CHORUS }, { "compressor", COMPRESSOR_EFFECT, AL_EFFECT_COMPRESSOR }, { "distortion", DISTORTION_EFFECT, AL_EFFECT_DISTORTION }, { "echo", ECHO_EFFECT, AL_EFFECT_ECHO }, { "equalizer", EQUALIZER_EFFECT, AL_EFFECT_EQUALIZER }, { "flanger", FLANGER_EFFECT, AL_EFFECT_FLANGER }, + { "fshifter", FSHIFTER_EFFECT, AL_EFFECT_FREQUENCY_SHIFTER }, { "modulator", MODULATOR_EFFECT, AL_EFFECT_RING_MODULATOR }, { "pshifter", PSHIFTER_EFFECT, AL_EFFECT_PITCH_SHIFTER }, { "dedicated", DEDICATED_EFFECT, AL_EFFECT_DEDICATED_LOW_FREQUENCY_EFFECT }, @@ -533,6 +535,13 @@ static void InitEffectParams(ALeffect *effect, ALenum type) effect->Props.Reverb.DecayHFLimit = AL_REVERB_DEFAULT_DECAY_HFLIMIT; effect->vtab = &ALreverb_vtable; break; + case AL_EFFECT_AUTOWAH: + effect->Props.Autowah.AttackTime = AL_AUTOWAH_DEFAULT_ATTACK_TIME; + effect->Props.Autowah.ReleaseTime = AL_AUTOWAH_DEFAULT_RELEASE_TIME; + effect->Props.Autowah.Resonance = AL_AUTOWAH_DEFAULT_RESONANCE; + effect->Props.Autowah.PeakGain = AL_AUTOWAH_DEFAULT_PEAK_GAIN; + effect->vtab = &ALautowah_vtable; + break; case AL_EFFECT_CHORUS: effect->Props.Chorus.Waveform = AL_CHORUS_DEFAULT_WAVEFORM; effect->Props.Chorus.Phase = AL_CHORUS_DEFAULT_PHASE; @@ -584,6 +593,12 @@ static void InitEffectParams(ALeffect *effect, ALenum type) effect->Props.Chorus.Delay = AL_FLANGER_DEFAULT_DELAY; effect->vtab = &ALflanger_vtable; break; + case AL_EFFECT_FREQUENCY_SHIFTER: + effect->Props.Fshifter.Frequency = AL_FREQUENCY_SHIFTER_DEFAULT_FREQUENCY; + effect->Props.Fshifter.LeftDirection = AL_FREQUENCY_SHIFTER_DEFAULT_LEFT_DIRECTION; + effect->Props.Fshifter.RightDirection = AL_FREQUENCY_SHIFTER_DEFAULT_RIGHT_DIRECTION; + effect->vtab = &ALfshifter_vtable; + break; case AL_EFFECT_RING_MODULATOR: effect->Props.Modulator.Frequency = AL_RING_MODULATOR_DEFAULT_FREQUENCY; effect->Props.Modulator.HighPassCutoff = AL_RING_MODULATOR_DEFAULT_HIGHPASS_CUTOFF; diff --git a/Engine/lib/openal-soft/OpenAL32/alFilter.c b/Engine/lib/openal-soft/OpenAL32/alFilter.c index 7d8a886c5..e57653e01 100644 --- a/Engine/lib/openal-soft/OpenAL32/alFilter.c +++ b/Engine/lib/openal-soft/OpenAL32/alFilter.c @@ -28,6 +28,9 @@ #include "alError.h" +#define FILTER_MIN_GAIN 0.0f +#define FILTER_MAX_GAIN 4.0f /* +12dB */ + extern inline void LockFilterList(ALCdevice *device); extern inline void UnlockFilterList(ALCdevice *device); @@ -347,7 +350,7 @@ static void ALlowpass_setParamf(ALfilter *filter, ALCcontext *context, ALenum pa switch(param) { case AL_LOWPASS_GAIN: - if(!(val >= AL_LOWPASS_MIN_GAIN && val <= AL_LOWPASS_MAX_GAIN)) + if(!(val >= FILTER_MIN_GAIN && val <= FILTER_MAX_GAIN)) SETERR_RETURN(context, AL_INVALID_VALUE,, "Low-pass gain %f out of range", val); filter->Gain = val; break; @@ -400,7 +403,7 @@ static void ALhighpass_setParamf(ALfilter *filter, ALCcontext *context, ALenum p switch(param) { case AL_HIGHPASS_GAIN: - if(!(val >= AL_HIGHPASS_MIN_GAIN && val <= AL_HIGHPASS_MAX_GAIN)) + if(!(val >= FILTER_MIN_GAIN && val <= FILTER_MAX_GAIN)) SETERR_RETURN(context, AL_INVALID_VALUE,, "High-pass gain out of range"); filter->Gain = val; break; @@ -453,7 +456,7 @@ static void ALbandpass_setParamf(ALfilter *filter, ALCcontext *context, ALenum p switch(param) { case AL_BANDPASS_GAIN: - if(!(val >= AL_BANDPASS_MIN_GAIN && val <= AL_BANDPASS_MAX_GAIN)) + if(!(val >= FILTER_MIN_GAIN && val <= FILTER_MAX_GAIN)) SETERR_RETURN(context, AL_INVALID_VALUE,, "Band-pass gain out of range"); filter->Gain = val; break; diff --git a/Engine/lib/openal-soft/OpenAL32/alSource.c b/Engine/lib/openal-soft/OpenAL32/alSource.c index ed6bd8ee3..d7c68e4e4 100644 --- a/Engine/lib/openal-soft/OpenAL32/alSource.c +++ b/Engine/lib/openal-soft/OpenAL32/alSource.c @@ -229,17 +229,16 @@ static inline bool SourceShouldUpdate(ALsource *source, ALCcontext *context) /** Can only be called while the mixer is locked! */ static void SendStateChangeEvent(ALCcontext *context, ALuint id, ALenum state) { + AsyncEvent evt = ASYNC_EVENT(EventType_SourceStateChange); ALbitfieldSOFT enabledevt; - AsyncEvent evt; enabledevt = ATOMIC_LOAD(&context->EnabledEvts, almemory_order_acquire); if(!(enabledevt&EventType_SourceStateChange)) return; - evt.EnumType = EventType_SourceStateChange; - evt.Type = AL_EVENT_TYPE_SOURCE_STATE_CHANGED_SOFT; - evt.ObjectId = id; - evt.Param = state; - snprintf(evt.Message, sizeof(evt.Message), "Source ID %u state changed to %s", id, + evt.u.user.type = AL_EVENT_TYPE_SOURCE_STATE_CHANGED_SOFT; + evt.u.user.id = id; + evt.u.user.param = state; + snprintf(evt.u.user.msg, sizeof(evt.u.user.msg), "Source ID %u state changed to %s", id, (state==AL_INITIAL) ? "AL_INITIAL" : (state==AL_PLAYING) ? "AL_PLAYING" : (state==AL_PAUSED) ? "AL_PAUSED" : @@ -1296,7 +1295,7 @@ static ALboolean GetSourcedv(ALsource *Source, ALCcontext *Context, SourceProp p */ values[0] = GetSourceSecOffset(Source, Context, &srcclock); almtx_lock(&device->BackendLock); - clocktime = V0(device->Backend,getClockLatency)(); + clocktime = GetClockLatency(device); almtx_unlock(&device->BackendLock); if(srcclock == (ALuint64)clocktime.ClockTime) values[1] = (ALdouble)clocktime.Latency / 1000000000.0; @@ -1560,7 +1559,7 @@ static ALboolean GetSourcei64v(ALsource *Source, ALCcontext *Context, SourceProp */ values[0] = GetSourceSampleOffset(Source, Context, &srcclock); almtx_lock(&device->BackendLock); - clocktime = V0(device->Backend,getClockLatency)(); + clocktime = GetClockLatency(device); almtx_unlock(&device->BackendLock); if(srcclock == (ALuint64)clocktime.ClockTime) values[1] = clocktime.Latency; @@ -2826,6 +2825,121 @@ done: ALCcontext_DecRef(context); } +AL_API void AL_APIENTRY alSourceQueueBufferLayersSOFT(ALuint src, ALsizei nb, const ALuint *buffers) +{ + ALCdevice *device; + ALCcontext *context; + ALbufferlistitem *BufferListStart; + ALbufferlistitem *BufferList; + ALbuffer *BufferFmt = NULL; + ALsource *source; + ALsizei i; + + if(nb == 0) + return; + + context = GetContextRef(); + if(!context) return; + + device = context->Device; + + LockSourceList(context); + if(!(nb >= 0 && nb < 16)) + SETERR_GOTO(context, AL_INVALID_VALUE, done, "Queueing %d buffer layers", nb); + if((source=LookupSource(context, src)) == NULL) + SETERR_GOTO(context, AL_INVALID_NAME, done, "Invalid source ID %u", src); + + if(source->SourceType == AL_STATIC) + { + /* Can't queue on a Static Source */ + SETERR_GOTO(context, AL_INVALID_OPERATION, done, "Queueing onto static source %u", src); + } + + /* Check for a valid Buffer, for its frequency and format */ + BufferList = source->queue; + while(BufferList) + { + for(i = 0;i < BufferList->num_buffers;i++) + { + if((BufferFmt=BufferList->buffers[i]) != NULL) + break; + } + if(BufferFmt) break; + BufferList = ATOMIC_LOAD(&BufferList->next, almemory_order_relaxed); + } + + LockBufferList(device); + BufferListStart = al_calloc(DEF_ALIGN, FAM_SIZE(ALbufferlistitem, buffers, nb)); + BufferList = BufferListStart; + ATOMIC_INIT(&BufferList->next, NULL); + BufferList->max_samples = 0; + BufferList->num_buffers = 0; + for(i = 0;i < nb;i++) + { + ALbuffer *buffer = NULL; + if(buffers[i] && (buffer=LookupBuffer(device, buffers[i])) == NULL) + SETERR_GOTO(context, AL_INVALID_NAME, buffer_error, "Queueing invalid buffer ID %u", + buffers[i]); + + BufferList->buffers[BufferList->num_buffers++] = buffer; + if(!buffer) continue; + + IncrementRef(&buffer->ref); + + BufferList->max_samples = maxi(BufferList->max_samples, buffer->SampleLen); + + if(buffer->MappedAccess != 0 && !(buffer->MappedAccess&AL_MAP_PERSISTENT_BIT_SOFT)) + SETERR_GOTO(context, AL_INVALID_OPERATION, buffer_error, + "Queueing non-persistently mapped buffer %u", buffer->id); + + if(BufferFmt == NULL) + BufferFmt = buffer; + else if(BufferFmt->Frequency != buffer->Frequency || + BufferFmt->FmtChannels != buffer->FmtChannels || + BufferFmt->OriginalType != buffer->OriginalType) + { + alSetError(context, AL_INVALID_OPERATION, "Queueing buffer with mismatched format"); + + buffer_error: + /* A buffer failed (invalid ID or format), so unlock and release + * each buffer we had. */ + while(BufferListStart) + { + ALbufferlistitem *next = ATOMIC_LOAD(&BufferListStart->next, + almemory_order_relaxed); + for(i = 0;i < BufferListStart->num_buffers;i++) + { + if((buffer=BufferListStart->buffers[i]) != NULL) + DecrementRef(&buffer->ref); + } + al_free(BufferListStart); + BufferListStart = next; + } + UnlockBufferList(device); + goto done; + } + } + /* All buffers good. */ + UnlockBufferList(device); + + /* Source is now streaming */ + source->SourceType = AL_STREAMING; + + if(!(BufferList=source->queue)) + source->queue = BufferListStart; + else + { + ALbufferlistitem *next; + while((next=ATOMIC_LOAD(&BufferList->next, almemory_order_relaxed)) != NULL) + BufferList = next; + ATOMIC_STORE(&BufferList->next, BufferListStart, almemory_order_release); + } + +done: + UnlockSourceList(context); + ALCcontext_DecRef(context); +} + AL_API ALvoid AL_APIENTRY alSourceUnqueueBuffers(ALuint src, ALsizei nb, ALuint *buffers) { ALCcontext *context; diff --git a/Engine/lib/openal-soft/OpenAL32/event.c b/Engine/lib/openal-soft/OpenAL32/event.c index 12636489b..4c9c0be2c 100644 --- a/Engine/lib/openal-soft/OpenAL32/event.c +++ b/Engine/lib/openal-soft/OpenAL32/event.c @@ -6,19 +6,16 @@ #include "AL/alext.h" #include "alMain.h" #include "alError.h" +#include "alAuxEffectSlot.h" #include "ringbuffer.h" -static int EventThread(void *arg) +int EventThread(void *arg) { ALCcontext *context = arg; + bool quitnow = false; - /* Clear all pending posts on the semaphore. */ - while(alsem_trywait(&context->EventSem) == althrd_success) - { - } - - while(1) + while(!quitnow) { ALbitfieldSOFT enabledevts; AsyncEvent evt; @@ -28,14 +25,24 @@ static int EventThread(void *arg) alsem_wait(&context->EventSem); continue; } - if(!evt.EnumType) - break; almtx_lock(&context->EventCbLock); - enabledevts = ATOMIC_LOAD(&context->EnabledEvts, almemory_order_acquire); - if(context->EventCb && (enabledevts&evt.EnumType) == evt.EnumType) - context->EventCb(evt.Type, evt.ObjectId, evt.Param, (ALsizei)strlen(evt.Message), - evt.Message, context->EventParam); + do { + quitnow = evt.EnumType == EventType_KillThread; + if(quitnow) break; + + if(evt.EnumType == EventType_ReleaseEffectState) + { + ALeffectState_DecRef(evt.u.EffectState); + continue; + } + + enabledevts = ATOMIC_LOAD(&context->EnabledEvts, almemory_order_acquire); + if(context->EventCb && (enabledevts&evt.EnumType) == evt.EnumType) + context->EventCb(evt.u.user.type, evt.u.user.id, evt.u.user.param, + (ALsizei)strlen(evt.u.user.msg), evt.u.user.msg, context->EventParam + ); + } while(ll_ringbuffer_read(context->AsyncEvents, (char*)&evt, 1) != 0); almtx_unlock(&context->EventCbLock); } return 0; @@ -46,7 +53,6 @@ AL_API void AL_APIENTRY alEventControlSOFT(ALsizei count, const ALenum *types, A ALCcontext *context; ALbitfieldSOFT enabledevts; ALbitfieldSOFT flags = 0; - bool isrunning; ALsizei i; context = GetContextRef(); @@ -74,13 +80,9 @@ AL_API void AL_APIENTRY alEventControlSOFT(ALsizei count, const ALenum *types, A SETERR_GOTO(context, AL_INVALID_ENUM, done, "Invalid event type 0x%04x", types[i]); } - almtx_lock(&context->EventThrdLock); if(enable) { - if(!context->AsyncEvents) - context->AsyncEvents = ll_ringbuffer_create(63, sizeof(AsyncEvent), false); enabledevts = ATOMIC_LOAD(&context->EnabledEvts, almemory_order_relaxed); - isrunning = !!enabledevts; while(ATOMIC_COMPARE_EXCHANGE_WEAK(&context->EnabledEvts, &enabledevts, enabledevts|flags, almemory_order_acq_rel, almemory_order_acquire) == 0) { @@ -88,35 +90,20 @@ AL_API void AL_APIENTRY alEventControlSOFT(ALsizei count, const ALenum *types, A * just try again. */ } - if(!isrunning && flags) - althrd_create(&context->EventThread, EventThread, context); } else { enabledevts = ATOMIC_LOAD(&context->EnabledEvts, almemory_order_relaxed); - isrunning = !!enabledevts; while(ATOMIC_COMPARE_EXCHANGE_WEAK(&context->EnabledEvts, &enabledevts, enabledevts&~flags, almemory_order_acq_rel, almemory_order_acquire) == 0) { } - if(isrunning && !(enabledevts&~flags)) - { - static const AsyncEvent kill_evt = { 0 }; - while(ll_ringbuffer_write(context->AsyncEvents, (const char*)&kill_evt, 1) == 0) - althrd_yield(); - alsem_post(&context->EventSem); - althrd_join(context->EventThread, NULL); - } - else - { - /* Wait to ensure the event handler sees the changed flags before - * returning. - */ - almtx_lock(&context->EventCbLock); - almtx_unlock(&context->EventCbLock); - } + /* Wait to ensure the event handler sees the changed flags before + * returning. + */ + almtx_lock(&context->EventCbLock); + almtx_unlock(&context->EventCbLock); } - almtx_unlock(&context->EventThrdLock); done: ALCcontext_DecRef(context); diff --git a/Engine/lib/openal-soft/appveyor.yml b/Engine/lib/openal-soft/appveyor.yml index 0e5b7ce40..6d826eedc 100644 --- a/Engine/lib/openal-soft/appveyor.yml +++ b/Engine/lib/openal-soft/appveyor.yml @@ -1,4 +1,4 @@ -version: 1.18.2.{build} +version: 1.19.0.{build} environment: matrix: diff --git a/Engine/lib/openal-soft/common/alcomplex.c b/Engine/lib/openal-soft/common/alcomplex.c new file mode 100644 index 000000000..d4045aebd --- /dev/null +++ b/Engine/lib/openal-soft/common/alcomplex.c @@ -0,0 +1,92 @@ + +#include "config.h" + +#include "alcomplex.h" +#include "math_defs.h" + + +extern inline ALcomplex complex_add(ALcomplex a, ALcomplex b); +extern inline ALcomplex complex_sub(ALcomplex a, ALcomplex b); +extern inline ALcomplex complex_mult(ALcomplex a, ALcomplex b); + +void complex_fft(ALcomplex *FFTBuffer, ALsizei FFTSize, ALdouble Sign) +{ + ALsizei i, j, k, mask, step, step2; + ALcomplex temp, u, w; + ALdouble arg; + + /* Bit-reversal permutation applied to a sequence of FFTSize items */ + for(i = 1;i < FFTSize-1;i++) + { + for(mask = 0x1, j = 0;mask < FFTSize;mask <<= 1) + { + if((i&mask) != 0) + j++; + j <<= 1; + } + j >>= 1; + + if(i < j) + { + temp = FFTBuffer[i]; + FFTBuffer[i] = FFTBuffer[j]; + FFTBuffer[j] = temp; + } + } + + /* Iterative form of Danielson–Lanczos lemma */ + for(i = 1, step = 2;i < FFTSize;i<<=1, step<<=1) + { + step2 = step >> 1; + arg = M_PI / step2; + + w.Real = cos(arg); + w.Imag = sin(arg) * Sign; + + u.Real = 1.0; + u.Imag = 0.0; + + for(j = 0;j < step2;j++) + { + for(k = j;k < FFTSize;k+=step) + { + temp = complex_mult(FFTBuffer[k+step2], u); + FFTBuffer[k+step2] = complex_sub(FFTBuffer[k], temp); + FFTBuffer[k] = complex_add(FFTBuffer[k], temp); + } + + u = complex_mult(u, w); + } + } +} + +void complex_hilbert(ALcomplex *Buffer, ALsizei size) +{ + const ALdouble inverse_size = 1.0/(ALdouble)size; + ALsizei todo, i; + + for(i = 0;i < size;i++) + Buffer[i].Imag = 0.0; + + complex_fft(Buffer, size, 1.0); + + todo = size >> 1; + Buffer[0].Real *= inverse_size; + Buffer[0].Imag *= inverse_size; + for(i = 1;i < todo;i++) + { + Buffer[i].Real *= 2.0*inverse_size; + Buffer[i].Imag *= 2.0*inverse_size; + } + Buffer[i].Real *= inverse_size; + Buffer[i].Imag *= inverse_size; + i++; + + for(;i < size;i++) + { + Buffer[i].Real = 0.0; + Buffer[i].Imag = 0.0; + } + + complex_fft(Buffer, size, -1.0); +} diff --git a/Engine/lib/openal-soft/common/alcomplex.h b/Engine/lib/openal-soft/common/alcomplex.h new file mode 100644 index 000000000..2418ce781 --- /dev/null +++ b/Engine/lib/openal-soft/common/alcomplex.h @@ -0,0 +1,71 @@ +#ifndef ALCOMPLEX_H +#define ALCOMPLEX_H + +#include "AL/al.h" + + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct ALcomplex { + ALdouble Real; + ALdouble Imag; +} ALcomplex; + +/** Addition of two complex numbers. */ +inline ALcomplex complex_add(ALcomplex a, ALcomplex b) +{ + ALcomplex result; + + result.Real = a.Real + b.Real; + result.Imag = a.Imag + b.Imag; + + return result; +} + +/** Subtraction of two complex numbers. */ +inline ALcomplex complex_sub(ALcomplex a, ALcomplex b) +{ + ALcomplex result; + + result.Real = a.Real - b.Real; + result.Imag = a.Imag - b.Imag; + + return result; +} + +/** Multiplication of two complex numbers. */ +inline ALcomplex complex_mult(ALcomplex a, ALcomplex b) +{ + ALcomplex result; + + result.Real = a.Real*b.Real - a.Imag*b.Imag; + result.Imag = a.Imag*b.Real + a.Real*b.Imag; + + return result; +} + +/** + * Iterative implementation of 2-radix FFT (In-place algorithm). Sign = -1 is + * FFT and 1 is iFFT (inverse). Fills FFTBuffer[0...FFTSize-1] with the + * Discrete Fourier Transform (DFT) of the time domain data stored in + * FFTBuffer[0...FFTSize-1]. FFTBuffer is an array of complex numbers, FFTSize + * MUST BE power of two. + */ +void complex_fft(ALcomplex *FFTBuffer, ALsizei FFTSize, ALdouble Sign); + +/** + * Calculate the complex helical sequence (discrete-time analytical signal) of + * the given input using the discrete Hilbert transform (In-place algorithm). + * Fills Buffer[0...size-1] with the discrete-time analytical signal stored in + * Buffer[0...size-1]. Buffer is an array of complex numbers, size MUST BE + * power of two. + */ +void complex_hilbert(ALcomplex *Buffer, ALsizei size); + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif /* ALCOMPLEX_H */ diff --git a/Engine/lib/openal-soft/common/math_defs.h b/Engine/lib/openal-soft/common/math_defs.h index 8ce93d0a9..aa79b6956 100644 --- a/Engine/lib/openal-soft/common/math_defs.h +++ b/Engine/lib/openal-soft/common/math_defs.h @@ -18,6 +18,12 @@ #define FLT_EPSILON (1.19209290e-07f) #endif +#define SQRT_2 1.41421356237309504880 +#define SQRT_3 1.73205080756887719318 + +#define SQRTF_2 1.41421356237309504880f +#define SQRTF_3 1.73205080756887719318f + #ifndef HUGE_VALF static const union msvc_inf_hack { unsigned char b[4]; @@ -40,7 +46,20 @@ static inline float cbrtf(float f) } #endif -#define DEG2RAD(x) ((float)(x) * (F_PI/180.0f)) -#define RAD2DEG(x) ((float)(x) * (180.0f/F_PI)) +#ifndef HAVE_COPYSIGNF +static inline float copysignf(float x, float y) +{ + union { + float f; + unsigned int u; + } ux = { x }, uy = { y }; + ux.u &= 0x7fffffffu; + ux.u |= (uy.u&0x80000000u); + return ux.f; +} +#endif + +#define DEG2RAD(x) ((float)(x) * (float)(M_PI/180.0)) +#define RAD2DEG(x) ((float)(x) * (float)(180.0/M_PI)) #endif /* AL_MATH_DEFS_H */ diff --git a/Engine/lib/openal-soft/common/threads.c b/Engine/lib/openal-soft/common/threads.c index 2655a2445..6cfe383ba 100644 --- a/Engine/lib/openal-soft/common/threads.c +++ b/Engine/lib/openal-soft/common/threads.c @@ -428,7 +428,11 @@ void althrd_thread_detach(void) { void *ptr = altss_get(TlsDestructors.keys[i]); altss_dtor_t callback = (altss_dtor_t)TlsDestructors.values[i]; - if(ptr && callback) callback(ptr); + if(ptr) + { + if(callback) callback(ptr); + altss_set(TlsDestructors.keys[i], NULL); + } } UnlockUIntMapRead(&TlsDestructors); } diff --git a/Engine/lib/openal-soft/config.h.in b/Engine/lib/openal-soft/config.h.in index 1ff64fe47..9cc6c16b3 100644 --- a/Engine/lib/openal-soft/config.h.in +++ b/Engine/lib/openal-soft/config.h.in @@ -98,6 +98,9 @@ /* Define if we have the cbrtf function */ #cmakedefine HAVE_CBRTF +/* Define if we have the copysignf function */ +#cmakedefine HAVE_COPYSIGNF + /* Define if we have the strtof function */ #cmakedefine HAVE_STRTOF diff --git a/Engine/lib/sdl/Android.mk b/Engine/lib/sdl/Android.mk index d56b5c00c..9ce879a11 100755 --- a/Engine/lib/sdl/Android.mk +++ b/Engine/lib/sdl/Android.mk @@ -31,11 +31,13 @@ LOCAL_SRC_FILES := \ $(wildcard $(LOCAL_PATH)/src/haptic/android/*.c) \ $(wildcard $(LOCAL_PATH)/src/joystick/*.c) \ $(wildcard $(LOCAL_PATH)/src/joystick/android/*.c) \ - $(LOCAL_PATH)/src/joystick/steam/SDL_steamcontroller.c \ + $(wildcard $(LOCAL_PATH)/src/joystick/hidapi/*.c) \ $(wildcard $(LOCAL_PATH)/src/loadso/dlopen/*.c) \ $(wildcard $(LOCAL_PATH)/src/power/*.c) \ $(wildcard $(LOCAL_PATH)/src/power/android/*.c) \ $(wildcard $(LOCAL_PATH)/src/filesystem/android/*.c) \ + $(wildcard $(LOCAL_PATH)/src/sensor/*.c) \ + $(wildcard $(LOCAL_PATH)/src/sensor/android/*.c) \ $(wildcard $(LOCAL_PATH)/src/render/*.c) \ $(wildcard $(LOCAL_PATH)/src/render/*/*.c) \ $(wildcard $(LOCAL_PATH)/src/stdlib/*.c) \ @@ -48,10 +50,14 @@ LOCAL_SRC_FILES := \ $(wildcard $(LOCAL_PATH)/src/video/yuv2rgb/*.c) \ $(wildcard $(LOCAL_PATH)/src/test/*.c)) +LOCAL_SHARED_LIBRARIES := hidapi + LOCAL_CFLAGS += -DGL_GLEXT_PROTOTYPES LOCAL_LDLIBS := -ldl -lGLESv1_CM -lGLESv2 -llog -landroid -cmd-strip := +ifeq ($(NDK_DEBUG),1) + cmd-strip := +endif include $(BUILD_SHARED_LIBRARY) @@ -86,4 +92,19 @@ LOCAL_MODULE_FILENAME := libSDL2main include $(BUILD_STATIC_LIBRARY) +########################### +# +# hidapi library +# +########################### +include $(CLEAR_VARS) + +LOCAL_CPPFLAGS += -std=c++11 + +LOCAL_SRC_FILES := src/hidapi/android/hid.cpp + +LOCAL_MODULE := libhidapi +LOCAL_LDLIBS := -llog + +include $(BUILD_SHARED_LIBRARY) diff --git a/Engine/lib/sdl/CMakeLists.txt b/Engine/lib/sdl/CMakeLists.txt index 8f1e828d1..0128c7ac7 100644 --- a/Engine/lib/sdl/CMakeLists.txt +++ b/Engine/lib/sdl/CMakeLists.txt @@ -42,10 +42,13 @@ include(${SDL2_SOURCE_DIR}/cmake/sdlchecks.cmake) # set SDL_BINARY_AGE and SDL_INTERFACE_AGE to 0. set(SDL_MAJOR_VERSION 2) set(SDL_MINOR_VERSION 0) -set(SDL_MICRO_VERSION 8) +set(SDL_MICRO_VERSION 9) set(SDL_INTERFACE_AGE 0) -set(SDL_BINARY_AGE 8) +set(SDL_BINARY_AGE 9) set(SDL_VERSION "${SDL_MAJOR_VERSION}.${SDL_MINOR_VERSION}.${SDL_MICRO_VERSION}") +# the following should match the versions in Xcode project file: +set(DYLIB_CURRENT_VERSION 10.0.0) +set(DYLIB_COMPATIBILITY_VERSION 1.0.0) # Set defaults preventing destination file conflicts set(SDL_CMAKE_DEBUG_POSTFIX "d" @@ -210,8 +213,14 @@ endif() set(SDL_LIBS "-lSDL2") set(SDL_CFLAGS "") -# Emscripten toolchain has a nonempty default value for this, and the checks -# in this file need to change that, so remember the original value, and +# When building shared lib for Windows with MinGW, +# avoid the DLL having a "lib" prefix +if(WINDOWS) + set(CMAKE_SHARED_LIBRARY_PREFIX "") +endif() + +# Emscripten toolchain has a nonempty default value for this, and the checks +# in this file need to change that, so remember the original value, and # restore back to that afterwards. For check_function_exists() to work in # Emscripten, this value must be at its default value. set(ORIG_CMAKE_REQUIRED_FLAGS ${CMAKE_REQUIRED_FLAGS}) @@ -244,7 +253,7 @@ endif() set(OPT_DEF_ASM TRUE) if(EMSCRIPTEN) # Set up default values for the currently supported set of subsystems: - # Emscripten/Javascript does not have assembly support, a dynamic library + # Emscripten/Javascript does not have assembly support, a dynamic library # loading architecture, low-level CPU inspection or multithreading. set(OPT_DEF_ASM FALSE) set(SDL_SHARED_ENABLED_BY_DEFAULT OFF) @@ -261,7 +270,7 @@ endif() set(SDL_SUBSYSTEMS Atomic Audio Video Render Events Joystick Haptic Power Threads Timers - File Loadso CPUinfo Filesystem Dlopen) + File Loadso CPUinfo Filesystem Dlopen Sensor) foreach(_SUB ${SDL_SUBSYSTEMS}) string(TOUPPER ${_SUB} _OPT) if (NOT DEFINED SDL_${_OPT}_ENABLED_BY_DEFAULT) @@ -328,6 +337,7 @@ foreach(_SUB ${SDL_X11_OPTIONS}) endforeach() set_option(VIDEO_COCOA "Use Cocoa video driver" ${APPLE}) set_option(DIRECTX "Use DirectX for Windows audio/video" ${WINDOWS}) +set_option(WASAPI "Use the Windows WASAPI audio driver" ${WINDOWS}) set_option(RENDER_D3D "Enable the Direct3D render driver" ${WINDOWS}) set_option(VIDEO_VIVANTE "Use Vivante EGL video driver" ${UNIX_SYS}) dep_option(VIDEO_VULKAN "Enable Vulkan support" ON "ANDROID OR APPLE OR LINUX OR WINDOWS" OFF) @@ -444,6 +454,8 @@ if(USE_GCC OR USE_CLANG) if(APPLE) list(APPEND EXTRA_LDFLAGS "-Wl,-undefined,error") + list(APPEND EXTRA_LDFLAGS "-Wl,-compatibility_version,${DYLIB_COMPATIBILITY_VERSION}") + list(APPEND EXTRA_LDFLAGS "-Wl,-current_version,${DYLIB_CURRENT_VERSION}") else() set(CMAKE_REQUIRED_FLAGS "-Wl,--no-undefined") check_c_compiler_flag("" HAVE_NO_UNDEFINED) @@ -643,7 +655,7 @@ if(LIBC) _ultoa strtol strtoul strtoll strtod atoi atof strcmp strncmp _stricmp _strnicmp sscanf acos acosf asin asinf atan atanf atan2 atan2f ceil ceilf - copysign copysignf cos cosf fabs fabsf floor floorf fmod fmodf + copysign copysignf cos cosf exp expf fabs fabsf floor floorf fmod fmodf log logf log10 log10f pow powf scalbn scalbnf sin sinf sqrt sqrtf tan tanf) string(TOUPPER ${_FN} _UPPER) set(HAVE_${_UPPER} 1) @@ -743,6 +755,10 @@ if(SDL_HAPTIC) file(GLOB HAPTIC_SOURCES ${SDL2_SOURCE_DIR}/src/haptic/*.c) set(SOURCE_FILES ${SOURCE_FILES} ${HAPTIC_SOURCES}) endif() +if(SDL_SENSOR) + file(GLOB SENSOR_SOURCES ${SDL2_SOURCE_DIR}/src/sensor/*.c) + set(SOURCE_FILES ${SOURCE_FILES} ${SENSOR_SOURCES}) +endif() if(SDL_POWER) file(GLOB POWER_SOURCES ${SDL2_SOURCE_DIR}/src/power/*.c) set(SOURCE_FILES ${SOURCE_FILES} ${POWER_SOURCES}) @@ -844,6 +860,12 @@ if(ANDROID) set(SOURCE_FILES ${SOURCE_FILES} ${TIMER_SOURCES}) set(HAVE_SDL_TIMERS TRUE) endif() + if(SDL_SENSOR) + set(SDL_SENSOR_ANDROID 1) + set(HAVE_SDL_SENSORS TRUE) + file(GLOB ANDROID_SENSOR_SOURCES ${SDL2_SOURCE_DIR}/src/sensor/android/*.c) + set(SOURCE_FILES ${SOURCE_FILES} ${ANDROID_SENSOR_SOURCES}) + endif() if(SDL_VIDEO) set(SDL_VIDEO_DRIVER_ANDROID 1) file(GLOB ANDROID_VIDEO_SOURCES ${SDL2_SOURCE_DIR}/src/video/android/*.c) @@ -1001,7 +1023,7 @@ elseif(UNIX AND NOT APPLE AND NOT ANDROID) #include #include - int main(int argc, char **argv) + int main(int argc, char **argv) { struct kbentry kbe; kbe.kb_table = KG_CTRL; @@ -1042,6 +1064,12 @@ elseif(UNIX AND NOT APPLE AND NOT ANDROID) include_directories(${IBUS_INCLUDE_DIRS}) list(APPEND EXTRA_LIBS ${IBUS_LIBRARIES}) endif() + if(HAVE_LIBUNWIND_H) + # We've already found the header, so REQUIRE the lib to be present + pkg_search_module(UNWIND REQUIRED libunwind) + pkg_search_module(UNWIND_GENERIC REQUIRED libunwind-generic) + list(APPEND EXTRA_LIBS ${UNWIND_LIBRARIES} ${UNWIND_GENERIC_LIBRARIES}) + endif() endif() check_include_file("fcitx/frontend.h" HAVE_FCITX_FRONTEND_H) @@ -1183,8 +1211,6 @@ elseif(WINDOWS) check_include_file(ddraw.h HAVE_DDRAW_H) check_include_file(dsound.h HAVE_DSOUND_H) check_include_file(dinput.h HAVE_DINPUT_H) - check_include_file(mmdeviceapi.h HAVE_MMDEVICEAPI_H) - check_include_file(audioclient.h HAVE_AUDIOCLIENT_H) check_include_file(dxgi.h HAVE_DXGI_H) if(HAVE_D3D_H OR HAVE_D3D11_H OR HAVE_DDRAW_H OR HAVE_DSOUND_H OR HAVE_DINPUT_H) set(HAVE_DIRECTX TRUE) @@ -1197,6 +1223,11 @@ elseif(WINDOWS) set(CMAKE_REQUIRED_FLAGS ${ORIG_CMAKE_REQUIRED_FLAGS}) endif() + # headers needed elsewhere ... + check_include_file(mmdeviceapi.h HAVE_MMDEVICEAPI_H) + check_include_file(audioclient.h HAVE_AUDIOCLIENT_H) + check_include_file(endpointvolume.h HAVE_ENDPOINTVOLUME_H) + if(SDL_AUDIO) set(SDL_AUDIO_DRIVER_WINMM 1) file(GLOB WINMM_AUDIO_SOURCES ${SDL2_SOURCE_DIR}/src/audio/winmm/*.c) @@ -1209,7 +1240,7 @@ elseif(WINDOWS) set(SOURCE_FILES ${SOURCE_FILES} ${DSOUND_AUDIO_SOURCES}) endif() - if(HAVE_AUDIOCLIENT_H AND HAVE_MMDEVICEAPI_H) + if(WASAPI AND HAVE_AUDIOCLIENT_H AND HAVE_MMDEVICEAPI_H) set(SDL_AUDIO_DRIVER_WASAPI 1) file(GLOB WASAPI_AUDIO_SOURCES ${SDL2_SOURCE_DIR}/src/audio/wasapi/*.c) set(SOURCE_FILES ${SOURCE_FILES} ${WASAPI_AUDIO_SOURCES}) @@ -1261,7 +1292,7 @@ elseif(WINDOWS) endif() # Libraries for Win32 native and MinGW - list(APPEND EXTRA_LIBS user32 gdi32 winmm imm32 ole32 oleaut32 version uuid) + list(APPEND EXTRA_LIBS user32 gdi32 winmm imm32 ole32 oleaut32 version uuid advapi32 shell32) # TODO: in configure.in the check for timers is set on # cygwin | mingw32* - does this include mingw32CE? @@ -1527,6 +1558,7 @@ endif() if(VIDEO_VULKAN) set(SDL_VIDEO_VULKAN 1) + set(HAVE_VIDEO_VULKAN TRUE) endif() # Dummies @@ -1538,7 +1570,7 @@ endif() # This leads to missing internal references on building, since the # src/X/*.c does not get included. if(NOT HAVE_SDL_JOYSTICK) - set(SDL_JOYSTICK_DISABLED 1) + set(SDL_JOYSTICK_DUMMY 1) if(SDL_JOYSTICK AND NOT APPLE) # results in unresolved symbols on OSX file(GLOB JOYSTICK_SOURCES ${SDL2_SOURCE_DIR}/src/joystick/dummy/*.c) @@ -1546,10 +1578,15 @@ if(NOT HAVE_SDL_JOYSTICK) endif() endif() if(NOT HAVE_SDL_HAPTIC) - set(SDL_HAPTIC_DISABLED 1) + set(SDL_HAPTIC_DUMMY 1) file(GLOB HAPTIC_SOURCES ${SDL2_SOURCE_DIR}/src/haptic/dummy/*.c) set(SOURCE_FILES ${SOURCE_FILES} ${HAPTIC_SOURCES}) endif() +if(NOT HAVE_SDL_SENSORS) + set(SDL_SENSOR_DUMMY 1) + file(GLOB SENSORS_SOURCES ${SDL2_SOURCE_DIR}/src/sensor/dummy/*.c) + set(SOURCE_FILES ${SOURCE_FILES} ${SENSORS_SOURCES}) +endif() if(NOT HAVE_SDL_LOADSO) set(SDL_LOADSO_DISABLED 1) file(GLOB LOADSO_SOURCES ${SDL2_SOURCE_DIR}/src/loadso/dummy/*.c) @@ -1695,7 +1732,7 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${EXTRA_CFLAGS}") # Always build SDLmain add_library(SDL2main STATIC ${SDLMAIN_SOURCES}) -target_include_directories(SDL2main PUBLIC $) +target_include_directories(SDL2main PUBLIC "$" $) set(_INSTALL_LIBS "SDL2main") if (NOT ANDROID) set_target_properties(SDL2main PROPERTIES DEBUG_POSTFIX ${SDL_CMAKE_DEBUG_POSTFIX}) @@ -1704,7 +1741,9 @@ endif() if(SDL_SHARED) add_library(SDL2 SHARED ${SOURCE_FILES} ${VERSION_SOURCES}) if(APPLE) - set_target_properties(SDL2 PROPERTIES MACOSX_RPATH 1) + set_target_properties(SDL2 PROPERTIES + MACOSX_RPATH 1 + OUTPUT_NAME "SDL2-${LT_RELEASE}") elseif(UNIX AND NOT ANDROID) set_target_properties(SDL2 PROPERTIES VERSION ${LT_VERSION} @@ -1724,7 +1763,7 @@ if(SDL_SHARED) endif() set(_INSTALL_LIBS "SDL2" ${_INSTALL_LIBS}) target_link_libraries(SDL2 ${EXTRA_LIBS} ${EXTRA_LDFLAGS}) - target_include_directories(SDL2 PUBLIC $) + target_include_directories(SDL2 PUBLIC "$" $) if (NOT ANDROID) set_target_properties(SDL2 PROPERTIES DEBUG_POSTFIX ${SDL_CMAKE_DEBUG_POSTFIX}) endif() @@ -1750,7 +1789,7 @@ if(SDL_STATIC) # libraries - do we need to consider this? set(_INSTALL_LIBS "SDL2-static" ${_INSTALL_LIBS}) target_link_libraries(SDL2-static ${EXTRA_LIBS} ${EXTRA_LDFLAGS}) - target_include_directories(SDL2-static PUBLIC $) + target_include_directories(SDL2-static PUBLIC "$" $) if (NOT ANDROID) set_target_properties(SDL2-static PROPERTIES DEBUG_POSTFIX ${SDL_CMAKE_DEBUG_POSTFIX}) endif() @@ -1761,7 +1800,7 @@ endif() if(SDL_TEST) file(GLOB TEST_SOURCES ${SDL2_SOURCE_DIR}/src/test/*.c) add_library(SDL2_test STATIC ${TEST_SOURCES}) - + add_subdirectory(test) endif() @@ -1808,18 +1847,23 @@ endforeach() list(APPEND INCLUDE_FILES ${BIN_INCLUDE_FILES}) install(FILES ${INCLUDE_FILES} DESTINATION include/SDL2) +string(TOUPPER "${CMAKE_BUILD_TYPE}" UPPER_BUILD_TYPE) +if (UPPER_BUILD_TYPE MATCHES DEBUG) + set(SOPOSTFIX "${SDL_CMAKE_DEBUG_POSTFIX}") +else() + set(SOPOSTFIX "") +endif() + if(NOT (WINDOWS OR CYGWIN)) if(SDL_SHARED) - if (APPLE) - set(SOEXT "dylib") - else() - set(SOEXT "so") - endif() + set(SOEXT ${CMAKE_SHARED_LIBRARY_SUFFIX}) # ".so", ".dylib", etc. + get_target_property(SONAME SDL2 OUTPUT_NAME) if(NOT ANDROID) install(CODE " execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink - \"libSDL2-2.0.${SOEXT}\" \"libSDL2.${SOEXT}\")") - install(FILES ${SDL2_BINARY_DIR}/libSDL2.${SOEXT} DESTINATION "lib${LIB_SUFFIX}") + \"lib${SONAME}${SOPOSTFIX}${SOEXT}\" \"libSDL2${SOPOSTFIX}${SOEXT}\")" + WORKING_DIR "${SDL2_BINARY_DIR}") + install(FILES ${SDL2_BINARY_DIR}/libSDL2${SOPOSTFIX}${SOEXT} DESTINATION "lib${LIB_SUFFIX}") endif() endif() if(FREEBSD) diff --git a/Engine/lib/sdl/Makefile.in b/Engine/lib/sdl/Makefile.in index fe566523d..4eb6e6323 100644 --- a/Engine/lib/sdl/Makefile.in +++ b/Engine/lib/sdl/Makefile.in @@ -44,7 +44,7 @@ SDLTEST_OBJECTS = @SDLTEST_OBJECTS@ WAYLAND_SCANNER = @WAYLAND_SCANNER@ -SRC_DIST = *.txt acinclude Android.mk autogen.sh android-project build-scripts cmake cmake_uninstall.cmake.in configure configure.in debian docs include Makefile.* sdl2-config.cmake.in sdl2-config.in sdl2.m4 sdl2.pc.in SDL2.spec.in SDL2Config.cmake src test VisualC.html VisualC VisualC-WinRT Xcode Xcode-iOS +SRC_DIST = *.txt acinclude Android.mk autogen.sh android-project build-scripts cmake cmake_uninstall.cmake.in configure configure.in debian docs include Makefile.* sdl2-config.cmake.in sdl2-config.in sdl2.m4 sdl2.pc.in SDL2.spec.in SDL2Config.cmake src test VisualC.html VisualC VisualC-WinRT Xcode Xcode-iOS wayland-protocols GEN_DIST = SDL2.spec ifneq ($V,1) @@ -101,6 +101,7 @@ HDRS = \ SDL_render.h \ SDL_rwops.h \ SDL_scancode.h \ + SDL_sensor.h \ SDL_shape.h \ SDL_stdinc.h \ SDL_surface.h \ diff --git a/Engine/lib/sdl/Makefile.minimal b/Engine/lib/sdl/Makefile.minimal index 6ec1ce81c..7f0264974 100644 --- a/Engine/lib/sdl/Makefile.minimal +++ b/Engine/lib/sdl/Makefile.minimal @@ -22,6 +22,8 @@ SOURCES = \ src/filesystem/dummy/*.c \ src/render/*.c \ src/render/software/*.c \ + src/sensor/*.c \ + src/sensor/dummy/*.c \ src/stdlib/*.c \ src/thread/*.c \ src/thread/generic/*.c \ diff --git a/Engine/lib/sdl/Makefile.os2 b/Engine/lib/sdl/Makefile.os2 new file mode 100644 index 000000000..95c5cef59 --- /dev/null +++ b/Engine/lib/sdl/Makefile.os2 @@ -0,0 +1,126 @@ +# Open Watcom makefile to build SDL2.dll for OS/2: +# wmake -f Makefile.os2 + +LIBNAME = SDL2 +VERSION = 2.0.9 +DESCRIPTION = Simple DirectMedia Layer 2 + +LIBHOME = . +LIBPATH = $(LIBHOME)/lib +DLLFILE = $(LIBHOME)/$(LIBNAME).dll +LIBFILE = $(LIBHOME)/$(LIBNAME).lib +LNKFILE = $(LIBNAME).lnk + +INCPATH = -I"$(%WATCOM)/h/os2" -I"$(%WATCOM)/h" +INCPATH+= -I"$(LIBHOME)/h" +INCPATH+= -Iinclude + +LIBS = mmpm2.lib libuls.lib libconv.lib + +CFLAGS = -bt=os2 -d0 -q -bm -5s -fp5 -fpi87 -sg -oteanbmier -ei +# max warnings: +CFLAGS+= -wx +# building dll: +CFLAGS+= -bd +# the include paths : +CFLAGS+= $(INCPATH) +# building SDL itself: +CFLAGS+= -DBUILD_SDL + +SRCS = SDL.c SDL_assert.c SDL_error.c SDL_log.c SDL_dataqueue.c SDL_hints.c +SRCS+= SDL_getenv.c SDL_iconv.c SDL_malloc.c SDL_qsort.c SDL_stdlib.c SDL_string.c +SRCS+= SDL_cpuinfo.c SDL_atomic.c SDL_spinlock.c SDL_thread.c SDL_timer.c +SRCS+= SDL_rwops.c SDL_power.c +SRCS+= SDL_audio.c SDL_audiocvt.c SDL_audiodev.c SDL_audiotypecvt.c SDL_mixer.c SDL_wave.c +SRCS+= SDL_events.c SDL_quit.c SDL_keyboard.c SDL_mouse.c SDL_windowevents.c & + SDL_clipboardevents.c SDL_dropevents.c SDL_displayevents.c SDL_gesture.c & + SDL_sensor.c SDL_touch.c +SRCS+= SDL_haptic.c SDL_gamecontroller.c SDL_joystick.c +SRCS+= SDL_render.c yuv_rgb.c SDL_yuv.c SDL_yuv_sw.c SDL_blendfillrect.c & + SDL_blendline.c SDL_blendpoint.c SDL_drawline.c SDL_drawpoint.c & + SDL_render_sw.c SDL_rotate.c +SRCS+= SDL_blit.c SDL_blit_0.c SDL_blit_1.c SDL_blit_A.c SDL_blit_auto.c & + SDL_blit_copy.c SDL_blit_N.c SDL_blit_slow.c SDL_fillrect.c SDL_bmp.c & + SDL_pixels.c SDL_rect.c SDL_RLEaccel.c SDL_shape.c SDL_stretch.c & + SDL_surface.c SDL_video.c SDL_clipboard.c SDL_vulkan_utils.c SDL_egl.c + +SRCS+= SDL_syscond.c SDL_sysmutex.c SDL_syssem.c SDL_systhread.c SDL_systls.c +SRCS+= SDL_systimer.c +SRCS+= SDL_sysloadso.c +SRCS+= SDL_sysfilesystem.c +SRCS+= SDL_syshaptic.c SDL_sysjoystick.c +SRCS+= SDL_dummyaudio.c SDL_diskaudio.c +SRCS+= SDL_nullvideo.c SDL_nullframebuffer.c SDL_nullevents.c +SRCS+= SDL_dummysensor.c + +SRCS+= SDL_dynapi.c + +OBJS = $(SRCS:.c=.obj) + +.extensions: +.extensions: .lib .dll .obj .c .asm + +.c: ./src;./src/dynapi;./src/audio;./src/cpuinfo;./src/events;./src/file;./src/haptic;./src/joystick;./src/power;./src/render;./src/render/software;./src/sensor;./src/stdlib;./src/thread;./src/timer;./src/video;./src/video/yuv2rgb;./src/atomic;./src/audio/disk; +.c: ./src/haptic/dummy;./src/joystick/dummy;./src/audio/dummy;./src/video/dummy;./src/sensor/dummy; +.c: ./src/loadso/dummy;./src/filesystem/dummy;./src/timer/dummy;./src/thread/generic; + +all: $(DLLFILE) $(LIBFILE) .symbolic + +$(DLLFILE): $(OBJS) $(LNKFILE) + @echo * Linking: $@ + wlink @$(LNKFILE) + +$(LIBFILE): $(DLLFILE) + @echo * Creating LIB file: $@ + wlib -q -b -n -c -pa -s -t -zld -ii -io $* $(DLLFILE) + +.c.obj: + wcc386 $(CFLAGS) -fo=$^@ $< + +SDL_cpuinfo.obj: SDL_cpuinfo.c + wcc386 $(CFLAGS) -wcd=200 -fo=$^@ $< + +SDL_rwops.obj: SDL_rwops.c + wcc386 $(CFLAGS) -wcd=136 -fo=$^@ $< + +SDL_blendfillrect.obj: SDL_blendfillrect.c + wcc386 $(CFLAGS) -wcd=200 -fo=$^@ $< + +SDL_blendline.obj: SDL_blendline.c + wcc386 $(CFLAGS) -wcd=200 -fo=$^@ $< + +SDL_blendpoint.obj: SDL_blendpoint.c + wcc386 $(CFLAGS) -wcd=200 -fo=$^@ $< + +SDL_RLEaccel.obj: SDL_RLEaccel.c + wcc386 $(CFLAGS) -wcd=201 -fo=$^@ $< + +$(LNKFILE): + @echo * Creating linker file: $@ + @%create $@ + @%append $@ SYSTEM os2v2_dll INITINSTANCE TERMINSTANCE + @%append $@ NAME $(DLLFILE) + @for %i in ($(OBJS)) do @%append $@ FILE %i + @%append $@ LIBPATH $(%LIB);$(LIBPATH) + @for %i in ($(LIBS)) do @%append $@ LIB %i + @%append $@ OPTION QUIET + @%append $@ OPTION IMPF=$(LIBHOME)/$^&.exp + @%append $@ OPTION MAP=$(LIBHOME)/$^&.map + @%append $@ OPTION DESCRIPTION '@$#libsdl org:$(VERSION)$#@$(DESCRIPTION)' + @%append $@ OPTION QUIET + @%append $@ OPTION ELIMINATE + @%append $@ OPTION MANYAUTODATA + @%append $@ OPTION OSNAME='OS/2 and eComStation' + @%append $@ OPTION SHOWDEAD + +clean: .SYMBOLIC + @ echo * Clean: $(LIBNAME) + @if exist *.obj rm *.obj + @if exist *.err rm *.err + @if exist $(LNKFILE) rm $(LNKFILE) + +distclean: .SYMBOLIC clean + @if exist $(LIBHOME)/*.exp rm $(LIBHOME)/*.exp + @if exist $(LIBHOME)/*.map rm $(LIBHOME)/*.map + @if exist $(LIBFILE) rm $(LIBFILE) + @if exist $(DLLFILE) rm $(DLLFILE) diff --git a/Engine/lib/sdl/Makefile.pandora b/Engine/lib/sdl/Makefile.pandora index 56f171b4e..f4cb66848 100644 --- a/Engine/lib/sdl/Makefile.pandora +++ b/Engine/lib/sdl/Makefile.pandora @@ -12,15 +12,38 @@ CFLAGS = -O3 -march=armv7-a -mcpu=cortex-a8 -mtune=cortex-a8 -mfloat-abi=softfp TARGET = libSDL.a -SOURCES = ./src/*.c ./src/audio/*.c ./src/cpuinfo/*.c ./src/events/*.c \ - ./src/file/*.c ./src/stdlib/*.c ./src/thread/*.c ./src/timer/*.c ./src/video/*.c \ - ./src/joystick/*.c ./src/haptic/*.c ./src/power/*.c ./src/video/dummy/*.c ./src/audio/disk/*.c \ - ./src/audio/dummy/*.c ./src/loadso/dlopen/*.c ./src/audio/dsp/*.c \ - ./src/thread/pthread/SDL_systhread.c ./src/thread/pthread/SDL_syssem.c \ - ./src/thread/pthread/SDL_sysmutex.c ./src/thread/pthread/SDL_syscond.c \ - ./src/joystick/linux/*.c ./src/haptic/linux/*.c ./src/timer/unix/*.c \ - ./src/atomic/*.c ./src/filesystem/unix/*.c \ - ./src/video/pandora/SDL_pandora.o ./src/video/pandora/SDL_pandora_events.o ./src/video/x11/*.c +SOURCES = + ./src/*.c \ + ./src/atomic/*.c \ + ./src/audio/*.c \ + ./src/audio/disk/*.c \ + ./src/audio/dsp/*.c \ + ./src/audio/dummy/*.c \ + ./src/cpuinfo/*.c \ + ./src/events/*.c \ + ./src/file/*.c \ + ./src/filesystem/unix/*.c \ + ./src/haptic/*.c \ + ./src/haptic/linux/*.c \ + ./src/joystick/*.c \ + ./src/joystick/linux/*.c \ + ./src/loadso/dlopen/*.c \ + ./src/power/*.c \ + ./src/sensor/*.c \ + ./src/sensor/dummy/*.c \ + ./src/stdlib/*.c \ + ./src/thread/*.c \ + ./src/thread/pthread/SDL_syscond.c \ + ./src/thread/pthread/SDL_sysmutex.c \ + ./src/thread/pthread/SDL_syssem.c \ + ./src/thread/pthread/SDL_systhread.c \ + ./src/timer/*.c \ + ./src/timer/unix/*.c \ + ./src/video/*.c \ + ./src/video/dummy/*.c \ + ./src/video/pandora/SDL_pandora.o \ + ./src/video/pandora/SDL_pandora_events.o \ + ./src/video/x11/*.c \ OBJECTS = $(shell echo $(SOURCES) | sed -e 's,\.c,\.o,g') diff --git a/Engine/lib/sdl/Makefile.psp b/Engine/lib/sdl/Makefile.psp index 93fb9e447..de0e50e2f 100644 --- a/Engine/lib/sdl/Makefile.psp +++ b/Engine/lib/sdl/Makefile.psp @@ -42,6 +42,8 @@ OBJS= src/SDL.o \ src/render/software/SDL_drawpoint.o \ src/render/software/SDL_render_sw.o \ src/render/software/SDL_rotate.o \ + src/sensor/SDL_sensor.o \ + src/sensor/dummy/SDL_dummysensor.o \ src/stdlib/SDL_getenv.o \ src/stdlib/SDL_iconv.o \ src/stdlib/SDL_malloc.o \ diff --git a/Engine/lib/sdl/Makefile.wiz b/Engine/lib/sdl/Makefile.wiz index 0981be853..8ed58ee76 100644 --- a/Engine/lib/sdl/Makefile.wiz +++ b/Engine/lib/sdl/Makefile.wiz @@ -12,14 +12,33 @@ CFLAGS = -Wall -fPIC -I./include -I$(WIZSDK)/include -DWIZ_GLES_LITE TARGET_STATIC = libSDL2.a TARGET_SHARED = libSDL2.so -SOURCES = ./src/*.c ./src/audio/*.c ./src/cpuinfo/*.c ./src/events/*.c \ - ./src/file/*.c ./src/stdlib/*.c ./src/thread/*.c ./src/timer/*.c ./src/video/*.c \ - ./src/joystick/*.c ./src/haptic/*.c ./src/video/dummy/*.c ./src/audio/disk/*.c \ - ./src/audio/dummy/*.c ./src/loadso/dlopen/*.c ./src/audio/dsp/*.c \ - ./src/thread/pthread/SDL_systhread.c ./src/thread/pthread/SDL_syssem.c \ - ./src/thread/pthread/SDL_sysmutex.c ./src/thread/pthread/SDL_syscond.c \ - ./src/joystick/linux/*.c ./src/haptic/linux/*.c ./src/timer/unix/*.c \ - ./src/video/pandora/SDL_pandora.o ./src/video/pandora/SDL_pandora_events.o +SOURCES = \ + ./src/*.c \ + ./src/audio/*.c \ + ./src/audio/disk/*.c \ + ./src/audio/dsp/*.c \ + ./src/audio/dummy/*.c \ + ./src/cpuinfo/*.c \ + ./src/events/*.c \ + ./src/file/*.c \ + ./src/haptic/*.c \ + ./src/haptic/linux/*.c \ + ./src/joystick/*.c \ + ./src/joystick/linux/*.c \ + ./src/loadso/dlopen/*.c \ + ./src/sensor/*.c \ + ./src/sensor/dummy/*.c \ + ./src/stdlib/*.c \ + ./src/thread/*.c \ + ./src/thread/pthread/SDL_syscond.c \ + ./src/thread/pthread/SDL_sysmutex.c \ + ./src/thread/pthread/SDL_syssem.c \ + ./src/thread/pthread/SDL_systhread.c \ + ./src/timer/*.c \ + ./src/timer/unix/*.c \ + ./src/video/*.c \ + ./src/video/dummy/*.c \ + ./src/video/pandora/*.c \ OBJECTS = $(shell echo $(SOURCES) | sed -e 's,\.c,\.o,g') diff --git a/Engine/lib/sdl/SDL2.spec b/Engine/lib/sdl/SDL2.spec index 26454b74a..67e819690 100644 --- a/Engine/lib/sdl/SDL2.spec +++ b/Engine/lib/sdl/SDL2.spec @@ -1,6 +1,6 @@ Summary: Simple DirectMedia Layer Name: SDL2 -Version: 2.0.8 +Version: 2.0.9 Release: 2 Source: http://www.libsdl.org/release/%{name}-%{version}.tar.gz URL: http://www.libsdl.org/ diff --git a/Engine/lib/sdl/WhatsNew.txt b/Engine/lib/sdl/WhatsNew.txt index 48fe2510a..9074b12ba 100644 --- a/Engine/lib/sdl/WhatsNew.txt +++ b/Engine/lib/sdl/WhatsNew.txt @@ -1,6 +1,47 @@ This is a list of major changes in SDL's version history. +--------------------------------------------------------------------------- +2.0.9: +--------------------------------------------------------------------------- + +General: +* Added a new sensor API, initialized by passing SDL_INIT_SENSOR to SDL_Init(), and defined in SDL_sensor.h +* Added an event SDL_SENSORUPDATE which is sent when a sensor is updated +* Added SDL_GetDisplayOrientation() to return the current display orientation +* Added an event SDL_DISPLAYEVENT which is sent when the display orientation changes +* Added HIDAPI joystick drivers for more consistent support for Xbox, PS4 and Nintendo Switch Pro controller support across platforms. (Thanks to Valve for contributing the PS4 and Nintendo Switch Pro controller support) +* Added support for many other popular game controllers +* Added SDL_JoystickGetDevicePlayerIndex(), SDL_JoystickGetPlayerIndex(), and SDL_GameControllerGetPlayerIndex() to get the player index for a controller. For XInput controllers this returns the XInput index for the controller. +* Added SDL_GameControllerRumble() and SDL_JoystickRumble() which allow simple rumble without using the haptics API +* Added SDL_GameControllerMappingForDeviceIndex() to get the mapping for a controller before it's opened +* Added the hint SDL_HINT_MOUSE_DOUBLE_CLICK_TIME to control the mouse double-click time +* Added the hint SDL_HINT_MOUSE_DOUBLE_CLICK_RADIUS to control the mouse double-click radius, in pixels +* Added SDL_HasColorKey() to return whether a surface has a colorkey active +* Added SDL_HasAVX512F() to return whether the CPU has AVX-512F features +* Added SDL_IsTablet() to return whether the application is running on a tablet +* Added SDL_THREAD_PRIORITY_TIME_CRITICAL for threads that must run at the highest priority + +Mac OS X: +* Fixed black screen at start on Mac OS X Mojave + +Linux: +* Added SDL_LinuxSetThreadPriority() to allow adjusting the thread priority of native threads using RealtimeKit if available. + +iOS: +* Fixed Asian IME input + +Android: +* Updated required Android SDK to API 26, to match Google's new App Store requirements +* Added support for wired USB Xbox, PS4, and Nintendo Switch Pro controllers +* Added support for relative mouse mode on Android 7.0 and newer (except where it's broken, on Chromebooks and when in DeX mode with Samsung Experience 9.0) +* Added support for custom mouse cursors on Android 7.0 and newer +* Added the hint SDL_HINT_ANDROID_TRAP_BACK_BUTTON to control whether the back button will back out of the app (the default) or be passed to the application as SDL_SCANCODE_AC_BACK +* Added SDL_AndroidBackButton() to trigger the Android system back button behavior when handling the back button in the application +* Added SDL_IsChromebook() to return whether the app is running in the Chromebook Android runtime +* Added SDL_IsDeXMode() to return whether the app is running while docked in the Samsung DeX + + --------------------------------------------------------------------------- 2.0.8: --------------------------------------------------------------------------- diff --git a/Engine/lib/sdl/acinclude/ax_gcc_x86_cpuid.m4 b/Engine/lib/sdl/acinclude/ax_gcc_x86_cpuid.m4 new file mode 100644 index 000000000..7d46fee02 --- /dev/null +++ b/Engine/lib/sdl/acinclude/ax_gcc_x86_cpuid.m4 @@ -0,0 +1,79 @@ +# =========================================================================== +# http://www.gnu.org/software/autoconf-archive/ax_gcc_x86_cpuid.html +# =========================================================================== +# +# SYNOPSIS +# +# AX_GCC_X86_CPUID(OP) +# +# DESCRIPTION +# +# On Pentium and later x86 processors, with gcc or a compiler that has a +# compatible syntax for inline assembly instructions, run a small program +# that executes the cpuid instruction with input OP. This can be used to +# detect the CPU type. +# +# On output, the values of the eax, ebx, ecx, and edx registers are stored +# as hexadecimal strings as "eax:ebx:ecx:edx" in the cache variable +# ax_cv_gcc_x86_cpuid_OP. +# +# If the cpuid instruction fails (because you are running a +# cross-compiler, or because you are not using gcc, or because you are on +# a processor that doesn't have this instruction), ax_cv_gcc_x86_cpuid_OP +# is set to the string "unknown". +# +# This macro mainly exists to be used in AX_GCC_ARCHFLAG. +# +# LICENSE +# +# Copyright (c) 2008 Steven G. Johnson +# Copyright (c) 2008 Matteo Frigo +# +# This program is free software: you can redistribute it and/or modify it +# under the terms of the GNU General Public License as published by the +# Free Software Foundation, either version 3 of the License, or (at your +# option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +# Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program. If not, see . +# +# As a special exception, the respective Autoconf Macro's copyright owner +# gives unlimited permission to copy, distribute and modify the configure +# scripts that are the output of Autoconf when processing the Macro. You +# need not follow the terms of the GNU General Public License when using +# or distributing such scripts, even though portions of the text of the +# Macro appear in them. The GNU General Public License (GPL) does govern +# all other use of the material that constitutes the Autoconf Macro. +# +# This special exception to the GPL applies to versions of the Autoconf +# Macro released by the Autoconf Archive. When you make and distribute a +# modified version of the Autoconf Macro, you may extend this special +# exception to the GPL to apply to your modified version as well. + +#serial 7 + +AC_DEFUN([AX_GCC_X86_CPUID], +[AC_REQUIRE([AC_PROG_CC]) +AC_LANG_PUSH([C]) +AC_CACHE_CHECK(for x86 cpuid $1 output, ax_cv_gcc_x86_cpuid_$1, + [AC_RUN_IFELSE([AC_LANG_PROGRAM([#include ], [ + int op = $1, eax, ebx, ecx, edx; + FILE *f; + __asm__("cpuid" + : "=a" (eax), "=b" (ebx), "=c" (ecx), "=d" (edx) + : "a" (op)); + f = fopen("conftest_cpuid", "w"); if (!f) return 1; + fprintf(f, "%x:%x:%x:%x\n", eax, ebx, ecx, edx); + fclose(f); + return 0; +])], + [ax_cv_gcc_x86_cpuid_$1=`cat conftest_cpuid`; rm -f conftest_cpuid], + [ax_cv_gcc_x86_cpuid_$1=unknown; rm -f conftest_cpuid], + [ax_cv_gcc_x86_cpuid_$1=unknown])]) +AC_LANG_POP([C]) +]) diff --git a/Engine/lib/sdl/build-scripts/install-sh b/Engine/lib/sdl/build-scripts/install-sh index 1a8353401..377bb8687 100755 --- a/Engine/lib/sdl/build-scripts/install-sh +++ b/Engine/lib/sdl/build-scripts/install-sh @@ -1,7 +1,7 @@ #!/bin/sh # install - install a program, script, or datafile -scriptversion=2005-02-02.21 +scriptversion=2011-11-20.07; # UTC # This originates from X11R5 (mit/util/scripts/install.sh), which was # later released in X11R6 (xc/config/util/install.sh) with the @@ -35,42 +35,72 @@ scriptversion=2005-02-02.21 # FSF changes to this file are in the public domain. # # Calling this script install-sh is preferred over install.sh, to prevent -# `make' implicit rules from creating a file called install from it +# 'make' implicit rules from creating a file called install from it # when there is no Makefile. # # This script is compatible with the BSD install script, but was written -# from scratch. It can only install one file at a time, a restriction -# shared with many OS's install programs. +# from scratch. + +nl=' +' +IFS=" "" $nl" # set DOITPROG to echo to test this script # Don't use :- since 4.3BSD and earlier shells don't like it. -doit="${DOITPROG-}" +doit=${DOITPROG-} +if test -z "$doit"; then + doit_exec=exec +else + doit_exec=$doit +fi -# put in absolute paths if you don't have them in your path; or use env. vars. +# Put in absolute file names if you don't have them in your path; +# or use environment vars. -mvprog="${MVPROG-mv}" -cpprog="${CPPROG-cp}" -chmodprog="${CHMODPROG-chmod}" -chownprog="${CHOWNPROG-chown}" -chgrpprog="${CHGRPPROG-chgrp}" -stripprog="${STRIPPROG-strip}" -rmprog="${RMPROG-rm}" -mkdirprog="${MKDIRPROG-mkdir}" +chgrpprog=${CHGRPPROG-chgrp} +chmodprog=${CHMODPROG-chmod} +chownprog=${CHOWNPROG-chown} +cmpprog=${CMPPROG-cmp} +cpprog=${CPPROG-cp} +mkdirprog=${MKDIRPROG-mkdir} +mvprog=${MVPROG-mv} +rmprog=${RMPROG-rm} +stripprog=${STRIPPROG-strip} + +posix_glob='?' +initialize_posix_glob=' + test "$posix_glob" != "?" || { + if (set -f) 2>/dev/null; then + posix_glob= + else + posix_glob=: + fi + } +' + +posix_mkdir= + +# Desired mode of installed file. +mode=0755 -chmodcmd="$chmodprog 0755" -chowncmd= chgrpcmd= -stripcmd= +chmodcmd=$chmodprog +chowncmd= +mvcmd=$mvprog rmcmd="$rmprog -f" -mvcmd="$mvprog" +stripcmd= + src= dst= dir_arg= -dstarg= +dst_arg= + +copy_on_change=false no_target_directory= -usage="Usage: $0 [OPTION]... [-T] SRCFILE DSTFILE +usage="\ +Usage: $0 [OPTION]... [-T] SRCFILE DSTFILE or: $0 [OPTION]... SRCFILES... DIRECTORY or: $0 [OPTION]... -t DIRECTORY SRCFILES... or: $0 [OPTION]... -d DIRECTORIES... @@ -80,108 +110,148 @@ In the 2nd and 3rd, copy all SRCFILES to DIRECTORY. In the 4th, create DIRECTORIES. Options: --c (ignored) --d create directories instead of installing files. --g GROUP $chgrpprog installed files to GROUP. --m MODE $chmodprog installed files to MODE. --o USER $chownprog installed files to USER. --s $stripprog installed files. --t DIRECTORY install into DIRECTORY. --T report an error if DSTFILE is a directory. ---help display this help and exit. ---version display version info and exit. + --help display this help and exit. + --version display version info and exit. + + -c (ignored) + -C install only if different (preserve the last data modification time) + -d create directories instead of installing files. + -g GROUP $chgrpprog installed files to GROUP. + -m MODE $chmodprog installed files to MODE. + -o USER $chownprog installed files to USER. + -s $stripprog installed files. + -t DIRECTORY install into DIRECTORY. + -T report an error if DSTFILE is a directory. Environment variables override the default commands: - CHGRPPROG CHMODPROG CHOWNPROG CPPROG MKDIRPROG MVPROG RMPROG STRIPPROG + CHGRPPROG CHMODPROG CHOWNPROG CMPPROG CPPROG MKDIRPROG MVPROG + RMPROG STRIPPROG " -while test -n "$1"; do +while test $# -ne 0; do case $1 in - -c) shift - continue;; + -c) ;; - -d) dir_arg=true - shift - continue;; + -C) copy_on_change=true;; + + -d) dir_arg=true;; -g) chgrpcmd="$chgrpprog $2" - shift - shift - continue;; + shift;; --help) echo "$usage"; exit $?;; - -m) chmodcmd="$chmodprog $2" - shift - shift - continue;; + -m) mode=$2 + case $mode in + *' '* | *' '* | *' +'* | *'*'* | *'?'* | *'['*) + echo "$0: invalid mode: $mode" >&2 + exit 1;; + esac + shift;; -o) chowncmd="$chownprog $2" - shift - shift - continue;; + shift;; - -s) stripcmd=$stripprog - shift - continue;; + -s) stripcmd=$stripprog;; - -t) dstarg=$2 - shift - shift - continue;; + -t) dst_arg=$2 + # Protect names problematic for 'test' and other utilities. + case $dst_arg in + -* | [=\(\)!]) dst_arg=./$dst_arg;; + esac + shift;; - -T) no_target_directory=true - shift - continue;; + -T) no_target_directory=true;; --version) echo "$0 $scriptversion"; exit $?;; - *) # When -d is used, all remaining arguments are directories to create. - # When -t is used, the destination is already specified. - test -n "$dir_arg$dstarg" && break - # Otherwise, the last argument is the destination. Remove it from $@. - for arg - do - if test -n "$dstarg"; then - # $@ is not empty: it contains at least $arg. - set fnord "$@" "$dstarg" - shift # fnord - fi - shift # arg - dstarg=$arg - done + --) shift break;; + + -*) echo "$0: invalid option: $1" >&2 + exit 1;; + + *) break;; esac + shift done -if test -z "$1"; then +if test $# -ne 0 && test -z "$dir_arg$dst_arg"; then + # When -d is used, all remaining arguments are directories to create. + # When -t is used, the destination is already specified. + # Otherwise, the last argument is the destination. Remove it from $@. + for arg + do + if test -n "$dst_arg"; then + # $@ is not empty: it contains at least $arg. + set fnord "$@" "$dst_arg" + shift # fnord + fi + shift # arg + dst_arg=$arg + # Protect names problematic for 'test' and other utilities. + case $dst_arg in + -* | [=\(\)!]) dst_arg=./$dst_arg;; + esac + done +fi + +if test $# -eq 0; then if test -z "$dir_arg"; then echo "$0: no input file specified." >&2 exit 1 fi - # It's OK to call `install-sh -d' without argument. + # It's OK to call 'install-sh -d' without argument. # This can happen when creating conditional directories. exit 0 fi +if test -z "$dir_arg"; then + do_exit='(exit $ret); exit $ret' + trap "ret=129; $do_exit" 1 + trap "ret=130; $do_exit" 2 + trap "ret=141; $do_exit" 13 + trap "ret=143; $do_exit" 15 + + # Set umask so as not to create temps with too-generous modes. + # However, 'strip' requires both read and write access to temps. + case $mode in + # Optimize common cases. + *644) cp_umask=133;; + *755) cp_umask=22;; + + *[0-7]) + if test -z "$stripcmd"; then + u_plus_rw= + else + u_plus_rw='% 200' + fi + cp_umask=`expr '(' 777 - $mode % 1000 ')' $u_plus_rw`;; + *) + if test -z "$stripcmd"; then + u_plus_rw= + else + u_plus_rw=,u+rw + fi + cp_umask=$mode$u_plus_rw;; + esac +fi + for src do - # Protect names starting with `-'. + # Protect names problematic for 'test' and other utilities. case $src in - -*) src=./$src ;; + -* | [=\(\)!]) src=./$src;; esac if test -n "$dir_arg"; then dst=$src - src= - - if test -d "$dst"; then - mkdircmd=: - chmodcmd= - else - mkdircmd=$mkdirprog - fi + dstdir=$dst + test -d "$dstdir" + dstdir_status=$? else + # Waiting for this to be detected by the "$cpprog $src $dsttmp" command # might cause directories to be created, which would be especially bad # if $src (and thus $dsttmp) contains '*'. @@ -190,71 +260,194 @@ do exit 1 fi - if test -z "$dstarg"; then + if test -z "$dst_arg"; then echo "$0: no destination specified." >&2 exit 1 fi - - dst=$dstarg - # Protect names starting with `-'. - case $dst in - -*) dst=./$dst ;; - esac + dst=$dst_arg # If destination is a directory, append the input filename; won't work # if double slashes aren't ignored. if test -d "$dst"; then if test -n "$no_target_directory"; then - echo "$0: $dstarg: Is a directory" >&2 + echo "$0: $dst_arg: Is a directory" >&2 exit 1 fi - dst=$dst/`basename "$src"` + dstdir=$dst + dst=$dstdir/`basename "$src"` + dstdir_status=0 + else + # Prefer dirname, but fall back on a substitute if dirname fails. + dstdir=` + (dirname "$dst") 2>/dev/null || + expr X"$dst" : 'X\(.*[^/]\)//*[^/][^/]*/*$' \| \ + X"$dst" : 'X\(//\)[^/]' \| \ + X"$dst" : 'X\(//\)$' \| \ + X"$dst" : 'X\(/\)' \| . 2>/dev/null || + echo X"$dst" | + sed '/^X\(.*[^/]\)\/\/*[^/][^/]*\/*$/{ + s//\1/ + q + } + /^X\(\/\/\)[^/].*/{ + s//\1/ + q + } + /^X\(\/\/\)$/{ + s//\1/ + q + } + /^X\(\/\).*/{ + s//\1/ + q + } + s/.*/./; q' + ` + + test -d "$dstdir" + dstdir_status=$? fi fi - # This sed command emulates the dirname command. - dstdir=`echo "$dst" | sed -e 's,/*$,,;s,[^/]*$,,;s,/*$,,;s,^$,.,'` + obsolete_mkdir_used=false - # Make sure that the destination directory exists. + if test $dstdir_status != 0; then + case $posix_mkdir in + '') + # Create intermediate dirs using mode 755 as modified by the umask. + # This is like FreeBSD 'install' as of 1997-10-28. + umask=`umask` + case $stripcmd.$umask in + # Optimize common cases. + *[2367][2367]) mkdir_umask=$umask;; + .*0[02][02] | .[02][02] | .[02]) mkdir_umask=22;; - # Skip lots of stat calls in the usual case. - if test ! -d "$dstdir"; then - defaultIFS=' - ' - IFS="${IFS-$defaultIFS}" + *[0-7]) + mkdir_umask=`expr $umask + 22 \ + - $umask % 100 % 40 + $umask % 20 \ + - $umask % 10 % 4 + $umask % 2 + `;; + *) mkdir_umask=$umask,go-w;; + esac - oIFS=$IFS - # Some sh's can't handle IFS=/ for some reason. - IFS='%' - set x `echo "$dstdir" | sed -e 's@/@%@g' -e 's@^%@/@'` - shift - IFS=$oIFS + # With -d, create the new directory with the user-specified mode. + # Otherwise, rely on $mkdir_umask. + if test -n "$dir_arg"; then + mkdir_mode=-m$mode + else + mkdir_mode= + fi - pathcomp= + posix_mkdir=false + case $umask in + *[123567][0-7][0-7]) + # POSIX mkdir -p sets u+wx bits regardless of umask, which + # is incompatible with FreeBSD 'install' when (umask & 300) != 0. + ;; + *) + tmpdir=${TMPDIR-/tmp}/ins$RANDOM-$$ + trap 'ret=$?; rmdir "$tmpdir/d" "$tmpdir" 2>/dev/null; exit $ret' 0 - while test $# -ne 0 ; do - pathcomp=$pathcomp$1 + if (umask $mkdir_umask && + exec $mkdirprog $mkdir_mode -p -- "$tmpdir/d") >/dev/null 2>&1 + then + if test -z "$dir_arg" || { + # Check for POSIX incompatibilities with -m. + # HP-UX 11.23 and IRIX 6.5 mkdir -m -p sets group- or + # other-writable bit of parent directory when it shouldn't. + # FreeBSD 6.1 mkdir -m -p sets mode of existing directory. + ls_ld_tmpdir=`ls -ld "$tmpdir"` + case $ls_ld_tmpdir in + d????-?r-*) different_mode=700;; + d????-?--*) different_mode=755;; + *) false;; + esac && + $mkdirprog -m$different_mode -p -- "$tmpdir" && { + ls_ld_tmpdir_1=`ls -ld "$tmpdir"` + test "$ls_ld_tmpdir" = "$ls_ld_tmpdir_1" + } + } + then posix_mkdir=: + fi + rmdir "$tmpdir/d" "$tmpdir" + else + # Remove any dirs left behind by ancient mkdir implementations. + rmdir ./$mkdir_mode ./-p ./-- 2>/dev/null + fi + trap '' 0;; + esac;; + esac + + if + $posix_mkdir && ( + umask $mkdir_umask && + $doit_exec $mkdirprog $mkdir_mode -p -- "$dstdir" + ) + then : + else + + # The umask is ridiculous, or mkdir does not conform to POSIX, + # or it failed possibly due to a race condition. Create the + # directory the slow way, step by step, checking for races as we go. + + case $dstdir in + /*) prefix='/';; + [-=\(\)!]*) prefix='./';; + *) prefix='';; + esac + + eval "$initialize_posix_glob" + + oIFS=$IFS + IFS=/ + $posix_glob set -f + set fnord $dstdir shift - if test ! -d "$pathcomp"; then - $mkdirprog "$pathcomp" - # mkdir can fail with a `File exist' error in case several - # install-sh are creating the directory concurrently. This - # is OK. - test -d "$pathcomp" || exit + $posix_glob set +f + IFS=$oIFS + + prefixes= + + for d + do + test X"$d" = X && continue + + prefix=$prefix$d + if test -d "$prefix"; then + prefixes= + else + if $posix_mkdir; then + (umask=$mkdir_umask && + $doit_exec $mkdirprog $mkdir_mode -p -- "$dstdir") && break + # Don't fail if two instances are running concurrently. + test -d "$prefix" || exit 1 + else + case $prefix in + *\'*) qprefix=`echo "$prefix" | sed "s/'/'\\\\\\\\''/g"`;; + *) qprefix=$prefix;; + esac + prefixes="$prefixes '$qprefix'" + fi + fi + prefix=$prefix/ + done + + if test -n "$prefixes"; then + # Don't fail if two instances are running concurrently. + (umask $mkdir_umask && + eval "\$doit_exec \$mkdirprog $prefixes") || + test -d "$dstdir" || exit 1 + obsolete_mkdir_used=true fi - pathcomp=$pathcomp/ - done + fi fi if test -n "$dir_arg"; then - $doit $mkdircmd "$dst" \ - && { test -z "$chowncmd" || $doit $chowncmd "$dst"; } \ - && { test -z "$chgrpcmd" || $doit $chgrpcmd "$dst"; } \ - && { test -z "$stripcmd" || $doit $stripcmd "$dst"; } \ - && { test -z "$chmodcmd" || $doit $chmodcmd "$dst"; } - + { test -z "$chowncmd" || $doit $chowncmd "$dst"; } && + { test -z "$chgrpcmd" || $doit $chgrpcmd "$dst"; } && + { test "$obsolete_mkdir_used$chowncmd$chgrpcmd" = false || + test -z "$chmodcmd" || $doit $chmodcmd $mode "$dst"; } || exit 1 else - dstfile=`basename "$dst"` # Make a couple of temp file names in the proper directory. dsttmp=$dstdir/_inst.$$_ @@ -262,10 +455,9 @@ do # Trap to clean up those temp files at exit. trap 'ret=$?; rm -f "$dsttmp" "$rmtmp" && exit $ret' 0 - trap '(exit $?); exit' 1 2 13 15 # Copy the file name to the temp name. - $doit $cpprog "$src" "$dsttmp" && + (umask $cp_umask && $doit_exec $cpprog "$src" "$dsttmp") && # and set any options; do chmod last to preserve setuid bits. # @@ -273,51 +465,63 @@ do # ignore errors from any of these, just make sure not to ignore # errors from the above "$doit $cpprog $src $dsttmp" command. # - { test -z "$chowncmd" || $doit $chowncmd "$dsttmp"; } \ - && { test -z "$chgrpcmd" || $doit $chgrpcmd "$dsttmp"; } \ - && { test -z "$stripcmd" || $doit $stripcmd "$dsttmp"; } \ - && { test -z "$chmodcmd" || $doit $chmodcmd "$dsttmp"; } && + { test -z "$chowncmd" || $doit $chowncmd "$dsttmp"; } && + { test -z "$chgrpcmd" || $doit $chgrpcmd "$dsttmp"; } && + { test -z "$stripcmd" || $doit $stripcmd "$dsttmp"; } && + { test -z "$chmodcmd" || $doit $chmodcmd $mode "$dsttmp"; } && - # Now rename the file to the real destination. - { $doit $mvcmd -f "$dsttmp" "$dstdir/$dstfile" 2>/dev/null \ - || { - # The rename failed, perhaps because mv can't rename something else - # to itself, or perhaps because mv is so ancient that it does not - # support -f. + # If -C, don't bother to copy if it wouldn't change the file. + if $copy_on_change && + old=`LC_ALL=C ls -dlL "$dst" 2>/dev/null` && + new=`LC_ALL=C ls -dlL "$dsttmp" 2>/dev/null` && - # Now remove or move aside any old file at destination location. - # We try this two ways since rm can't unlink itself on some - # systems and the destination file might be busy for other - # reasons. In this case, the final cleanup might fail but the new - # file should still install successfully. - { - if test -f "$dstdir/$dstfile"; then - $doit $rmcmd -f "$dstdir/$dstfile" 2>/dev/null \ - || $doit $mvcmd -f "$dstdir/$dstfile" "$rmtmp" 2>/dev/null \ - || { - echo "$0: cannot unlink or rename $dstdir/$dstfile" >&2 - (exit 1); exit 1 - } - else - : - fi - } && + eval "$initialize_posix_glob" && + $posix_glob set -f && + set X $old && old=:$2:$4:$5:$6 && + set X $new && new=:$2:$4:$5:$6 && + $posix_glob set +f && - # Now rename the file to the real destination. - $doit $mvcmd "$dsttmp" "$dstdir/$dstfile" - } - } - fi || { (exit 1); exit 1; } + test "$old" = "$new" && + $cmpprog "$dst" "$dsttmp" >/dev/null 2>&1 + then + rm -f "$dsttmp" + else + # Rename the file to the real destination. + $doit $mvcmd -f "$dsttmp" "$dst" 2>/dev/null || + + # The rename failed, perhaps because mv can't rename something else + # to itself, or perhaps because mv is so ancient that it does not + # support -f. + { + # Now remove or move aside any old file at destination location. + # We try this two ways since rm can't unlink itself on some + # systems and the destination file might be busy for other + # reasons. In this case, the final cleanup might fail but the new + # file should still install successfully. + { + test ! -f "$dst" || + $doit $rmcmd -f "$dst" 2>/dev/null || + { $doit $mvcmd -f "$dst" "$rmtmp" 2>/dev/null && + { $doit $rmcmd -f "$rmtmp" 2>/dev/null; :; } + } || + { echo "$0: cannot unlink or rename $dst" >&2 + (exit 1); exit 1 + } + } && + + # Now rename the file to the real destination. + $doit $mvcmd "$dsttmp" "$dst" + } + fi || exit 1 + + trap '' 0 + fi done -# The final little trick to "correctly" pass the exit status to the exit trap. -{ - (exit 0); exit 0 -} - # Local variables: # eval: (add-hook 'write-file-hooks 'time-stamp) # time-stamp-start: "scriptversion=" # time-stamp-format: "%:y-%02m-%02d.%02H" -# time-stamp-end: "$" +# time-stamp-time-zone: "UTC" +# time-stamp-end: "; # UTC" # End: diff --git a/Engine/lib/sdl/build-scripts/ltmain.sh b/Engine/lib/sdl/build-scripts/ltmain.sh index 6635343b2..3cbc4a776 100755 --- a/Engine/lib/sdl/build-scripts/ltmain.sh +++ b/Engine/lib/sdl/build-scripts/ltmain.sh @@ -7404,8 +7404,11 @@ func_mode_link () # Darwin ld doesn't like 0 for these options... func_arith $current + 1 minor_current=$func_arith_result - xlcverstring="${wl}-compatibility_version ${wl}$minor_current ${wl}-current_version ${wl}$minor_current.$revision" - verstring="-compatibility_version $minor_current -current_version $minor_current.$revision" + #xlcverstring="${wl}-compatibility_version ${wl}$minor_current ${wl}-current_version ${wl}$minor_current.$revision" + #verstring="-compatibility_version $minor_current -current_version $minor_current.$revision" + # make the compatibility version match the Xcode project files, i.e. 1.0 + xlcverstring="${wl}-compatibility_version 1.0 ${wl}-current_version ${wl}$minor_current.$revision" + verstring="-compatibility_version 1.0 -current_version $minor_current.$revision" ;; freebsd-aout) diff --git a/Engine/lib/sdl/build-scripts/mkinstalldirs b/Engine/lib/sdl/build-scripts/mkinstalldirs index 8ab885ec9..55d537f87 100755 --- a/Engine/lib/sdl/build-scripts/mkinstalldirs +++ b/Engine/lib/sdl/build-scripts/mkinstalldirs @@ -1,29 +1,59 @@ #! /bin/sh # mkinstalldirs --- make directory hierarchy -# Author: Noah Friedman -# Created: 1993-05-16 -# Public domain +scriptversion=2009-04-28.21; # UTC + +# Original author: Noah Friedman +# Created: 1993-05-16 +# Public domain. +# +# This file is maintained in Automake, please report +# bugs to or send patches to +# . + +nl=' +' +IFS=" "" $nl" errstatus=0 -dirmode="" +dirmode= usage="\ -Usage: mkinstalldirs [-h] [--help] [-m mode] dir ..." +Usage: mkinstalldirs [-h] [--help] [--version] [-m MODE] DIR ... + +Create each directory DIR (with mode MODE, if specified), including all +leading file name components. + +Report bugs to ." # process command line arguments while test $# -gt 0 ; do - case "${1}" in - -h | --help | --h* ) # -h for help - echo "${usage}" 1>&2; exit 0 ;; - -m ) # -m PERM arg - shift - test $# -eq 0 && { echo "${usage}" 1>&2; exit 1; } - dirmode="${1}" - shift ;; - -- ) shift; break ;; # stop option processing - -* ) echo "${usage}" 1>&2; exit 1 ;; # unknown option - * ) break ;; # first non-opt arg - esac + case $1 in + -h | --help | --h*) # -h for help + echo "$usage" + exit $? + ;; + -m) # -m PERM arg + shift + test $# -eq 0 && { echo "$usage" 1>&2; exit 1; } + dirmode=$1 + shift + ;; + --version) + echo "$0 $scriptversion" + exit $? + ;; + --) # stop option processing + shift + break + ;; + -*) # unknown option + echo "$usage" 1>&2 + exit 1 + ;; + *) # first non-opt arg + break + ;; + esac done for file @@ -36,64 +66,97 @@ do done case $# in -0) exit 0 ;; + 0) exit 0 ;; esac +# Solaris 8's mkdir -p isn't thread-safe. If you mkdir -p a/b and +# mkdir -p a/c at the same time, both will detect that a is missing, +# one will create a, then the other will try to create a and die with +# a "File exists" error. This is a problem when calling mkinstalldirs +# from a parallel make. We use --version in the probe to restrict +# ourselves to GNU mkdir, which is thread-safe. case $dirmode in -'') - if mkdir -p -- . 2>/dev/null; then - echo "mkdir -p -- $*" - exec mkdir -p -- "$@" - fi ;; -*) - if mkdir -m "$dirmode" -p -- . 2>/dev/null; then - echo "mkdir -m $dirmode -p -- $*" - exec mkdir -m "$dirmode" -p -- "$@" - fi ;; + '') + if mkdir -p --version . >/dev/null 2>&1 && test ! -d ./--version; then + echo "mkdir -p -- $*" + exec mkdir -p -- "$@" + else + # On NextStep and OpenStep, the 'mkdir' command does not + # recognize any option. It will interpret all options as + # directories to create, and then abort because '.' already + # exists. + test -d ./-p && rmdir ./-p + test -d ./--version && rmdir ./--version + fi + ;; + *) + if mkdir -m "$dirmode" -p --version . >/dev/null 2>&1 && + test ! -d ./--version; then + echo "mkdir -m $dirmode -p -- $*" + exec mkdir -m "$dirmode" -p -- "$@" + else + # Clean up after NextStep and OpenStep mkdir. + for d in ./-m ./-p ./--version "./$dirmode"; + do + test -d $d && rmdir $d + done + fi + ;; esac for file do - set fnord `echo ":$file" | sed -ne 's/^:\//#/;s/^://;s/\// /g;s/^#/\//;p'` - shift + case $file in + /*) pathcomp=/ ;; + *) pathcomp= ;; + esac + oIFS=$IFS + IFS=/ + set fnord $file + shift + IFS=$oIFS - pathcomp= - for d - do - pathcomp="$pathcomp$d" - case "$pathcomp" in - -* ) pathcomp=./$pathcomp ;; - esac + for d + do + test "x$d" = x && continue - if test ! -d "$pathcomp"; then - echo "mkdir $pathcomp" + pathcomp=$pathcomp$d + case $pathcomp in + -*) pathcomp=./$pathcomp ;; + esac - mkdir "$pathcomp" || lasterr=$? + if test ! -d "$pathcomp"; then + echo "mkdir $pathcomp" - if test ! -d "$pathcomp"; then - errstatus=$lasterr - else - if test ! -z "$dirmode"; then - echo "chmod $dirmode $pathcomp" + mkdir "$pathcomp" || lasterr=$? - lasterr="" - chmod "$dirmode" "$pathcomp" || lasterr=$? + if test ! -d "$pathcomp"; then + errstatus=$lasterr + else + if test ! -z "$dirmode"; then + echo "chmod $dirmode $pathcomp" + lasterr= + chmod "$dirmode" "$pathcomp" || lasterr=$? - if test ! -z "$lasterr"; then - errstatus=$lasterr - fi + if test ! -z "$lasterr"; then + errstatus=$lasterr fi fi - fi + fi + fi - pathcomp="$pathcomp/" - done + pathcomp=$pathcomp/ + done done exit $errstatus # Local Variables: # mode: shell-script -# sh-indentation: 3 +# sh-indentation: 2 +# eval: (add-hook 'write-file-hooks 'time-stamp) +# time-stamp-start: "scriptversion=" +# time-stamp-format: "%:y-%02m-%02d.%02H" +# time-stamp-time-zone: "UTC" +# time-stamp-end: "; # UTC" # End: -# mkinstalldirs ends here diff --git a/Engine/lib/sdl/build-scripts/winrtbuild.ps1 b/Engine/lib/sdl/build-scripts/winrtbuild.ps1 index 7ce050e48..1d16229b1 100644 --- a/Engine/lib/sdl/build-scripts/winrtbuild.ps1 +++ b/Engine/lib/sdl/build-scripts/winrtbuild.ps1 @@ -39,7 +39,7 @@ # # Base version of SDL, used for packaging purposes -$SDLVersion = "2.0.7" +$SDLVersion = "2.0.9" # Gets the .bat file that sets up an MSBuild environment, given one of # Visual Studio's, "PlatformToolset"s. diff --git a/Engine/lib/sdl/cmake/sdlchecks.cmake b/Engine/lib/sdl/cmake/sdlchecks.cmake index b822c7a56..4a2c3ed57 100644 --- a/Engine/lib/sdl/cmake/sdlchecks.cmake +++ b/Engine/lib/sdl/cmake/sdlchecks.cmake @@ -381,9 +381,21 @@ macro(CheckX11) FindLibraryAndSONAME("${_LIB}") endforeach() - find_path(X_INCLUDEDIR X11/Xlib.h) + find_path(X_INCLUDEDIR X11/Xlib.h + /usr/pkg/xorg/include + /usr/X11R6/include + /usr/X11R7/include + /usr/local/include/X11 + /usr/include/X11 + /usr/openwin/include + /usr/openwin/share/include + /opt/graphics/OpenGL/include + /opt/X11/include + ) + if(X_INCLUDEDIR) - set(X_CFLAGS "-I${X_INCLUDEDIR}") + list(APPEND EXTRA_CFLAGS "-I${X_INCLUDEDIR}") + list(APPEND CMAKE_REQUIRED_INCLUDES "${X_INCLUDEDIR}") endif() check_include_file(X11/Xcursor/Xcursor.h HAVE_XCURSOR_H) @@ -420,7 +432,7 @@ macro(CheckX11) endif() if(NOT HAVE_SHMAT) add_definitions(-DNO_SHARED_MEMORY) - set(X_CFLAGS "${X_CFLAGS} -DNO_SHARED_MEMORY") + list(APPEND EXTRA_CFLAGS "-DNO_SHARED_MEMORY") endif() endif() @@ -439,8 +451,6 @@ macro(CheckX11) endif() endif() - set(SDL_CFLAGS "${SDL_CFLAGS} ${X_CFLAGS}") - set(CMAKE_REQUIRED_LIBRARIES ${X11_LIB} ${X11_LIB}) check_c_source_compiles(" #include @@ -625,35 +635,6 @@ macro(CheckWayland) if(VIDEO_WAYLAND) pkg_check_modules(WAYLAND wayland-client wayland-scanner wayland-protocols wayland-egl wayland-cursor egl xkbcommon) - # We have to generate some protocol interface code for some various Wayland features. - if(WAYLAND_FOUND) - execute_process( - COMMAND ${PKG_CONFIG_EXECUTABLE} --variable=pkgdatadir wayland-client - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - RESULT_VARIABLE WAYLAND_CORE_PROTOCOL_DIR_RC - OUTPUT_VARIABLE WAYLAND_CORE_PROTOCOL_DIR - ERROR_QUIET - OUTPUT_STRIP_TRAILING_WHITESPACE - ) - if(NOT WAYLAND_CORE_PROTOCOL_DIR_RC EQUAL 0) - set(WAYLAND_FOUND FALSE) - endif() - endif() - - if(WAYLAND_FOUND) - execute_process( - COMMAND ${PKG_CONFIG_EXECUTABLE} --variable=pkgdatadir wayland-protocols - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - RESULT_VARIABLE WAYLAND_PROTOCOLS_DIR_RC - OUTPUT_VARIABLE WAYLAND_PROTOCOLS_DIR - ERROR_QUIET - OUTPUT_STRIP_TRAILING_WHITESPACE - ) - if(NOT WAYLAND_PROTOCOLS_DIR_RC EQUAL 0) - set(WAYLAND_FOUND FALSE) - endif() - endif() - if(WAYLAND_FOUND) execute_process( COMMAND ${PKG_CONFIG_EXECUTABLE} --variable=wayland_scanner wayland-scanner @@ -685,11 +666,10 @@ macro(CheckWayland) file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/wayland-generated-protocols") include_directories("${CMAKE_CURRENT_BINARY_DIR}/wayland-generated-protocols") - WaylandProtocolGen("${WAYLAND_SCANNER}" "${WAYLAND_CORE_PROTOCOL_DIR}/wayland.xml" "wayland") - - foreach(_PROTL relative-pointer-unstable-v1 pointer-constraints-unstable-v1 xdg-shell-unstable-v6) - string(REGEX REPLACE "\\-unstable\\-.*$" "" PROTSUBDIR ${_PROTL}) - WaylandProtocolGen("${WAYLAND_SCANNER}" "${WAYLAND_PROTOCOLS_DIR}/unstable/${PROTSUBDIR}/${_PROTL}.xml" "${_PROTL}") + file(GLOB WAYLAND_PROTOCOLS_XML RELATIVE "${SDL2_SOURCE_DIR}/wayland-protocols/" "${SDL2_SOURCE_DIR}/wayland-protocols/*.xml") + foreach(_XML ${WAYLAND_PROTOCOLS_XML}) + string(REGEX REPLACE "\\.xml$" "" _PROTL "${_XML}") + WaylandProtocolGen("${WAYLAND_SCANNER}" "${SDL2_SOURCE_DIR}/wayland-protocols/${_XML}" "${_PROTL}") endforeach() if(VIDEO_WAYLAND_QT_TOUCH) diff --git a/Engine/lib/sdl/configure b/Engine/lib/sdl/configure index 1c7f87eb0..aee0cb6ec 100755 --- a/Engine/lib/sdl/configure +++ b/Engine/lib/sdl/configure @@ -785,6 +785,7 @@ enable_render enable_events enable_joystick enable_haptic +enable_sensor enable_power enable_filesystem enable_threads @@ -866,7 +867,9 @@ enable_input_tslib enable_pthreads enable_pthread_sem enable_directx +enable_wasapi enable_sdl_dlopen +enable_hidapi enable_clock_gettime enable_rpath enable_render_d3d @@ -1521,6 +1524,7 @@ Optional Features: --enable-joystick Enable the joystick subsystem [[default=yes]] --enable-haptic Enable the haptic (force feedback) subsystem [[default=yes]] + --enable-sensor Enable the sensor subsystem [[default=yes]] --enable-power Enable the power subsystem [[default=yes]] --enable-filesystem Enable the filesystem subsystem [[default=yes]] --enable-threads Enable the threading subsystem [[default=yes]] @@ -1572,7 +1576,7 @@ Optional Features: QtWayland server support for Wayland video driver [[default=yes]] --enable-wayland-shared dynamically load Wayland support [[default=maybe]] - --enable-video-mir use Mir video driver [[default=yes]] + --enable-video-mir use Mir video driver [[default=no]] --enable-mir-shared dynamically load Mir support [[default=maybe]] --enable-video-rpi use Raspberry Pi video driver [[default=yes]] --enable-video-x11 use X11 video driver [[default=yes]] @@ -1620,7 +1624,10 @@ Optional Features: [[default=yes]] --enable-pthread-sem use pthread semaphores [[default=yes]] --enable-directx use DirectX for Windows audio/video [[default=yes]] + --enable-wasapi use the Windows WASAPI audio driver [[default=yes]] --enable-sdl-dlopen use dlopen for shared object loading [[default=yes]] + --enable-hidapi use HIDAPI for low level joystick drivers + [[default=no]] --enable-clock_gettime use clock_gettime() instead of gettimeofday() on UNIX [[default=yes]] --enable-rpath use an rpath when linking SDL [[default=yes]] @@ -2712,9 +2719,9 @@ orig_CFLAGS="$CFLAGS" # SDL_MAJOR_VERSION=2 SDL_MINOR_VERSION=0 -SDL_MICRO_VERSION=8 +SDL_MICRO_VERSION=9 SDL_INTERFACE_AGE=0 -SDL_BINARY_AGE=8 +SDL_BINARY_AGE=9 SDL_VERSION=$SDL_MAJOR_VERSION.$SDL_MINOR_VERSION.$SDL_MICRO_VERSION @@ -15752,10 +15759,17 @@ EXTRA_LDFLAGS="$BASE_LDFLAGS" # fi #done SDL_CFLAGS="$BASE_CFLAGS" -SDL_LIBS="-lSDL2 $BASE_LDFLAGS" -CPPFLAGS="$CPPFLAGS $EXTRA_CFLAGS" -CFLAGS="$CFLAGS $EXTRA_CFLAGS" -LDFLAGS="$LDFLAGS $EXTRA_LDFLAGS" +SDL_LIBS="-lSDL2" +if test "x$BASE_LDFLAGS" != x; then + SDL_LIBS="$SDL_LIBS $BASE_LDFLAGS" +fi +if test "x$EXTRA_CFLAGS" != x; then + CPPFLAGS="$CPPFLAGS $EXTRA_CFLAGS" + CFLAGS="$CFLAGS $EXTRA_CFLAGS" +fi +if test "x$EXTRA_LDFLAGS" != x; then + LDFLAGS="$LDFLAGS $EXTRA_LDFLAGS" +fi base_libdir=`echo \${libdir} | sed 's/.*\/\(.*\)/\1/; q'` @@ -16690,7 +16704,7 @@ if test "x$ac_cv_lib_m_pow" = xyes; then : LIBS="$LIBS -lm"; EXTRA_LDFLAGS="$EXTRA_LDFLAGS -lm" fi - for ac_func in acos acosf asin asinf atan atanf atan2 atan2f ceil ceilf copysign copysignf cos cosf fabs fabsf floor floorf fmod fmodf log logf log10 log10f pow powf scalbn scalbnf sin sinf sqrt sqrtf tan tanf + for ac_func in acos acosf asin asinf atan atanf atan2 atan2f ceil ceilf copysign copysignf cos cosf exp expf fabs fabsf floor floorf fmod fmodf log logf log10 log10f pow powf scalbn scalbnf sin sinf sqrt sqrtf tan tanf do : as_ac_var=`$as_echo "ac_cv_func_$ac_func" | $as_tr_sh` ac_fn_c_check_func "$LINENO" "$ac_func" "$as_ac_var" @@ -16764,7 +16778,7 @@ $as_echo "#define HAVE_SA_SIGACTION 1" >>confdefs.h fi - for ac_header in libunwind.h + for ac_header in libunwind.h do : ac_fn_c_check_header_mongrel "$LINENO" "libunwind.h" "ac_cv_header_libunwind_h" "$ac_includes_default" if test "x$ac_cv_header_libunwind_h" = xyes; then : @@ -16872,6 +16886,7 @@ SOURCES="$SOURCES $srcdir/src/power/*.c" #SOURCES="$SOURCES $srcdir/src/filesystem/*.c" SOURCES="$SOURCES $srcdir/src/render/*.c" SOURCES="$SOURCES $srcdir/src/render/*/*.c" +SOURCES="$SOURCES $srcdir/src/sensor/*.c" SOURCES="$SOURCES $srcdir/src/stdlib/*.c" SOURCES="$SOURCES $srcdir/src/thread/*.c" SOURCES="$SOURCES $srcdir/src/timer/*.c" @@ -16977,6 +16992,20 @@ $as_echo "#define SDL_HAPTIC_DISABLED 1" >>confdefs.h else SUMMARY_modules="${SUMMARY_modules} haptic" fi +# Check whether --enable-sensor was given. +if test "${enable_sensor+set}" = set; then : + enableval=$enable_sensor; +else + enable_sensor=yes +fi + +if test x$enable_sensor != xyes; then + +$as_echo "#define SDL_SENSOR_DISABLED 1" >>confdefs.h + +else + SUMMARY_modules="${SUMMARY_modules} sensor" +fi # Check whether --enable-power was given. if test "${enable_power+set}" = set; then : enableval=$enable_power; @@ -18368,7 +18397,7 @@ fi { $as_echo "$as_me:${as_lineno-$LINENO}: checking for PulseAudio $PULSEAUDIO_REQUIRED_VERSION support" >&5 $as_echo_n "checking for PulseAudio $PULSEAUDIO_REQUIRED_VERSION support... " >&6; } if test x$PKG_CONFIG != xno; then - if $PKG_CONFIG --atleast-pkgconfig-version 0.7 && $PKG_CONFIG --atleast-version $PULSEAUDIO_REQUIRED_VERSION libpulse-simple; then + if $PKG_CONFIG --atleast-pkgconfig-version 0.7 && $PKG_CONFIG --atleast-version $PULSEAUDIO_REQUIRED_VERSION libpulse-simple; then PULSEAUDIO_CFLAGS=`$PKG_CONFIG --cflags libpulse-simple` PULSEAUDIO_LIBS=`$PKG_CONFIG --libs libpulse-simple` audio_pulseaudio=yes @@ -19202,8 +19231,6 @@ $as_echo_n "checking for Wayland support... " >&6; } WAYLAND_CFLAGS=`$PKG_CONFIG --cflags wayland-client wayland-egl wayland-cursor xkbcommon` WAYLAND_LIBS=`$PKG_CONFIG --libs wayland-client wayland-egl wayland-cursor xkbcommon` WAYLAND_SCANNER=`$PKG_CONFIG --variable=wayland_scanner wayland-scanner` - WAYLAND_CORE_PROTOCOL_DIR=`$PKG_CONFIG --variable=pkgdatadir wayland-client` - WAYLAND_PROTOCOLS_DIR=`$PKG_CONFIG --variable=pkgdatadir wayland-protocols` video_wayland=yes fi fi @@ -19220,9 +19247,8 @@ $as_echo "#define SDL_VIDEO_DRIVER_WAYLAND_QT_TOUCH 1" >>confdefs.h fi - WAYLAND_PROTOCOLS_UNSTABLE="relative-pointer-unstable-v1 pointer-constraints-unstable-v1 xdg-shell-unstable-v6" - - SOURCES="$SOURCES $srcdir/src/video/wayland/*.c" + WAYLAND_SOURCES="$srcdir/src/video/wayland/*.c" + SOURCES="$SOURCES $WAYLAND_SOURCES" EXTRA_CFLAGS="$EXTRA_CFLAGS $WAYLAND_CFLAGS -I\$(gen)" # Check whether --enable-wayland-shared was given. if test "${enable_wayland_shared+set}" = set; then : @@ -19300,7 +19326,7 @@ CheckMir() if test "${enable_video_mir+set}" = set; then : enableval=$enable_video_mir; else - enable_video_mir=yes + enable_video_mir=no fi @@ -20457,7 +20483,7 @@ $as_echo_n "checking for XGenericEvent... " >&6; } cat confdefs.h - <<_ACEOF >conftest.$ac_ext /* end confdefs.h. */ - #include + #include int main () @@ -20801,13 +20827,13 @@ $as_echo "#define SDL_VIDEO_DRIVER_X11_XINPUT2 1" >>confdefs.h { $as_echo "$as_me:${as_lineno-$LINENO}: checking for xinput2 multitouch" >&5 $as_echo_n "checking for xinput2 multitouch... " >&6; } - have_xinput2_multitouch=no - cat confdefs.h - <<_ACEOF >conftest.$ac_ext + have_xinput2_multitouch=no + cat confdefs.h - <<_ACEOF >conftest.$ac_ext /* end confdefs.h. */ - #include - #include - #include + #include + #include + #include int main () @@ -20822,14 +20848,14 @@ XITouchClassInfo *t; _ACEOF if ac_fn_c_try_compile "$LINENO"; then : - have_xinput2_multitouch=yes - $as_echo "#define SDL_VIDEO_DRIVER_X11_XINPUT2_SUPPORTS_MULTITOUCH 1" >>confdefs.h + have_xinput2_multitouch=yes + $as_echo "#define SDL_VIDEO_DRIVER_X11_XINPUT2_SUPPORTS_MULTITOUCH 1" >>confdefs.h - SUMMARY_video_x11="${SUMMARY_video_x11} xinput2_multitouch" + SUMMARY_video_x11="${SUMMARY_video_x11} xinput2_multitouch" fi rm -f core conftest.err conftest.$ac_objext conftest.$ac_ext - { $as_echo "$as_me:${as_lineno-$LINENO}: result: $have_xinput2_multitouch" >&5 + { $as_echo "$as_me:${as_lineno-$LINENO}: result: $have_xinput2_multitouch" >&5 $as_echo "$have_xinput2_multitouch" >&6; } fi # Check whether --enable-video-x11-xrandr was given. @@ -21475,6 +21501,9 @@ fi $as_echo "#define SDL_VIDEO_DRIVER_DIRECTFB 1" >>confdefs.h + +$as_echo "#define SDL_VIDEO_RENDER_DIRECTFB 1" >>confdefs.h + SOURCES="$SOURCES $srcdir/src/video/directfb/*.c" EXTRA_CFLAGS="$EXTRA_CFLAGS $DIRECTFB_CFLAGS" @@ -22018,6 +22047,24 @@ $as_echo "#define SDL_VIDEO_RENDER_OGL 1" >>confdefs.h fi } +CheckMacGLES() +{ + if test x$enable_video = xyes -a x$enable_video_opengles = xyes; then + video_opengl_egl=yes + +$as_echo "#define SDL_VIDEO_OPENGL_EGL 1" >>confdefs.h + + video_opengles_v2=yes + +$as_echo "#define SDL_VIDEO_OPENGL_ES2 1" >>confdefs.h + + +$as_echo "#define SDL_VIDEO_RENDER_OGL_ES2 1" >>confdefs.h + + SUMMARY_video="${SUMMARY_video} opengl_es2" + fi +} + CheckEmscriptenGLES() { if test x$enable_video = xyes -a x$enable_video_opengles = xyes; then @@ -23118,20 +23165,6 @@ if test "x$ac_cv_header_xinput_h" = xyes; then : fi - ac_fn_c_check_header_mongrel "$LINENO" "mmdeviceapi.h" "ac_cv_header_mmdeviceapi_h" "$ac_includes_default" -if test "x$ac_cv_header_mmdeviceapi_h" = xyes; then : - have_wasapi=yes -fi - - - ac_fn_c_check_header_mongrel "$LINENO" "audioclient.h" "ac_cv_header_audioclient_h" "$ac_includes_default" -if test "x$ac_cv_header_audioclient_h" = xyes; then : - -else - have_wasapi=no -fi - - cat confdefs.h - <<_ACEOF >conftest.$ac_ext /* end confdefs.h. */ @@ -23218,6 +23251,45 @@ $as_echo "#define HAVE_XINPUT_STATE_EX 1" >>confdefs.h ;; esac fi + + ac_fn_c_check_header_mongrel "$LINENO" "mmdeviceapi.h" "ac_cv_header_mmdeviceapi_h" "$ac_includes_default" +if test "x$ac_cv_header_mmdeviceapi_h" = xyes; then : + have_wasapi=yes +fi + + + if test x$have_wasapi = xyes; then + $as_echo "#define HAVE_MMDEVICEAPI_H 1" >>confdefs.h + + fi + ac_fn_c_check_header_mongrel "$LINENO" "audioclient.h" "ac_cv_header_audioclient_h" "$ac_includes_default" +if test "x$ac_cv_header_audioclient_h" = xyes; then : + +else + have_wasapi=no +fi + + + if test x$have_wasapi = xyes; then + $as_echo "#define HAVE_AUDIOCLIENT_H 1" >>confdefs.h + + fi + + ac_fn_c_check_header_mongrel "$LINENO" "endpointvolume.h" "ac_cv_header_endpointvolume_h" "$ac_includes_default" +if test "x$ac_cv_header_endpointvolume_h" = xyes; then : + $as_echo "#define HAVE_ENDPOINTVOLUME_H 1" >>confdefs.h + +fi + + + + # Check whether --enable-wasapi was given. +if test "${enable_wasapi+set}" = set; then : + enableval=$enable_wasapi; +else + enable_wasapi=yes +fi + } CheckDLOPEN() @@ -23696,6 +23768,93 @@ $as_echo "#define SDL_JOYSTICK_USBHID 1" >>confdefs.h esac } +CheckHIDAPI() +{ + # The hidraw support doesn't catch Xbox, PS4 and Nintendo controllers, + # so we'll just use libusb when it's available. + # + # Except that libusb requires root permissions to open devices, so that's not generally useful, and we'll disable this by default. + # Check whether --enable-hidapi was given. +if test "${enable_hidapi+set}" = set; then : + enableval=$enable_hidapi; +else + enable_hidapi=no +fi + + if test x$enable_joystick = xyes -a x$enable_hidapi = xyes; then + hidapi_support=no + # Extract the first word of "pkg-config", so it can be a program name with args. +set dummy pkg-config; ac_word=$2 +{ $as_echo "$as_me:${as_lineno-$LINENO}: checking for $ac_word" >&5 +$as_echo_n "checking for $ac_word... " >&6; } +if ${ac_cv_path_PKG_CONFIG+:} false; then : + $as_echo_n "(cached) " >&6 +else + case $PKG_CONFIG in + [\\/]* | ?:[\\/]*) + ac_cv_path_PKG_CONFIG="$PKG_CONFIG" # Let the user override the test with a path. + ;; + *) + as_save_IFS=$IFS; IFS=$PATH_SEPARATOR +for as_dir in $PATH +do + IFS=$as_save_IFS + test -z "$as_dir" && as_dir=. + for ac_exec_ext in '' $ac_executable_extensions; do + if as_fn_executable_p "$as_dir/$ac_word$ac_exec_ext"; then + ac_cv_path_PKG_CONFIG="$as_dir/$ac_word$ac_exec_ext" + $as_echo "$as_me:${as_lineno-$LINENO}: found $as_dir/$ac_word$ac_exec_ext" >&5 + break 2 + fi +done + done +IFS=$as_save_IFS + + test -z "$ac_cv_path_PKG_CONFIG" && ac_cv_path_PKG_CONFIG="no" + ;; +esac +fi +PKG_CONFIG=$ac_cv_path_PKG_CONFIG +if test -n "$PKG_CONFIG"; then + { $as_echo "$as_me:${as_lineno-$LINENO}: result: $PKG_CONFIG" >&5 +$as_echo "$PKG_CONFIG" >&6; } +else + { $as_echo "$as_me:${as_lineno-$LINENO}: result: no" >&5 +$as_echo "no" >&6; } +fi + + + if test x$PKG_CONFIG != xno; then + LIBUSB_CFLAGS=`$PKG_CONFIG --cflags libusb-1.0` + LIBUSB_LDFLAGS=`$PKG_CONFIG --libs libusb-1.0` + save_CFLAGS="$CFLAGS" + CFLAGS="$save_CFLAGS $LIBUSB_CFLAGS" + ac_fn_c_check_header_mongrel "$LINENO" "libusb.h" "ac_cv_header_libusb_h" "$ac_includes_default" +if test "x$ac_cv_header_libusb_h" = xyes; then : + have_libusb_h=yes +fi + + + CFLAGS="$save_CFLAGS" + fi + if test x$have_libusb_h = xyes; then + hidapi_support=yes + +$as_echo "#define SDL_JOYSTICK_HIDAPI 1" >>confdefs.h + + EXTRA_CFLAGS="$EXTRA_CFLAGS -I$srcdir/src/hidapi/hidapi" + SOURCES="$SOURCES $srcdir/src/joystick/hidapi/*.c" + SOURCES="$SOURCES $srcdir/src/hidapi/libusb/hid.c" + EXTRA_CFLAGS="$EXTRA_CFLAGS $LIBUSB_CFLAGS" + EXTRA_LDFLAGS="$EXTRA_LDFLAGS $LIBUSB_LDFLAGS" + fi + { $as_echo "$as_me:${as_lineno-$LINENO}: checking for hidapi support" >&5 +$as_echo_n "checking for hidapi support... " >&6; } + { $as_echo "$as_me:${as_lineno-$LINENO}: result: $hidapi_support" >&5 +$as_echo "$hidapi_support" >&6; } + fi +} + CheckClockGettime() { # Check whether --enable-clock_gettime was given. @@ -23907,6 +24066,7 @@ case "$host" in esac CheckTslib CheckUSBHID + CheckHIDAPI CheckPTHREAD CheckClockGettime CheckLinuxVersion @@ -23995,6 +24155,18 @@ $as_echo "#define SDL_HAPTIC_ANDROID 1" >>confdefs.h ;; esac fi + # Set up files for the sensor library + if test x$enable_sensor = xyes; then + case $ARCH in + android) + +$as_echo "#define SDL_SENSOR_ANDROID 1" >>confdefs.h + + SOURCES="$SOURCES $srcdir/src/sensor/android/*.c" + have_sensor=yes + ;; + esac + fi # Set up files for the power library if test x$enable_power = xyes; then case $ARCH in @@ -24113,10 +24285,11 @@ $as_echo "#define SDL_AUDIO_DRIVER_DSOUND 1" >>confdefs.h SOURCES="$SOURCES $srcdir/src/audio/directsound/*.c" fi - if test x$have_wasapi = xyes; then + if test x$have_wasapi = xyes -a x$enable_wasapi = xyes; then $as_echo "#define SDL_AUDIO_DRIVER_WASAPI 1" >>confdefs.h + SUMMARY_audio="${SUMMARY_audio} wasapi" SOURCES="$SOURCES $srcdir/src/audio/wasapi/*.c" fi have_audio=yes @@ -24140,7 +24313,13 @@ $as_echo "#define SDL_JOYSTICK_DINPUT 1" >>confdefs.h $as_echo "#define SDL_JOYSTICK_WINMM 1" >>confdefs.h fi + +$as_echo "#define SDL_JOYSTICK_HIDAPI 1" >>confdefs.h + SOURCES="$SOURCES $srcdir/src/joystick/windows/*.c" + SOURCES="$SOURCES $srcdir/src/joystick/hidapi/*.c" + SOURCES="$SOURCES $srcdir/src/hidapi/windows/hid.c" + EXTRA_CFLAGS="$EXTRA_CFLAGS -I$srcdir/src/hidapi/hidapi" have_joystick=yes fi if test x$enable_haptic = xyes; then @@ -24204,7 +24383,7 @@ $as_echo "#define SDL_LOADSO_WINDOWS 1" >>confdefs.h else LIBUUID=-luuid fi - EXTRA_LDFLAGS="$EXTRA_LDFLAGS -luser32 -lgdi32 -lwinmm -limm32 -lole32 -loleaut32 -lshell32 -lversion $LIBUUID -static-libgcc" + EXTRA_LDFLAGS="$EXTRA_LDFLAGS -luser32 -lgdi32 -lwinmm -limm32 -lole32 -loleaut32 -lshell32 -lsetupapi -lversion $LIBUUID -static-libgcc" # The Windows platform requires special setup VERSION_SOURCES="$srcdir/src/main/windows/*.rc" SDLMAIN_SOURCES="$srcdir/src/main/windows/*.c" @@ -24461,6 +24640,7 @@ $as_echo "#define SDL_VIDEO_RENDER_OGL_ES2 1" >>confdefs.h CheckMETAL CheckX11 CheckMacGL + CheckMacGLES CheckOpenGLX11 CheckVulkan CheckPTHREAD @@ -24480,7 +24660,13 @@ $as_echo "#define SDL_AUDIO_DRIVER_COREAUDIO 1" >>confdefs.h $as_echo "#define SDL_JOYSTICK_IOKIT 1" >>confdefs.h + +$as_echo "#define SDL_JOYSTICK_HIDAPI 1" >>confdefs.h + SOURCES="$SOURCES $srcdir/src/joystick/darwin/*.c" + SOURCES="$SOURCES $srcdir/src/joystick/hidapi/*.c" + SOURCES="$SOURCES $srcdir/src/hidapi/mac/hid.c" + EXTRA_CFLAGS="$EXTRA_CFLAGS -I$srcdir/src/hidapi/hidapi" have_joystick=yes fi # Set up files for the haptic library @@ -24633,7 +24819,7 @@ esac if test x$have_joystick != xyes; then if test x$enable_joystick = xyes; then -$as_echo "#define SDL_JOYSTICK_DISABLED 1" >>confdefs.h +$as_echo "#define SDL_JOYSTICK_DUMMY 1" >>confdefs.h fi SOURCES="$SOURCES $srcdir/src/joystick/dummy/*.c" @@ -24641,11 +24827,19 @@ fi if test x$have_haptic != xyes; then if test x$enable_haptic = xyes; then -$as_echo "#define SDL_HAPTIC_DISABLED 1" >>confdefs.h +$as_echo "#define SDL_HAPTIC_DUMMY 1" >>confdefs.h fi SOURCES="$SOURCES $srcdir/src/haptic/dummy/*.c" fi +if test x$have_sensor != xyes; then + if test x$enable_sensor = xyes; then + +$as_echo "#define SDL_SENSOR_DUMMY 1" >>confdefs.h + + fi + SOURCES="$SOURCES $srcdir/src/sensor/dummy/*.c" +fi if test x$have_threads != xyes; then if test x$enable_threads = xyes; then @@ -24684,54 +24878,27 @@ fi SDLTEST_SOURCES="$srcdir/src/test/*.c" if test x$video_wayland = xyes; then - WAYLAND_CORE_PROTOCOL_SOURCE='$(gen)/wayland-protocol.c' - WAYLAND_CORE_PROTOCOL_HEADER='$(gen)/wayland-client-protocol.h' - WAYLAND_PROTOCOLS_UNSTABLE_SOURCES=`echo $WAYLAND_PROTOCOLS_UNSTABLE |\ - sed 's,[^ ]\+,\\$(gen)/&-protocol.c,g'` - WAYLAND_PROTOCOLS_UNSTABLE_HEADERS=`echo $WAYLAND_PROTOCOLS_UNSTABLE |\ - sed 's,[^ ]\+,\\$(gen)/&-client-protocol.h,g'` - GEN_SOURCES="$GEN_SOURCES $WAYLAND_CORE_PROTOCOL_SOURCE $WAYLAND_PROTOCOLS_UNSTABLE_SOURCES" - GEN_HEADERS="$GEN_HEADERS $WAYLAND_CORE_PROTOCOL_HEADER $WAYLAND_PROTOCOLS_UNSTABLE_HEADERS" + WAYLAND_PROTOCOLS=`cd $srcdir/wayland-protocols ; for p in *.xml ; do echo -n "\$p" |sed 's,\\.xml\$, ,g' ; done` + WAYLAND_PROTOCOLS_SOURCES=`for p in $WAYLAND_PROTOCOLS ; do echo -n "\\$(gen)/\$p-protocol.c " ; done` + WAYLAND_PROTOCOLS_HEADERS=`for p in $WAYLAND_PROTOCOLS ; do echo -n "\\$(gen)/\$p-client-protocol.h " ; done` + GEN_SOURCES="$GEN_SOURCES $WAYLAND_PROTOCOLS_SOURCES" + GEN_HEADERS="$GEN_HEADERS $WAYLAND_PROTOCOLS_HEADERS" - WAYLAND_CORE_PROTOCOL_SOURCE_DEPENDS=" -$WAYLAND_CORE_PROTOCOL_SOURCE: $WAYLAND_CORE_PROTOCOL_DIR/wayland.xml - \$(SHELL) \$(auxdir)/mkinstalldirs \$(gen) - \$(RUN_CMD_GEN)\$(WAYLAND_SCANNER) code \$< \$@" - - WAYLAND_CORE_PROTOCOL_HEADER_DEPENDS=" -$WAYLAND_CORE_PROTOCOL_HEADER: $WAYLAND_CORE_PROTOCOL_DIR/wayland.xml - \$(SHELL) \$(auxdir)/mkinstalldirs \$(gen) - \$(RUN_CMD_GEN)\$(WAYLAND_SCANNER) client-header \$< \$@" - - WAYLAND_CORE_PROTOCOL_OBJECT=" -\$(objects)/`echo $WAYLAND_CORE_PROTOCOL_SOURCE | sed 's/\$(gen)\/\(.*\).c$/\1.lo/'`: $WAYLAND_CORE_PROTOCOL_SOURCE - \$(RUN_CMD_CC)\$(LIBTOOL) --tag=CC --mode=compile \$(CC) \$(CFLAGS) \$(EXTRA_CFLAGS) $DEPENDENCY_TRACKING_OPTIONS -c \$< -o \$@" - - WAYLAND_PROTOCOLS_CLIENT_HEADER_UNSTABLE_DEPENDS=`for p in $WAYLAND_PROTOCOLS_UNSTABLE;\ - do echo ; echo \$p | sed\ - "s,^\\([a-z\\-]\\+\\)-unstable-\\(v[0-9]\+\\)\$,\\$(gen)/&-client-protocol.h: $WAYLAND_PROTOCOLS_DIR/unstable/\1/&.xml\\\\ - \\$(SHELL) \\$(auxdir)/mkinstalldirs \\$(gen)\\\\ - \\$(RUN_CMD_GEN)\\$(WAYLAND_SCANNER) client-header \\$< \\$@," ; done` - - WAYLAND_PROTOCOLS_CODE_UNSTABLE_DEPENDS=`for p in $WAYLAND_PROTOCOLS_UNSTABLE;\ - do echo ; echo \$p | sed\ - "s,^\\([a-z\\-]\\+\\)-unstable-\\(v[0-9]\+\\)\$,\\$(gen)/&-protocol.c: $WAYLAND_PROTOCOLS_DIR/unstable/\1/&.xml\\\\ - \\$(SHELL) \\$(auxdir)/mkinstalldirs \\$(gen)\\\\ - \\$(RUN_CMD_GEN)\\$(WAYLAND_SCANNER) code \\$< \\$@," ; done` - - WAYLAND_PROTOCOLS_OBJECTS_UNSTABLE=`for p in $WAYLAND_PROTOCOLS_UNSTABLE;\ - do echo ; echo \$p | sed\ - "s,^\\([a-z\\-]\\+\\)-unstable-\\(v[0-9]\+\\)\$,\\\$(objects)/&-protocol.lo: \\$(gen)/&-protocol.c \\$(gen)/&-client-protocol.h\\\\ - \\$(RUN_CMD_CC)\\$(LIBTOOL) --tag=CC --mode=compile \\$(CC) \\$(CFLAGS) \\$(EXTRA_CFLAGS) $DEPENDENCY_TRACKING_OPTIONS -c \\$< -o \\$@," ; done` - - WAYLAND_PROTOCOLS_DEPENDS=" -$WAYLAND_CORE_PROTOCOL_SOURCE_DEPENDS -$WAYLAND_CORE_PROTOCOL_HEADER_DEPENDS -$WAYLAND_CORE_PROTOCOL_OBJECT -$WAYLAND_PROTOCOLS_CLIENT_HEADER_UNSTABLE_DEPENDS -$WAYLAND_PROTOCOLS_CODE_UNSTABLE_DEPENDS -$WAYLAND_PROTOCOLS_OBJECTS_UNSTABLE -" + WAYLAND_PROTOCOLS_DEPENDS=`for p in $WAYLAND_PROTOCOLS ; do\ + echo ;\ + echo "\\$(gen)/\$p-client-protocol.h: \\$(srcdir)/wayland-protocols/\$p.xml" ;\ + echo " @\\$(SHELL) \\$(auxdir)/mkinstalldirs \\$(gen)" ;\ + echo " \\$(RUN_CMD_GEN)\\$(WAYLAND_SCANNER) client-header \\$< \\$@" ;\ + echo ;\ + echo "\\$(gen)/\$p-protocol.c: \\$(srcdir)/wayland-protocols/\$p.xml" ;\ + echo " @\\$(SHELL) \\$(auxdir)/mkinstalldirs \\$(gen)" ;\ + echo " \\$(RUN_CMD_GEN)\\$(WAYLAND_SCANNER) code \\$< \\$@" ;\ + echo ;\ + echo "\\$(objects)/\$p-protocol.lo: \\$(gen)/\$p-protocol.c \\$(gen)/\$p-client-protocol.h" ;\ + echo " \\$(RUN_CMD_CC)\\$(LIBTOOL) --tag=CC --mode=compile \\$(CC) \\$(CFLAGS) \\$(EXTRA_CFLAGS) $DEPENDENCY_TRACKING_OPTIONS -c \\$< -o \\$@" ;\ + done ;\ + echo ;\ + for s in $WAYLAND_SOURCES ; do echo -n "\$s:" ; for p in $WAYLAND_PROTOCOLS ; do echo -n " \\$(gen)/\$p-client-protocol.h" ; done ; echo ; done ; echo` fi OBJECTS=`echo $SOURCES` diff --git a/Engine/lib/sdl/configure.in b/Engine/lib/sdl/configure.in index 1c7e79338..ae866ff0c 100644 --- a/Engine/lib/sdl/configure.in +++ b/Engine/lib/sdl/configure.in @@ -20,9 +20,9 @@ dnl Set various version strings - taken gratefully from the GTk sources # SDL_MAJOR_VERSION=2 SDL_MINOR_VERSION=0 -SDL_MICRO_VERSION=8 +SDL_MICRO_VERSION=9 SDL_INTERFACE_AGE=0 -SDL_BINARY_AGE=8 +SDL_BINARY_AGE=9 SDL_VERSION=$SDL_MAJOR_VERSION.$SDL_MINOR_VERSION.$SDL_MICRO_VERSION AC_SUBST(SDL_MAJOR_VERSION) @@ -123,10 +123,17 @@ EXTRA_LDFLAGS="$BASE_LDFLAGS" # fi #done SDL_CFLAGS="$BASE_CFLAGS" -SDL_LIBS="-lSDL2 $BASE_LDFLAGS" -CPPFLAGS="$CPPFLAGS $EXTRA_CFLAGS" -CFLAGS="$CFLAGS $EXTRA_CFLAGS" -LDFLAGS="$LDFLAGS $EXTRA_LDFLAGS" +SDL_LIBS="-lSDL2" +if test "x$BASE_LDFLAGS" != x; then + SDL_LIBS="$SDL_LIBS $BASE_LDFLAGS" +fi +if test "x$EXTRA_CFLAGS" != x; then + CPPFLAGS="$CPPFLAGS $EXTRA_CFLAGS" + CFLAGS="$CFLAGS $EXTRA_CFLAGS" +fi +if test "x$EXTRA_LDFLAGS" != x; then + LDFLAGS="$LDFLAGS $EXTRA_LDFLAGS" +fi dnl set this to use on systems that use lib64 instead of lib base_libdir=`echo \${libdir} | sed 's/.*\/\(.*\)/\1/; q'` @@ -271,14 +278,14 @@ if test x$enable_libc = xyes; then AC_CHECK_FUNCS(malloc calloc realloc free getenv setenv putenv unsetenv qsort abs bcopy memset memcpy memmove wcslen wcscmp strlen strlcpy strlcat _strrev _strupr _strlwr strchr strrchr strstr itoa _ltoa _uitoa _ultoa strtol strtoul _i64toa _ui64toa strtoll strtoull atoi atof strcmp strncmp _stricmp strcasecmp _strnicmp strncasecmp vsscanf vsnprintf fopen64 fseeko fseeko64 sigaction setjmp nanosleep sysconf sysctlbyname getauxval poll) AC_CHECK_LIB(m, pow, [LIBS="$LIBS -lm"; EXTRA_LDFLAGS="$EXTRA_LDFLAGS -lm"]) - AC_CHECK_FUNCS(acos acosf asin asinf atan atanf atan2 atan2f ceil ceilf copysign copysignf cos cosf fabs fabsf floor floorf fmod fmodf log logf log10 log10f pow powf scalbn scalbnf sin sinf sqrt sqrtf tan tanf) + AC_CHECK_FUNCS(acos acosf asin asinf atan atanf atan2 atan2f ceil ceilf copysign copysignf cos cosf exp expf fabs fabsf floor floorf fmod fmodf log logf log10 log10f pow powf scalbn scalbnf sin sinf sqrt sqrtf tan tanf) AC_CHECK_LIB(iconv, iconv_open, [LIBS="$LIBS -liconv"; EXTRA_LDFLAGS="$EXTRA_LDFLAGS -liconv"]) AC_CHECK_FUNCS(iconv) AC_CHECK_MEMBER(struct sigaction.sa_sigaction,[AC_DEFINE([HAVE_SA_SIGACTION], 1, [ ])], ,[#include ]) - dnl Check for additional non-standard headers + dnl Check for additional non-standard headers AC_CHECK_HEADERS(libunwind.h) fi @@ -339,6 +346,7 @@ SOURCES="$SOURCES $srcdir/src/power/*.c" #SOURCES="$SOURCES $srcdir/src/filesystem/*.c" SOURCES="$SOURCES $srcdir/src/render/*.c" SOURCES="$SOURCES $srcdir/src/render/*/*.c" +SOURCES="$SOURCES $srcdir/src/sensor/*.c" SOURCES="$SOURCES $srcdir/src/stdlib/*.c" SOURCES="$SOURCES $srcdir/src/thread/*.c" SOURCES="$SOURCES $srcdir/src/timer/*.c" @@ -403,6 +411,14 @@ if test x$enable_haptic != xyes; then else SUMMARY_modules="${SUMMARY_modules} haptic" fi +AC_ARG_ENABLE(sensor, +AC_HELP_STRING([--enable-sensor], [Enable the sensor subsystem [[default=yes]]]), + , enable_sensor=yes) +if test x$enable_sensor != xyes; then + AC_DEFINE(SDL_SENSOR_DISABLED, 1, [ ]) +else + SUMMARY_modules="${SUMMARY_modules} sensor" +fi AC_ARG_ENABLE(power, AC_HELP_STRING([--enable-power], [Enable the power subsystem [[default=yes]]]), , enable_power=yes) @@ -956,7 +972,7 @@ AC_HELP_STRING([--enable-pulseaudio], [use PulseAudio [[default=yes]]]), AC_PATH_PROG(PKG_CONFIG, pkg-config, no) AC_MSG_CHECKING(for PulseAudio $PULSEAUDIO_REQUIRED_VERSION support) if test x$PKG_CONFIG != xno; then - if $PKG_CONFIG --atleast-pkgconfig-version 0.7 && $PKG_CONFIG --atleast-version $PULSEAUDIO_REQUIRED_VERSION libpulse-simple; then + if $PKG_CONFIG --atleast-pkgconfig-version 0.7 && $PKG_CONFIG --atleast-version $PULSEAUDIO_REQUIRED_VERSION libpulse-simple; then PULSEAUDIO_CFLAGS=`$PKG_CONFIG --cflags libpulse-simple` PULSEAUDIO_LIBS=`$PKG_CONFIG --libs libpulse-simple` audio_pulseaudio=yes @@ -1396,8 +1412,6 @@ AC_HELP_STRING([--enable-video-wayland-qt-touch], [QtWayland server support for WAYLAND_CFLAGS=`$PKG_CONFIG --cflags wayland-client wayland-egl wayland-cursor xkbcommon` WAYLAND_LIBS=`$PKG_CONFIG --libs wayland-client wayland-egl wayland-cursor xkbcommon` WAYLAND_SCANNER=`$PKG_CONFIG --variable=wayland_scanner wayland-scanner` - WAYLAND_CORE_PROTOCOL_DIR=`$PKG_CONFIG --variable=pkgdatadir wayland-client` - WAYLAND_PROTOCOLS_DIR=`$PKG_CONFIG --variable=pkgdatadir wayland-protocols` video_wayland=yes fi fi @@ -1409,9 +1423,8 @@ AC_HELP_STRING([--enable-video-wayland-qt-touch], [QtWayland server support for AC_DEFINE(SDL_VIDEO_DRIVER_WAYLAND_QT_TOUCH, 1, [ ]) fi - WAYLAND_PROTOCOLS_UNSTABLE="relative-pointer-unstable-v1 pointer-constraints-unstable-v1 xdg-shell-unstable-v6" - - SOURCES="$SOURCES $srcdir/src/video/wayland/*.c" + WAYLAND_SOURCES="$srcdir/src/video/wayland/*.c" + SOURCES="$SOURCES $WAYLAND_SOURCES" EXTRA_CFLAGS="$EXTRA_CFLAGS $WAYLAND_CFLAGS -I\$(gen)" AC_ARG_ENABLE(wayland-shared, AC_HELP_STRING([--enable-wayland-shared], [dynamically load Wayland support [[default=maybe]]]), @@ -1744,7 +1757,7 @@ AC_HELP_STRING([--enable-x11-shared], [dynamically load X11 support [[default=ma AC_MSG_CHECKING([for XGenericEvent]) have_XGenericEvent=no AC_TRY_COMPILE([ - #include + #include ],[ Display *display; XEvent event; @@ -1858,20 +1871,20 @@ AC_HELP_STRING([--enable-video-x11-xinput], [enable X11 XInput extension for man SUMMARY_video_x11="${SUMMARY_video_x11} xinput2" AC_DEFINE(SDL_VIDEO_DRIVER_X11_XINPUT2, 1, [ ]) AC_MSG_CHECKING(for xinput2 multitouch) - have_xinput2_multitouch=no - AC_TRY_COMPILE([ - #include - #include - #include - ],[ + have_xinput2_multitouch=no + AC_TRY_COMPILE([ + #include + #include + #include + ],[ int event_type = XI_TouchBegin; XITouchClassInfo *t; - ],[ - have_xinput2_multitouch=yes - AC_DEFINE([SDL_VIDEO_DRIVER_X11_XINPUT2_SUPPORTS_MULTITOUCH], 1, []) - SUMMARY_video_x11="${SUMMARY_video_x11} xinput2_multitouch" - ]) - AC_MSG_RESULT($have_xinput2_multitouch) + ],[ + have_xinput2_multitouch=yes + AC_DEFINE([SDL_VIDEO_DRIVER_X11_XINPUT2_SUPPORTS_MULTITOUCH], 1, []) + SUMMARY_video_x11="${SUMMARY_video_x11} xinput2_multitouch" + ]) + AC_MSG_RESULT($have_xinput2_multitouch) fi AC_ARG_ENABLE(video-x11-xrandr, AC_HELP_STRING([--enable-video-x11-xrandr], [enable X11 Xrandr extension for fullscreen [[default=yes]]]), @@ -2150,6 +2163,7 @@ AC_HELP_STRING([--enable-directfb-shared], [dynamically load directfb support [[ , enable_directfb_shared=yes) AC_DEFINE(SDL_VIDEO_DRIVER_DIRECTFB, 1, [ ]) + AC_DEFINE(SDL_VIDEO_RENDER_DIRECTFB, 1, [ ]) SOURCES="$SOURCES $srcdir/src/video/directfb/*.c" EXTRA_CFLAGS="$EXTRA_CFLAGS $DIRECTFB_CFLAGS" @@ -2462,6 +2476,19 @@ CheckMacGL() fi } +dnl Check for MacOS OpenGLES +CheckMacGLES() +{ + if test x$enable_video = xyes -a x$enable_video_opengles = xyes; then + video_opengl_egl=yes + AC_DEFINE(SDL_VIDEO_OPENGL_EGL, 1, [ ]) + video_opengles_v2=yes + AC_DEFINE(SDL_VIDEO_OPENGL_ES2, 1, [ ]) + AC_DEFINE(SDL_VIDEO_RENDER_OGL_ES2, 1, [ ]) + SUMMARY_video="${SUMMARY_video} opengl_es2" + fi +} + CheckEmscriptenGLES() { if test x$enable_video = xyes -a x$enable_video_opengles = xyes; then @@ -3025,8 +3052,6 @@ AC_HELP_STRING([--enable-directx], [use DirectX for Windows audio/video [[defaul AC_CHECK_HEADER(dinput.h, have_dinput=yes) AC_CHECK_HEADER(dxgi.h, have_dxgi=yes) AC_CHECK_HEADER(xinput.h, have_xinput=yes) - AC_CHECK_HEADER(mmdeviceapi.h, have_wasapi=yes) - AC_CHECK_HEADER(audioclient.h,,have_wasapi=no) AC_TRY_COMPILE([ #include #include @@ -3071,6 +3096,21 @@ XINPUT_STATE_EX s1; ;; esac fi + + AC_CHECK_HEADER(mmdeviceapi.h, have_wasapi=yes) + if test x$have_wasapi = xyes; then + AC_DEFINE(HAVE_MMDEVICEAPI_H,1,[]) + fi + AC_CHECK_HEADER(audioclient.h,,have_wasapi=no) + if test x$have_wasapi = xyes; then + AC_DEFINE(HAVE_AUDIOCLIENT_H,1,[]) + fi + + AC_CHECK_HEADER(endpointvolume.h,AC_DEFINE(HAVE_ENDPOINTVOLUME_H,1,[])) + + AC_ARG_ENABLE(wasapi, +AC_HELP_STRING([--enable-wasapi], [use the Windows WASAPI audio driver [[default=yes]]]), + , enable_wasapi=yes) } dnl Check for the dlfcn.h interface for dynamically loading objects @@ -3244,6 +3284,41 @@ CheckUSBHID() esac } +dnl Check for HIDAPI joystick drivers +CheckHIDAPI() +{ + # The hidraw support doesn't catch Xbox, PS4 and Nintendo controllers, + # so we'll just use libusb when it's available. + # + # Except that libusb requires root permissions to open devices, so that's not generally useful, and we'll disable this by default. + AC_ARG_ENABLE(hidapi, +AC_HELP_STRING([--enable-hidapi], [use HIDAPI for low level joystick drivers [[default=no]]]), + , enable_hidapi=no) + if test x$enable_joystick = xyes -a x$enable_hidapi = xyes; then + hidapi_support=no + AC_PATH_PROG(PKG_CONFIG, pkg-config, no) + if test x$PKG_CONFIG != xno; then + LIBUSB_CFLAGS=`$PKG_CONFIG --cflags libusb-1.0` + LIBUSB_LDFLAGS=`$PKG_CONFIG --libs libusb-1.0` + save_CFLAGS="$CFLAGS" + CFLAGS="$save_CFLAGS $LIBUSB_CFLAGS" + AC_CHECK_HEADER(libusb.h, have_libusb_h=yes) + CFLAGS="$save_CFLAGS" + fi + if test x$have_libusb_h = xyes; then + hidapi_support=yes + AC_DEFINE(SDL_JOYSTICK_HIDAPI, 1, [ ]) + EXTRA_CFLAGS="$EXTRA_CFLAGS -I$srcdir/src/hidapi/hidapi" + SOURCES="$SOURCES $srcdir/src/joystick/hidapi/*.c" + SOURCES="$SOURCES $srcdir/src/hidapi/libusb/hid.c" + EXTRA_CFLAGS="$EXTRA_CFLAGS $LIBUSB_CFLAGS" + EXTRA_LDFLAGS="$EXTRA_LDFLAGS $LIBUSB_LDFLAGS" + fi + AC_MSG_CHECKING(for hidapi support) + AC_MSG_RESULT($hidapi_support) + fi +} + dnl Check for clock_gettime() CheckClockGettime() { @@ -3365,6 +3440,7 @@ case "$host" in esac CheckTslib CheckUSBHID + CheckHIDAPI CheckPTHREAD CheckClockGettime CheckLinuxVersion @@ -3437,6 +3513,16 @@ case "$host" in ;; esac fi + # Set up files for the sensor library + if test x$enable_sensor = xyes; then + case $ARCH in + android) + AC_DEFINE(SDL_SENSOR_ANDROID, 1, [ ]) + SOURCES="$SOURCES $srcdir/src/sensor/android/*.c" + have_sensor=yes + ;; + esac + fi # Set up files for the power library if test x$enable_power = xyes; then case $ARCH in @@ -3531,8 +3617,9 @@ AC_HELP_STRING([--enable-render-d3d], [enable the Direct3D render driver [[defau AC_DEFINE(SDL_AUDIO_DRIVER_DSOUND, 1, [ ]) SOURCES="$SOURCES $srcdir/src/audio/directsound/*.c" fi - if test x$have_wasapi = xyes; then + if test x$have_wasapi = xyes -a x$enable_wasapi = xyes; then AC_DEFINE(SDL_AUDIO_DRIVER_WASAPI, 1, [ ]) + SUMMARY_audio="${SUMMARY_audio} wasapi" SOURCES="$SOURCES $srcdir/src/audio/wasapi/*.c" fi have_audio=yes @@ -3550,7 +3637,11 @@ AC_HELP_STRING([--enable-render-d3d], [enable the Direct3D render driver [[defau else AC_DEFINE(SDL_JOYSTICK_WINMM, 1, [ ]) fi + AC_DEFINE(SDL_JOYSTICK_HIDAPI, 1, [ ]) SOURCES="$SOURCES $srcdir/src/joystick/windows/*.c" + SOURCES="$SOURCES $srcdir/src/joystick/hidapi/*.c" + SOURCES="$SOURCES $srcdir/src/hidapi/windows/hid.c" + EXTRA_CFLAGS="$EXTRA_CFLAGS -I$srcdir/src/hidapi/hidapi" have_joystick=yes fi if test x$enable_haptic = xyes; then @@ -3600,7 +3691,7 @@ AC_HELP_STRING([--enable-render-d3d], [enable the Direct3D render driver [[defau else LIBUUID=-luuid fi - EXTRA_LDFLAGS="$EXTRA_LDFLAGS -luser32 -lgdi32 -lwinmm -limm32 -lole32 -loleaut32 -lshell32 -lversion $LIBUUID -static-libgcc" + EXTRA_LDFLAGS="$EXTRA_LDFLAGS -luser32 -lgdi32 -lwinmm -limm32 -lole32 -loleaut32 -lshell32 -lsetupapi -lversion $LIBUUID -static-libgcc" # The Windows platform requires special setup VERSION_SOURCES="$srcdir/src/main/windows/*.rc" SDLMAIN_SOURCES="$srcdir/src/main/windows/*.c" @@ -3773,6 +3864,7 @@ AC_HELP_STRING([--enable-render-d3d], [enable the Direct3D render driver [[defau CheckMETAL CheckX11 CheckMacGL + CheckMacGLES CheckOpenGLX11 CheckVulkan CheckPTHREAD @@ -3788,7 +3880,11 @@ AC_HELP_STRING([--enable-render-d3d], [enable the Direct3D render driver [[defau # Set up files for the joystick library if test x$enable_joystick = xyes; then AC_DEFINE(SDL_JOYSTICK_IOKIT, 1, [ ]) + AC_DEFINE(SDL_JOYSTICK_HIDAPI, 1, [ ]) SOURCES="$SOURCES $srcdir/src/joystick/darwin/*.c" + SOURCES="$SOURCES $srcdir/src/joystick/hidapi/*.c" + SOURCES="$SOURCES $srcdir/src/hidapi/mac/hid.c" + EXTRA_CFLAGS="$EXTRA_CFLAGS -I$srcdir/src/hidapi/hidapi" have_joystick=yes fi # Set up files for the haptic library @@ -3916,16 +4012,22 @@ esac if test x$have_joystick != xyes; then if test x$enable_joystick = xyes; then - AC_DEFINE(SDL_JOYSTICK_DISABLED, 1, [ ]) + AC_DEFINE(SDL_JOYSTICK_DUMMY, 1, [ ]) fi SOURCES="$SOURCES $srcdir/src/joystick/dummy/*.c" fi if test x$have_haptic != xyes; then if test x$enable_haptic = xyes; then - AC_DEFINE(SDL_HAPTIC_DISABLED, 1, [ ]) + AC_DEFINE(SDL_HAPTIC_DUMMY, 1, [ ]) fi SOURCES="$SOURCES $srcdir/src/haptic/dummy/*.c" fi +if test x$have_sensor != xyes; then + if test x$enable_sensor = xyes; then + AC_DEFINE(SDL_SENSOR_DUMMY, 1, [ ]) + fi + SOURCES="$SOURCES $srcdir/src/sensor/dummy/*.c" +fi if test x$have_threads != xyes; then if test x$enable_threads = xyes; then AC_DEFINE(SDL_THREADS_DISABLED, 1, [ ]) @@ -3956,54 +4058,27 @@ fi SDLTEST_SOURCES="$srcdir/src/test/*.c" if test x$video_wayland = xyes; then - WAYLAND_CORE_PROTOCOL_SOURCE='$(gen)/wayland-protocol.c' - WAYLAND_CORE_PROTOCOL_HEADER='$(gen)/wayland-client-protocol.h' - WAYLAND_PROTOCOLS_UNSTABLE_SOURCES=`echo $WAYLAND_PROTOCOLS_UNSTABLE |\ - sed 's,[[^ ]]\+,\\$(gen)/&-protocol.c,g'` - WAYLAND_PROTOCOLS_UNSTABLE_HEADERS=`echo $WAYLAND_PROTOCOLS_UNSTABLE |\ - sed 's,[[^ ]]\+,\\$(gen)/&-client-protocol.h,g'` - GEN_SOURCES="$GEN_SOURCES $WAYLAND_CORE_PROTOCOL_SOURCE $WAYLAND_PROTOCOLS_UNSTABLE_SOURCES" - GEN_HEADERS="$GEN_HEADERS $WAYLAND_CORE_PROTOCOL_HEADER $WAYLAND_PROTOCOLS_UNSTABLE_HEADERS" + WAYLAND_PROTOCOLS=`cd $srcdir/wayland-protocols ; for p in *.xml ; do echo -n "\$p" |sed 's,\\.xml\$, ,g' ; done` + WAYLAND_PROTOCOLS_SOURCES=`for p in $WAYLAND_PROTOCOLS ; do echo -n "\\$(gen)/\$p-protocol.c " ; done` + WAYLAND_PROTOCOLS_HEADERS=`for p in $WAYLAND_PROTOCOLS ; do echo -n "\\$(gen)/\$p-client-protocol.h " ; done` + GEN_SOURCES="$GEN_SOURCES $WAYLAND_PROTOCOLS_SOURCES" + GEN_HEADERS="$GEN_HEADERS $WAYLAND_PROTOCOLS_HEADERS" - WAYLAND_CORE_PROTOCOL_SOURCE_DEPENDS=" -$WAYLAND_CORE_PROTOCOL_SOURCE: $WAYLAND_CORE_PROTOCOL_DIR/wayland.xml - \$(SHELL) \$(auxdir)/mkinstalldirs \$(gen) - \$(RUN_CMD_GEN)\$(WAYLAND_SCANNER) code \$< \$@" - - WAYLAND_CORE_PROTOCOL_HEADER_DEPENDS=" -$WAYLAND_CORE_PROTOCOL_HEADER: $WAYLAND_CORE_PROTOCOL_DIR/wayland.xml - \$(SHELL) \$(auxdir)/mkinstalldirs \$(gen) - \$(RUN_CMD_GEN)\$(WAYLAND_SCANNER) client-header \$< \$@" - - WAYLAND_CORE_PROTOCOL_OBJECT=" -\$(objects)/`echo $WAYLAND_CORE_PROTOCOL_SOURCE | sed 's/\$(gen)\/\(.*\).c$/\1.lo/'`: $WAYLAND_CORE_PROTOCOL_SOURCE - \$(RUN_CMD_CC)\$(LIBTOOL) --tag=CC --mode=compile \$(CC) \$(CFLAGS) \$(EXTRA_CFLAGS) $DEPENDENCY_TRACKING_OPTIONS -c \$< -o \$@" - - WAYLAND_PROTOCOLS_CLIENT_HEADER_UNSTABLE_DEPENDS=`for p in $WAYLAND_PROTOCOLS_UNSTABLE;\ - do echo ; echo \$p | sed\ - "s,^\\([[a-z\\-]]\\+\\)-unstable-\\(v[[0-9]]\+\\)\$,\\$(gen)/&-client-protocol.h: $WAYLAND_PROTOCOLS_DIR/unstable/\1/&.xml\\\\ - \\$(SHELL) \\$(auxdir)/mkinstalldirs \\$(gen)\\\\ - \\$(RUN_CMD_GEN)\\$(WAYLAND_SCANNER) client-header \\$< \\$@," ; done` - - WAYLAND_PROTOCOLS_CODE_UNSTABLE_DEPENDS=`for p in $WAYLAND_PROTOCOLS_UNSTABLE;\ - do echo ; echo \$p | sed\ - "s,^\\([[a-z\\-]]\\+\\)-unstable-\\(v[[0-9]]\+\\)\$,\\$(gen)/&-protocol.c: $WAYLAND_PROTOCOLS_DIR/unstable/\1/&.xml\\\\ - \\$(SHELL) \\$(auxdir)/mkinstalldirs \\$(gen)\\\\ - \\$(RUN_CMD_GEN)\\$(WAYLAND_SCANNER) code \\$< \\$@," ; done` - - WAYLAND_PROTOCOLS_OBJECTS_UNSTABLE=`for p in $WAYLAND_PROTOCOLS_UNSTABLE;\ - do echo ; echo \$p | sed\ - "s,^\\([[a-z\\-]]\\+\\)-unstable-\\(v[[0-9]]\+\\)\$,\\\$(objects)/&-protocol.lo: \\$(gen)/&-protocol.c \\$(gen)/&-client-protocol.h\\\\ - \\$(RUN_CMD_CC)\\$(LIBTOOL) --tag=CC --mode=compile \\$(CC) \\$(CFLAGS) \\$(EXTRA_CFLAGS) $DEPENDENCY_TRACKING_OPTIONS -c \\$< -o \\$@," ; done` - - WAYLAND_PROTOCOLS_DEPENDS=" -$WAYLAND_CORE_PROTOCOL_SOURCE_DEPENDS -$WAYLAND_CORE_PROTOCOL_HEADER_DEPENDS -$WAYLAND_CORE_PROTOCOL_OBJECT -$WAYLAND_PROTOCOLS_CLIENT_HEADER_UNSTABLE_DEPENDS -$WAYLAND_PROTOCOLS_CODE_UNSTABLE_DEPENDS -$WAYLAND_PROTOCOLS_OBJECTS_UNSTABLE -" + WAYLAND_PROTOCOLS_DEPENDS=`for p in $WAYLAND_PROTOCOLS ; do\ + echo ;\ + echo "\\$(gen)/\$p-client-protocol.h: \\$(srcdir)/wayland-protocols/\$p.xml" ;\ + echo " @\\$(SHELL) \\$(auxdir)/mkinstalldirs \\$(gen)" ;\ + echo " \\$(RUN_CMD_GEN)\\$(WAYLAND_SCANNER) client-header \\$< \\$@" ;\ + echo ;\ + echo "\\$(gen)/\$p-protocol.c: \\$(srcdir)/wayland-protocols/\$p.xml" ;\ + echo " @\\$(SHELL) \\$(auxdir)/mkinstalldirs \\$(gen)" ;\ + echo " \\$(RUN_CMD_GEN)\\$(WAYLAND_SCANNER) code \\$< \\$@" ;\ + echo ;\ + echo "\\$(objects)/\$p-protocol.lo: \\$(gen)/\$p-protocol.c \\$(gen)/\$p-client-protocol.h" ;\ + echo " \\$(RUN_CMD_CC)\\$(LIBTOOL) --tag=CC --mode=compile \\$(CC) \\$(CFLAGS) \\$(EXTRA_CFLAGS) $DEPENDENCY_TRACKING_OPTIONS -c \\$< -o \\$@" ;\ + done ;\ + echo ;\ + for s in $WAYLAND_SOURCES ; do echo -n "\$s:" ; for p in $WAYLAND_PROTOCOLS ; do echo -n " \\$(gen)/\$p-client-protocol.h" ; done ; echo ; done ; echo` fi OBJECTS=`echo $SOURCES` diff --git a/Engine/lib/sdl/debian/changelog b/Engine/lib/sdl/debian/changelog index 54f43972d..ba4cf4a3e 100644 --- a/Engine/lib/sdl/debian/changelog +++ b/Engine/lib/sdl/debian/changelog @@ -1,3 +1,9 @@ +libsdl2 (2.0.9) UNRELEASED; urgency=low + + * Updated SDL to version 2.0.9 + + -- Sam Lantinga Wed, 26 Sep 2018 10:02:21 -0800 + libsdl2 (2.0.8) UNRELEASED; urgency=low * Updated SDL to version 2.0.8 diff --git a/Engine/lib/sdl/include/SDL.h b/Engine/lib/sdl/include/SDL.h index d48d9d4a0..fc35a419e 100644 --- a/Engine/lib/sdl/include/SDL.h +++ b/Engine/lib/sdl/include/SDL.h @@ -51,6 +51,7 @@ #include "SDL_power.h" #include "SDL_render.h" #include "SDL_rwops.h" +#include "SDL_sensor.h" #include "SDL_shape.h" #include "SDL_system.h" #include "SDL_thread.h" @@ -80,10 +81,11 @@ extern "C" { #define SDL_INIT_HAPTIC 0x00001000u #define SDL_INIT_GAMECONTROLLER 0x00002000u /**< SDL_INIT_GAMECONTROLLER implies SDL_INIT_JOYSTICK */ #define SDL_INIT_EVENTS 0x00004000u +#define SDL_INIT_SENSOR 0x00008000u #define SDL_INIT_NOPARACHUTE 0x00100000u /**< compatibility; this flag is ignored. */ #define SDL_INIT_EVERYTHING ( \ SDL_INIT_TIMER | SDL_INIT_AUDIO | SDL_INIT_VIDEO | SDL_INIT_EVENTS | \ - SDL_INIT_JOYSTICK | SDL_INIT_HAPTIC | SDL_INIT_GAMECONTROLLER \ + SDL_INIT_JOYSTICK | SDL_INIT_HAPTIC | SDL_INIT_GAMECONTROLLER | SDL_INIT_SENSOR \ ) /* @} */ diff --git a/Engine/lib/sdl/include/SDL_audio.h b/Engine/lib/sdl/include/SDL_audio.h index d6ea68954..d3e1bface 100644 --- a/Engine/lib/sdl/include/SDL_audio.h +++ b/Engine/lib/sdl/include/SDL_audio.h @@ -140,7 +140,8 @@ typedef Uint16 SDL_AudioFormat; #define SDL_AUDIO_ALLOW_FREQUENCY_CHANGE 0x00000001 #define SDL_AUDIO_ALLOW_FORMAT_CHANGE 0x00000002 #define SDL_AUDIO_ALLOW_CHANNELS_CHANGE 0x00000004 -#define SDL_AUDIO_ALLOW_ANY_CHANGE (SDL_AUDIO_ALLOW_FREQUENCY_CHANGE|SDL_AUDIO_ALLOW_FORMAT_CHANGE|SDL_AUDIO_ALLOW_CHANNELS_CHANGE) +#define SDL_AUDIO_ALLOW_SAMPLES_CHANGE 0x00000008 +#define SDL_AUDIO_ALLOW_ANY_CHANGE (SDL_AUDIO_ALLOW_FREQUENCY_CHANGE|SDL_AUDIO_ALLOW_FORMAT_CHANGE|SDL_AUDIO_ALLOW_CHANNELS_CHANGE|SDL_AUDIO_ALLOW_SAMPLES_CHANGE) /* @} */ /* @} *//* Audio flags */ diff --git a/Engine/lib/sdl/include/SDL_config.h b/Engine/lib/sdl/include/SDL_config.h index 7e0340cdf..32f4113ff 100644 --- a/Engine/lib/sdl/include/SDL_config.h +++ b/Engine/lib/sdl/include/SDL_config.h @@ -41,8 +41,10 @@ #include "SDL_config_android.h" #elif defined(__PSP__) #include "SDL_config_psp.h" +#elif defined(__OS2__) +#include "SDL_config_os2.h" #else -/* This is a minimal configuration just to get SDL running on new platforms */ +/* This is a minimal configuration just to get SDL running on new platforms. */ #include "SDL_config_minimal.h" #endif /* platform config */ diff --git a/Engine/lib/sdl/include/SDL_config.h.cmake b/Engine/lib/sdl/include/SDL_config.h.cmake index a8d230c46..48dd2d41b 100644 --- a/Engine/lib/sdl/include/SDL_config.h.cmake +++ b/Engine/lib/sdl/include/SDL_config.h.cmake @@ -144,6 +144,8 @@ #cmakedefine HAVE_COPYSIGNF 1 #cmakedefine HAVE_COS 1 #cmakedefine HAVE_COSF 1 +#cmakedefine HAVE_EXP 1 +#cmakedefine HAVE_EXPF 1 #cmakedefine HAVE_FABS 1 #cmakedefine HAVE_FABSF 1 #cmakedefine HAVE_FLOOR 1 @@ -207,6 +209,11 @@ #cmakedefine HAVE_DINPUT_H @HAVE_DINPUT_H@ #cmakedefine HAVE_XINPUT_H @HAVE_XINPUT_H@ #cmakedefine HAVE_DXGI_H @HAVE_DXGI_H@ + +#cmakedefine HAVE_ENDPOINTVOLUME_H @HAVE_ENDPOINTVOLUME_H@ +#cmakedefine HAVE_MMDEVICEAPI_H @HAVE_MMDEVICEAPI_H@ +#cmakedefine HAVE_AUDIOCLIENT_H @HAVE_AUDIOCLIENT_H@ + #cmakedefine HAVE_XINPUT_GAMEPAD_EX @HAVE_XINPUT_GAMEPAD_EX@ #cmakedefine HAVE_XINPUT_STATE_EX @HAVE_XINPUT_STATE_EX@ @@ -221,6 +228,7 @@ #cmakedefine SDL_FILE_DISABLED @SDL_FILE_DISABLED@ #cmakedefine SDL_JOYSTICK_DISABLED @SDL_JOYSTICK_DISABLED@ #cmakedefine SDL_HAPTIC_DISABLED @SDL_HAPTIC_DISABLED@ +#cmakedefine SDL_SENSOR_DISABLED @SDL_SENSOR_DISABLED@ #cmakedefine SDL_LOADSO_DISABLED @SDL_LOADSO_DISABLED@ #cmakedefine SDL_RENDER_DISABLED @SDL_RENDER_DISABLED@ #cmakedefine SDL_THREADS_DISABLED @SDL_THREADS_DISABLED@ @@ -285,6 +293,10 @@ #cmakedefine SDL_HAPTIC_XINPUT @SDL_HAPTIC_XINPUT@ #cmakedefine SDL_HAPTIC_ANDROID @SDL_HAPTIC_ANDROID@ +/* Enable various sensor drivers */ +#cmakedefine SDL_SENSOR_ANDROID @SDL_SENSOR_ANDROID@ +#cmakedefine SDL_SENSOR_DUMMY @SDL_SENSOR_DUMMY@ + /* Enable various shared object loading systems */ #cmakedefine SDL_LOADSO_DLOPEN @SDL_LOADSO_DLOPEN@ #cmakedefine SDL_LOADSO_DUMMY @SDL_LOADSO_DUMMY@ diff --git a/Engine/lib/sdl/include/SDL_config.h.in b/Engine/lib/sdl/include/SDL_config.h.in index 422f47f78..883b6f427 100644 --- a/Engine/lib/sdl/include/SDL_config.h.in +++ b/Engine/lib/sdl/include/SDL_config.h.in @@ -149,6 +149,8 @@ #undef HAVE_COPYSIGNF #undef HAVE_COS #undef HAVE_COSF +#undef HAVE_EXP +#undef HAVE_EXPF #undef HAVE_FABS #undef HAVE_FABSF #undef HAVE_FLOOR @@ -207,6 +209,9 @@ #undef HAVE_DSOUND_H #undef HAVE_DXGI_H #undef HAVE_XINPUT_H +#undef HAVE_ENDPOINTVOLUME_H +#undef HAVE_MMDEVICEAPI_H +#undef HAVE_AUDIOCLIENT_H #undef HAVE_XINPUT_GAMEPAD_EX #undef HAVE_XINPUT_STATE_EX @@ -221,6 +226,7 @@ #undef SDL_FILE_DISABLED #undef SDL_JOYSTICK_DISABLED #undef SDL_HAPTIC_DISABLED +#undef SDL_SENSOR_DISABLED #undef SDL_LOADSO_DISABLED #undef SDL_RENDER_DISABLED #undef SDL_THREADS_DISABLED @@ -277,6 +283,7 @@ #undef SDL_JOYSTICK_WINMM #undef SDL_JOYSTICK_USBHID #undef SDL_JOYSTICK_USBHID_MACHINE_JOYSTICK_H +#undef SDL_JOYSTICK_HIDAPI #undef SDL_JOYSTICK_EMSCRIPTEN #undef SDL_HAPTIC_DUMMY #undef SDL_HAPTIC_ANDROID @@ -285,6 +292,10 @@ #undef SDL_HAPTIC_DINPUT #undef SDL_HAPTIC_XINPUT +/* Enable various sensor drivers */ +#undef SDL_SENSOR_ANDROID +#undef SDL_SENSOR_DUMMY + /* Enable various shared object loading systems */ #undef SDL_LOADSO_DLOPEN #undef SDL_LOADSO_DUMMY diff --git a/Engine/lib/sdl/include/SDL_config_android.h b/Engine/lib/sdl/include/SDL_config_android.h index 4c4da37ec..f2b28cfb2 100644 --- a/Engine/lib/sdl/include/SDL_config_android.h +++ b/Engine/lib/sdl/include/SDL_config_android.h @@ -98,6 +98,8 @@ #define HAVE_COPYSIGNF 1 #define HAVE_COS 1 #define HAVE_COSF 1 +#define HAVE_EXP 1 +#define HAVE_EXPF 1 #define HAVE_FABS 1 #define HAVE_FABSF 1 #define HAVE_FLOOR 1 @@ -132,8 +134,12 @@ /* Enable various input drivers */ #define SDL_JOYSTICK_ANDROID 1 +#define SDL_JOYSTICK_HIDAPI 1 #define SDL_HAPTIC_ANDROID 1 +/* Enable sensor driver */ +#define SDL_SENSOR_ANDROID 1 + /* Enable various shared object loading systems */ #define SDL_LOADSO_DLOPEN 1 diff --git a/Engine/lib/sdl/include/SDL_config_iphoneos.h b/Engine/lib/sdl/include/SDL_config_iphoneos.h index 7b0a6ca2d..56e2b4338 100644 --- a/Engine/lib/sdl/include/SDL_config_iphoneos.h +++ b/Engine/lib/sdl/include/SDL_config_iphoneos.h @@ -99,6 +99,8 @@ #define HAVE_COPYSIGNF 1 #define HAVE_COS 1 #define HAVE_COSF 1 +#define HAVE_EXP 1 +#define HAVE_EXPF 1 #define HAVE_FABS 1 #define HAVE_FABSF 1 #define HAVE_FLOOR 1 @@ -135,6 +137,14 @@ /* Enable MFi joystick support */ #define SDL_JOYSTICK_MFI 1 +/*#define SDL_JOYSTICK_HIDAPI 1*/ + +#ifdef __TVOS__ +#define SDL_SENSOR_DUMMY 1 +#else +/* Enable the CoreMotion sensor driver */ +#define SDL_SENSOR_COREMOTION 1 +#endif /* Enable Unix style SO loading */ #define SDL_LOADSO_DLOPEN 1 diff --git a/Engine/lib/sdl/include/SDL_config_macosx.h b/Engine/lib/sdl/include/SDL_config_macosx.h index 29f583e10..9ebd4a335 100644 --- a/Engine/lib/sdl/include/SDL_config_macosx.h +++ b/Engine/lib/sdl/include/SDL_config_macosx.h @@ -102,6 +102,8 @@ #define HAVE_COPYSIGNF 1 #define HAVE_COS 1 #define HAVE_COSF 1 +#define HAVE_EXP 1 +#define HAVE_EXPF 1 #define HAVE_FABS 1 #define HAVE_FABSF 1 #define HAVE_FLOOR 1 @@ -135,8 +137,12 @@ /* Enable various input drivers */ #define SDL_JOYSTICK_IOKIT 1 +#define SDL_JOYSTICK_HIDAPI 1 #define SDL_HAPTIC_IOKIT 1 +/* Enable the dummy sensor driver */ +#define SDL_SENSOR_DUMMY 1 + /* Enable various shared object loading systems */ #define SDL_LOADSO_DLOPEN 1 diff --git a/Engine/lib/sdl/include/SDL_config_minimal.h b/Engine/lib/sdl/include/SDL_config_minimal.h index 5b03d8b69..bf7fc447f 100644 --- a/Engine/lib/sdl/include/SDL_config_minimal.h +++ b/Engine/lib/sdl/include/SDL_config_minimal.h @@ -64,6 +64,9 @@ typedef unsigned long uintptr_t; /* Enable the stub haptic driver (src/haptic/dummy/\*.c) */ #define SDL_HAPTIC_DISABLED 1 +/* Enable the stub sensor driver (src/sensor/dummy/\*.c) */ +#define SDL_SENSOR_DISABLED 1 + /* Enable the stub shared object loader (src/loadso/dummy/\*.c) */ #define SDL_LOADSO_DISABLED 1 diff --git a/Engine/lib/sdl/include/SDL_config_os2.h b/Engine/lib/sdl/include/SDL_config_os2.h new file mode 100644 index 000000000..d1e4bb24d --- /dev/null +++ b/Engine/lib/sdl/include/SDL_config_os2.h @@ -0,0 +1,170 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 SDL_config_os2_h_ +#define SDL_config_os2_h_ +#define SDL_config_h_ + +#include "SDL_platform.h" + +#define SDL_AUDIO_DRIVER_DUMMY 1 +#define SDL_AUDIO_DRIVER_DISK 1 + +#define SDL_POWER_DISABLED 1 +#define SDL_JOYSTICK_DISABLED 1 +#define SDL_HAPTIC_DISABLED 1 +/*#undef SDL_JOYSTICK_HIDAPI */ + +#define SDL_SENSOR_DUMMY 1 +#define SDL_VIDEO_DRIVER_DUMMY 1 + +/* Enable OpenGL support */ +/* #undef SDL_VIDEO_OPENGL */ + +/* Enable Vulkan support */ +/* #undef SDL_VIDEO_VULKAN */ + +#define SDL_LOADSO_DISABLED 1 +#define SDL_THREADS_DISABLED 1 +#define SDL_TIMERS_DISABLED 1 +#define SDL_FILESYSTEM_DUMMY 1 + +/* Enable assembly routines */ +#define SDL_ASSEMBLY_ROUTINES 1 + +/* #undef HAVE_LIBSAMPLERATE_H */ + +/* Enable dynamic libsamplerate support */ +/* #undef SDL_LIBSAMPLERATE_DYNAMIC */ + +#define HAVE_LIBC 1 + +#define HAVE_SYS_TYPES_H 1 +#define HAVE_STDIO_H 1 +#define STDC_HEADERS 1 +#define HAVE_STDLIB_H 1 +#define HAVE_STDARG_H 1 +#define HAVE_STDDEF_H 1 +#define HAVE_MALLOC_H 1 +#define HAVE_MEMORY_H 1 +#define HAVE_STRING_H 1 +#define HAVE_STRINGS_H 1 +#define HAVE_WCHAR_H 1 +#define HAVE_INTTYPES_H 1 +#define HAVE_STDINT_H 1 +#define HAVE_LIMITS_H 1 +#define HAVE_CTYPE_H 1 +#define HAVE_MATH_H 1 +#define HAVE_FLOAT_H 1 +#define HAVE_SIGNAL_H 1 + +#define HAVE_MALLOC 1 +#define HAVE_CALLOC 1 +#define HAVE_REALLOC 1 +#define HAVE_FREE 1 +#if defined(__WATCOMC__) +#define HAVE__FSEEKI64 1 +#define HAVE__FTELLI64 1 +#endif +#define HAVE_ALLOCA 1 +#define HAVE_GETENV 1 +#define HAVE_SETENV 1 +#define HAVE_PUTENV 1 +#define HAVE_QSORT 1 +#define HAVE_ABS 1 +#define HAVE_BCOPY 1 +#define HAVE_MEMSET 1 +#define HAVE_MEMCPY 1 +#define HAVE_MEMMOVE 1 +#define HAVE_MEMCMP 1 +#define HAVE_WCSLEN 1 +#define HAVE_WCSLCPY 1 +#define HAVE_WCSLCAT 1 +#define HAVE_WCSCMP 1 +#define HAVE_STRLEN 1 +#define HAVE_STRLCPY 1 +#define HAVE_STRLCAT 1 +#define HAVE__STRREV 1 +#define HAVE__STRUPR 1 +#define HAVE__STRLWR 1 +#define HAVE_INDEX 1 +#define HAVE_RINDEX 1 +#define HAVE_STRCHR 1 +#define HAVE_STRRCHR 1 +#define HAVE_STRSTR 1 +#define HAVE_ITOA 1 +#define HAVE__LTOA 1 +#define HAVE__ULTOA 1 +#define HAVE_STRTOL 1 +#define HAVE_STRTOUL 1 +#define HAVE__I64TOA 1 +#define HAVE__UI64TOA 1 +#define HAVE_STRTOLL 1 +#define HAVE_STRTOULL 1 +#define HAVE_STRTOD 1 +#define HAVE_ATOI 1 +#define HAVE_ATOF 1 +#define HAVE_STRCMP 1 +#define HAVE_STRNCMP 1 +#define HAVE_STRICMP 1 +#define HAVE_STRCASECMP 1 +#define HAVE_STRNCASECMP 1 +#define HAVE_SSCANF 1 +#define HAVE_SNPRINTF 1 +#define HAVE_VSNPRINTF 1 +#define HAVE_SETJMP 1 +#define HAVE_ACOS 1 +/* #undef HAVE_ACOSF */ +#define HAVE_ASIN 1 +/* #undef HAVE_ASINF */ +#define HAVE_ATAN 1 +#define HAVE_ATAN2 1 +/* #undef HAVE_ATAN2F */ +#define HAVE_CEIL 1 +/* #undef HAVE_CEILF */ +/* #undef HAVE_COPYSIGN */ +/* #undef HAVE_COPYSIGNF */ +#define HAVE_COS 1 +/* #undef HAVE_COSF */ +#define HAVE_EXP 1 +/* #undef HAVE_EXPF */ +#define HAVE_FABS 1 +/* #undef HAVE_FABSF */ +#define HAVE_FLOOR 1 +/* #undef HAVE_FLOORF */ +#define HAVE_FMOD 1 +/* #undef HAVE_FMODF */ +#define HAVE_LOG 1 +/* #undef HAVE_LOGF */ +#define HAVE_LOG10 1 +/* #undef HAVE_LOG10F */ +#define HAVE_POW 1 +/* #undef HAVE_POWF */ +#define HAVE_SIN 1 +/* #undef HAVE_SINF */ +/* #undef HAVE_SCALBN */ +/* #undef HAVE_SCALBNF */ +#define HAVE_SQRT 1 +/* #undef HAVE_SQRTF */ +#define HAVE_TAN 1 +/* #undef HAVE_TANF */ + +#endif /* SDL_config_os2_h_ */ diff --git a/Engine/lib/sdl/include/SDL_config_pandora.h b/Engine/lib/sdl/include/SDL_config_pandora.h index be5a85c19..64111a120 100644 --- a/Engine/lib/sdl/include/SDL_config_pandora.h +++ b/Engine/lib/sdl/include/SDL_config_pandora.h @@ -90,6 +90,7 @@ #define HAVE_COPYSIGN 1 #define HAVE_COS 1 #define HAVE_COSF 1 +#define HAVE_EXP 1 #define HAVE_FABS 1 #define HAVE_FLOOR 1 #define HAVE_LOG 1 @@ -113,6 +114,8 @@ #define SDL_JOYSTICK_LINUX 1 #define SDL_HAPTIC_LINUX 1 +#define SDL_SENSOR_DUMMY 1 + #define SDL_LOADSO_DLOPEN 1 #define SDL_THREAD_PTHREAD 1 diff --git a/Engine/lib/sdl/include/SDL_config_psp.h b/Engine/lib/sdl/include/SDL_config_psp.h index 61c334978..2422672a4 100644 --- a/Engine/lib/sdl/include/SDL_config_psp.h +++ b/Engine/lib/sdl/include/SDL_config_psp.h @@ -97,6 +97,8 @@ #define HAVE_COPYSIGNF 1 #define HAVE_COS 1 #define HAVE_COSF 1 +#define HAVE_EXP 1 +#define HAVE_EXPF 1 #define HAVE_FABS 1 #define HAVE_FABSF 1 #define HAVE_FLOOR 1 @@ -126,22 +128,25 @@ /* PSP isn't that sophisticated */ #define LACKS_SYS_MMAN_H 1 -/* Enable the stub thread support (src/thread/psp/\*.c) */ +/* Enable the PSP thread support (src/thread/psp/\*.c) */ #define SDL_THREAD_PSP 1 -/* Enable the stub timer support (src/timer/psp/\*.c) */ +/* Enable the PSP timer support (src/timer/psp/\*.c) */ #define SDL_TIMERS_PSP 1 -/* Enable the stub joystick driver (src/joystick/psp/\*.c) */ +/* Enable the PSP joystick driver (src/joystick/psp/\*.c) */ #define SDL_JOYSTICK_PSP 1 -/* Enable the stub audio driver (src/audio/psp/\*.c) */ +/* Enable the dummy sensor driver */ +#define SDL_SENSOR_DUMMY 1 + +/* Enable the PSP audio driver (src/audio/psp/\*.c) */ #define SDL_AUDIO_DRIVER_PSP 1 -/* PSP video dirver */ +/* PSP video driver */ #define SDL_VIDEO_DRIVER_PSP 1 -/* PSP render dirver */ +/* PSP render driver */ #define SDL_VIDEO_RENDER_PSP 1 #define SDL_POWER_PSP 1 diff --git a/Engine/lib/sdl/include/SDL_config_windows.h b/Engine/lib/sdl/include/SDL_config_windows.h index 52a9ece16..c58be8e72 100644 --- a/Engine/lib/sdl/include/SDL_config_windows.h +++ b/Engine/lib/sdl/include/SDL_config_windows.h @@ -82,6 +82,9 @@ typedef unsigned int uintptr_t; #define HAVE_DSOUND_H 1 #define HAVE_DXGI_H 1 #define HAVE_XINPUT_H 1 +#define HAVE_MMDEVICEAPI_H 1 +#define HAVE_AUDIOCLIENT_H 1 +#define HAVE_ENDPOINTVOLUME_H 1 /* This is disabled by default to avoid C runtime dependencies and manifest requirements */ #ifdef HAVE_LIBC @@ -139,6 +142,8 @@ typedef unsigned int uintptr_t; #define HAVE__COPYSIGN 1 #define HAVE_COS 1 #define HAVE_COSF 1 +#define HAVE_EXP 1 +#define HAVE_EXPF 1 #define HAVE_FABS 1 #define HAVE_FABSF 1 #define HAVE_FLOOR 1 @@ -188,9 +193,13 @@ typedef unsigned int uintptr_t; /* Enable various input drivers */ #define SDL_JOYSTICK_DINPUT 1 #define SDL_JOYSTICK_XINPUT 1 +#define SDL_JOYSTICK_HIDAPI 1 #define SDL_HAPTIC_DINPUT 1 #define SDL_HAPTIC_XINPUT 1 +/* Enable the dummy sensor driver */ +#define SDL_SENSOR_DUMMY 1 + /* Enable various shared object loading systems */ #define SDL_LOADSO_WINDOWS 1 diff --git a/Engine/lib/sdl/include/SDL_config_winrt.h b/Engine/lib/sdl/include/SDL_config_winrt.h index aac0e6014..e3fe55b07 100644 --- a/Engine/lib/sdl/include/SDL_config_winrt.h +++ b/Engine/lib/sdl/include/SDL_config_winrt.h @@ -97,6 +97,11 @@ typedef unsigned int uintptr_t; #if WINAPI_FAMILY != WINAPI_FAMILY_PHONE_APP #define HAVE_XINPUT_H 1 #endif + +#define HAVE_MMDEVICEAPI_H 1 +#define HAVE_AUDIOCLIENT_H 1 +#define HAVE_ENDPOINTVOLUME_H 1 + #define HAVE_LIBC 1 #define STDC_HEADERS 1 #define HAVE_CTYPE_H 1 @@ -155,6 +160,8 @@ typedef unsigned int uintptr_t; #define HAVE__COPYSIGN 1 #define HAVE_COS 1 #define HAVE_COSF 1 +#define HAVE_EXP 1 +#define HAVE_EXPF 1 #define HAVE_FABS 1 #define HAVE_FABSF 1 #define HAVE_FLOOR 1 @@ -190,6 +197,9 @@ typedef unsigned int uintptr_t; #define SDL_HAPTIC_XINPUT 1 #endif +/* Enable the dummy sensor driver */ +#define SDL_SENSOR_DUMMY 1 + /* Enable various shared object loading systems */ #define SDL_LOADSO_WINDOWS 1 diff --git a/Engine/lib/sdl/include/SDL_config_wiz.h b/Engine/lib/sdl/include/SDL_config_wiz.h index fe86d5ec3..b6c00d0fe 100644 --- a/Engine/lib/sdl/include/SDL_config_wiz.h +++ b/Engine/lib/sdl/include/SDL_config_wiz.h @@ -94,6 +94,8 @@ #define HAVE_COPYSIGNF 1 #define HAVE_COS 1 #define HAVE_COSF 1 +#define HAVE_EXP 1 +#define HAVE_EXPF 1 #define HAVE_FABS 1 #define HAVE_FABSF 1 #define HAVE_FLOOR 1 @@ -127,6 +129,8 @@ #define SDL_JOYSTICK_LINUX 1 #define SDL_HAPTIC_LINUX 1 +#define SDL_SENSOR_DUMMY 1 + #define SDL_LOADSO_DLOPEN 1 #define SDL_THREAD_PTHREAD 1 diff --git a/Engine/lib/sdl/include/SDL_cpuinfo.h b/Engine/lib/sdl/include/SDL_cpuinfo.h index 081270530..ee3a47e84 100644 --- a/Engine/lib/sdl/include/SDL_cpuinfo.h +++ b/Engine/lib/sdl/include/SDL_cpuinfo.h @@ -51,16 +51,19 @@ #include #else #ifdef __ALTIVEC__ -#if HAVE_ALTIVEC_H && !defined(__APPLE_ALTIVEC__) && !defined(SDL_DISABLE_ALTIVEC_H) +#if defined(HAVE_ALTIVEC_H) && !defined(__APPLE_ALTIVEC__) && !defined(SDL_DISABLE_ALTIVEC_H) #include #undef pixel #undef bool #endif #endif +#if defined(__ARM_NEON__) && !defined(SDL_DISABLE_ARM_NEON_H) +#include +#endif #if defined(__3dNOW__) && !defined(SDL_DISABLE_MM3DNOW_H) #include #endif -#if HAVE_IMMINTRIN_H && !defined(SDL_DISABLE_IMMINTRIN_H) +#if defined(HAVE_IMMINTRIN_H) && !defined(SDL_DISABLE_IMMINTRIN_H) #include #else #if defined(__MMX__) && !defined(SDL_DISABLE_MMINTRIN_H) @@ -159,6 +162,11 @@ extern DECLSPEC SDL_bool SDLCALL SDL_HasAVX(void); */ extern DECLSPEC SDL_bool SDLCALL SDL_HasAVX2(void); +/** + * This function returns true if the CPU has AVX-512F (foundation) features. + */ +extern DECLSPEC SDL_bool SDLCALL SDL_HasAVX512F(void); + /** * This function returns true if the CPU has NEON (ARM SIMD) features. */ @@ -169,7 +177,6 @@ extern DECLSPEC SDL_bool SDLCALL SDL_HasNEON(void); */ extern DECLSPEC int SDLCALL SDL_GetSystemRAM(void); - /* Ends C function definitions when using C++ */ #ifdef __cplusplus } diff --git a/Engine/lib/sdl/include/SDL_events.h b/Engine/lib/sdl/include/SDL_events.h index 3d39e6a73..af22eb646 100644 --- a/Engine/lib/sdl/include/SDL_events.h +++ b/Engine/lib/sdl/include/SDL_events.h @@ -85,6 +85,9 @@ typedef enum Called on Android in onResume() */ + /* Display events */ + SDL_DISPLAYEVENT = 0x150, /**< Display state change */ + /* Window events */ SDL_WINDOWEVENT = 0x200, /**< Window state change */ SDL_SYSWMEVENT, /**< System specific event */ @@ -144,6 +147,9 @@ typedef enum SDL_AUDIODEVICEADDED = 0x1100, /**< A new audio device is available */ SDL_AUDIODEVICEREMOVED, /**< An audio device has been removed. */ + /* Sensor events */ + SDL_SENSORUPDATE = 0x1200, /**< A sensor was updated */ + /* Render events */ SDL_RENDER_TARGETS_RESET = 0x2000, /**< The render targets have been reset and their contents need to be updated */ SDL_RENDER_DEVICE_RESET, /**< The device has been reset and all textures need to be recreated */ @@ -168,6 +174,21 @@ typedef struct SDL_CommonEvent Uint32 timestamp; /**< In milliseconds, populated using SDL_GetTicks() */ } SDL_CommonEvent; +/** + * \brief Display state change event data (event.display.*) + */ +typedef struct SDL_DisplayEvent +{ + Uint32 type; /**< ::SDL_DISPLAYEVENT */ + Uint32 timestamp; /**< In milliseconds, populated using SDL_GetTicks() */ + Uint32 display; /**< The associated display index */ + Uint8 event; /**< ::SDL_DisplayEventID */ + Uint8 padding1; + Uint8 padding2; + Uint8 padding3; + Sint32 data1; /**< event dependent data */ +} SDL_DisplayEvent; + /** * \brief Window state change event data (event.window.*) */ @@ -471,6 +492,17 @@ typedef struct SDL_DropEvent } SDL_DropEvent; +/** + * \brief Sensor event structure (event.sensor.*) + */ +typedef struct SDL_SensorEvent +{ + Uint32 type; /**< ::SDL_SENSORUPDATE */ + Uint32 timestamp; /**< In milliseconds, populated using SDL_GetTicks() */ + Sint32 which; /**< The instance ID of the sensor */ + float data[6]; /**< Up to 6 values from the sensor - additional values can be queried using SDL_SensorGetData() */ +} SDL_SensorEvent; + /** * \brief The "quit requested" event */ @@ -526,6 +558,7 @@ typedef union SDL_Event { Uint32 type; /**< Event type, shared with all events */ SDL_CommonEvent common; /**< Common event data */ + SDL_DisplayEvent display; /**< Window event data */ SDL_WindowEvent window; /**< Window event data */ SDL_KeyboardEvent key; /**< Keyboard event data */ SDL_TextEditingEvent edit; /**< Text editing event data */ @@ -542,6 +575,7 @@ typedef union SDL_Event SDL_ControllerButtonEvent cbutton; /**< Game Controller button event data */ SDL_ControllerDeviceEvent cdevice; /**< Game Controller device event data */ SDL_AudioDeviceEvent adevice; /**< Audio device event data */ + SDL_SensorEvent sensor; /**< Sensor event data */ SDL_QuitEvent quit; /**< Quit request event data */ SDL_UserEvent user; /**< Custom event data */ SDL_SysWMEvent syswm; /**< System dependent window event data */ diff --git a/Engine/lib/sdl/include/SDL_gamecontroller.h b/Engine/lib/sdl/include/SDL_gamecontroller.h index 2e024be65..6ae9c9542 100644 --- a/Engine/lib/sdl/include/SDL_gamecontroller.h +++ b/Engine/lib/sdl/include/SDL_gamecontroller.h @@ -175,6 +175,14 @@ extern DECLSPEC SDL_bool SDLCALL SDL_IsGameController(int joystick_index); */ extern DECLSPEC const char *SDLCALL SDL_GameControllerNameForIndex(int joystick_index); +/** + * Get the mapping of a game controller. + * This can be called before any controllers are opened. + * + * \return the mapping string. Must be freed with SDL_free(). Returns NULL if no mapping is available + */ +extern DECLSPEC char *SDLCALL SDL_GameControllerMappingForDeviceIndex(int joystick_index); + /** * Open a game controller for use. * The index passed as an argument refers to the N'th game controller on the system. @@ -196,6 +204,13 @@ extern DECLSPEC SDL_GameController *SDLCALL SDL_GameControllerFromInstanceID(SDL */ extern DECLSPEC const char *SDLCALL SDL_GameControllerName(SDL_GameController *gamecontroller); +/** + * Get the player index of an opened game controller, or -1 if it's not available + * + * For XInput controllers this returns the XInput user index. + */ +extern DECLSPEC int SDLCALL SDL_GameControllerGetPlayerIndex(SDL_GameController *gamecontroller); + /** * Get the USB vendor ID of an opened controller, if available. * If the vendor ID isn't available this function returns 0. @@ -345,6 +360,19 @@ SDL_GameControllerGetBindForButton(SDL_GameController *gamecontroller, extern DECLSPEC Uint8 SDLCALL SDL_GameControllerGetButton(SDL_GameController *gamecontroller, SDL_GameControllerButton button); +/** + * Trigger a rumble effect + * Each call to this function cancels any previous rumble effect, and calling it with 0 intensity stops any rumbling. + * + * \param gamecontroller The controller to vibrate + * \param low_frequency_rumble The intensity of the low frequency (left) rumble motor, from 0 to 0xFFFF + * \param high_frequency_rumble The intensity of the high frequency (right) rumble motor, from 0 to 0xFFFF + * \param duration_ms The duration of the rumble effect, in milliseconds + * + * \return 0, or -1 if rumble isn't supported on this joystick + */ +extern DECLSPEC int SDLCALL SDL_GameControllerRumble(SDL_GameController *gamecontroller, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms); + /** * Close a controller previously opened with SDL_GameControllerOpen(). */ diff --git a/Engine/lib/sdl/include/SDL_haptic.h b/Engine/lib/sdl/include/SDL_haptic.h index e3a2bca5f..2ea1bfc16 100644 --- a/Engine/lib/sdl/include/SDL_haptic.h +++ b/Engine/lib/sdl/include/SDL_haptic.h @@ -117,6 +117,17 @@ extern "C" { #endif /* __cplusplus */ +/* FIXME: For SDL 2.1, adjust all the magnitude variables to be Uint16 (0xFFFF). + * + * At the moment the magnitude variables are mixed between signed/unsigned, and + * it is also not made clear that ALL of those variables expect a max of 0x7FFF. + * + * Some platforms may have higher precision than that (Linux FF, Windows XInput) + * so we should fix the inconsistency in favor of higher possible precision, + * adjusting for platforms that use different scales. + * -flibit + */ + /** * \typedef SDL_Haptic * @@ -656,8 +667,8 @@ typedef struct SDL_HapticRamp * This struct is exclusively for the ::SDL_HAPTIC_LEFTRIGHT effect. * * The Left/Right effect is used to explicitly control the large and small - * motors, commonly found in modern game controllers. One motor is high - * frequency, the other is low frequency. + * motors, commonly found in modern game controllers. The small (right) motor + * is high frequency, and the large (left) motor is low frequency. * * \sa SDL_HAPTIC_LEFTRIGHT * \sa SDL_HapticEffect @@ -668,7 +679,7 @@ typedef struct SDL_HapticLeftRight Uint16 type; /**< ::SDL_HAPTIC_LEFTRIGHT */ /* Replay */ - Uint32 length; /**< Duration of the effect. */ + Uint32 length; /**< Duration of the effect in milliseconds. */ /* Rumble */ Uint16 large_magnitude; /**< Control of the large controller motor. */ diff --git a/Engine/lib/sdl/include/SDL_hints.h b/Engine/lib/sdl/include/SDL_hints.h index 3834640f2..4ee72e97d 100644 --- a/Engine/lib/sdl/include/SDL_hints.h +++ b/Engine/lib/sdl/include/SDL_hints.h @@ -262,6 +262,16 @@ extern "C" { */ #define SDL_HINT_GRAB_KEYBOARD "SDL_GRAB_KEYBOARD" +/** + * \brief A variable setting the double click time, in milliseconds. + */ +#define SDL_HINT_MOUSE_DOUBLE_CLICK_TIME "SDL_MOUSE_DOUBLE_CLICK_TIME" + +/** + * \brief A variable setting the double click radius, in pixels. + */ +#define SDL_HINT_MOUSE_DOUBLE_CLICK_RADIUS "SDL_MOUSE_DOUBLE_CLICK_RADIUS" + /** * \brief A variable setting the speed scale for mouse motion, in floating point, when the mouse is not in relative mode */ @@ -329,7 +339,7 @@ extern "C" { #define SDL_HINT_IDLE_TIMER_DISABLED "SDL_IOS_IDLE_TIMER_DISABLED" /** - * \brief A variable controlling which orientations are allowed on iOS. + * \brief A variable controlling which orientations are allowed on iOS/Android. * * In some circumstances it is necessary to be able to explicitly control * which UI orientations are allowed. @@ -465,6 +475,88 @@ extern "C" { */ #define SDL_HINT_JOYSTICK_ALLOW_BACKGROUND_EVENTS "SDL_JOYSTICK_ALLOW_BACKGROUND_EVENTS" +/** + * \brief A variable controlling whether the HIDAPI joystick drivers should be used. + * + * This variable can be set to the following values: + * "0" - HIDAPI drivers are not used + * "1" - HIDAPI drivers are used (the default) + * + * This variable is the default for all drivers, but can be overridden by the hints for specific drivers below. + */ +#define SDL_HINT_JOYSTICK_HIDAPI "SDL_JOYSTICK_HIDAPI" + +/** + * \brief A variable controlling whether the HIDAPI driver for PS4 controllers should be used. + * + * This variable can be set to the following values: + * "0" - HIDAPI driver is not used + * "1" - HIDAPI driver is used + * + * The default is the value of SDL_HINT_JOYSTICK_HIDAPI + */ +#define SDL_HINT_JOYSTICK_HIDAPI_PS4 "SDL_JOYSTICK_HIDAPI_PS4" + +/** + * \brief A variable controlling whether extended input reports should be used for PS4 controllers when using the HIDAPI driver. + * + * This variable can be set to the following values: + * "0" - extended reports are not enabled (the default) + * "1" - extended reports + * + * Extended input reports allow rumble on Bluetooth PS4 controllers, but + * break DirectInput handling for applications that don't use SDL. + * + * Once extended reports are enabled, they can not be disabled without + * power cycling the controller. + */ +#define SDL_HINT_JOYSTICK_HIDAPI_PS4_RUMBLE "SDL_JOYSTICK_HIDAPI_PS4_RUMBLE" + +/** + * \brief A variable controlling whether the HIDAPI driver for Steam Controllers should be used. + * + * This variable can be set to the following values: + * "0" - HIDAPI driver is not used + * "1" - HIDAPI driver is used + * + * The default is the value of SDL_HINT_JOYSTICK_HIDAPI + */ +#define SDL_HINT_JOYSTICK_HIDAPI_STEAM "SDL_JOYSTICK_HIDAPI_STEAM" + +/** + * \brief A variable controlling whether the HIDAPI driver for Nintendo Switch controllers should be used. + * + * This variable can be set to the following values: + * "0" - HIDAPI driver is not used + * "1" - HIDAPI driver is used + * + * The default is the value of SDL_HINT_JOYSTICK_HIDAPI + */ +#define SDL_HINT_JOYSTICK_HIDAPI_SWITCH "SDL_JOYSTICK_HIDAPI_SWITCH" + +/** + * \brief A variable controlling whether the HIDAPI driver for XBox controllers should be used. + * + * This variable can be set to the following values: + * "0" - HIDAPI driver is not used + * "1" - HIDAPI driver is used + * + * The default is the value of SDL_HINT_JOYSTICK_HIDAPI + */ +#define SDL_HINT_JOYSTICK_HIDAPI_XBOX "SDL_JOYSTICK_HIDAPI_XBOX" + +/** + * \brief A variable that controls whether Steam Controllers should be exposed using the SDL joystick and game controller APIs + * + * The variable can be set to the following values: + * "0" - Do not scan for Steam Controllers + * "1" - Scan for Steam Controllers (the default) + * + * The default value is "1". This hint must be set before initializing the joystick subsystem. + */ +#define SDL_HINT_ENABLE_STEAM_CONTROLLERS "SDL_ENABLE_STEAM_CONTROLLERS" + + /** * \brief If set to "0" then never set the top most bit on a SDL Window, even if the video mode expects it. * This is a debugging aid for developers and not expected to be used by end users. The default is "1" @@ -527,6 +619,10 @@ extern "C" { * This is specially useful if you build SDL against a non glibc libc library (such as musl) which * provides a relatively small default thread stack size (a few kilobytes versus the default 8MB glibc uses). * Support for this hint is currently available only in the pthread, Windows, and PSP backend. +* +* Instead of this hint, in 2.0.9 and later, you can use +* SDL_CreateThreadWithStackSize(). This hint only works with the classic +* SDL_CreateThread(). */ #define SDL_HINT_THREAD_STACK_SIZE "SDL_THREAD_STACK_SIZE" @@ -752,6 +848,23 @@ extern "C" { */ #define SDL_HINT_ANDROID_SEPARATE_MOUSE_AND_TOUCH "SDL_ANDROID_SEPARATE_MOUSE_AND_TOUCH" + /** + * \brief A variable to control whether we trap the Android back button to handle it manually. + * This is necessary for the right mouse button to work on some Android devices, or + * to be able to trap the back button for use in your code reliably. If set to true, + * the back button will show up as an SDL_KEYDOWN / SDL_KEYUP pair with a keycode of + * SDL_SCANCODE_AC_BACK. + * + * The variable can be set to the following values: + * "0" - Back button will be handled as usual for system. (default) + * "1" - Back button will be trapped, allowing you to handle the key press + * manually. (This will also let right mouse click work on systems + * where the right mouse button functions as back.) + * + * The value of this hint is used at runtime, so it can be changed at any time. + */ +#define SDL_HINT_ANDROID_TRAP_BACK_BUTTON "SDL_ANDROID_TRAP_BACK_BUTTON" + /** * \brief A variable to control whether the return key on the soft keyboard * should hide the soft keyboard on Android and iOS. diff --git a/Engine/lib/sdl/include/SDL_joystick.h b/Engine/lib/sdl/include/SDL_joystick.h index f67772d71..6e05a9c20 100644 --- a/Engine/lib/sdl/include/SDL_joystick.h +++ b/Engine/lib/sdl/include/SDL_joystick.h @@ -97,10 +97,10 @@ typedef enum typedef enum { SDL_JOYSTICK_POWER_UNKNOWN = -1, - SDL_JOYSTICK_POWER_EMPTY, - SDL_JOYSTICK_POWER_LOW, - SDL_JOYSTICK_POWER_MEDIUM, - SDL_JOYSTICK_POWER_FULL, + SDL_JOYSTICK_POWER_EMPTY, /* <= 5% */ + SDL_JOYSTICK_POWER_LOW, /* <= 20% */ + SDL_JOYSTICK_POWER_MEDIUM, /* <= 70% */ + SDL_JOYSTICK_POWER_FULL, /* <= 100% */ SDL_JOYSTICK_POWER_WIRED, SDL_JOYSTICK_POWER_MAX } SDL_JoystickPowerLevel; @@ -132,6 +132,12 @@ extern DECLSPEC int SDLCALL SDL_NumJoysticks(void); */ extern DECLSPEC const char *SDLCALL SDL_JoystickNameForIndex(int device_index); +/** + * Get the player index of a joystick, or -1 if it's not available + * This can be called before any joysticks are opened. + */ +extern DECLSPEC int SDLCALL SDL_JoystickGetDevicePlayerIndex(int device_index); + /** * Return the GUID for the joystick at this index * This can be called before any joysticks are opened. @@ -194,6 +200,13 @@ extern DECLSPEC SDL_Joystick *SDLCALL SDL_JoystickFromInstanceID(SDL_JoystickID */ extern DECLSPEC const char *SDLCALL SDL_JoystickName(SDL_Joystick * joystick); +/** + * Get the player index of an opened joystick, or -1 if it's not available + * + * For XInput controllers this returns the XInput user index. + */ +extern DECLSPEC int SDLCALL SDL_JoystickGetPlayerIndex(SDL_Joystick * joystick); + /** * Return the GUID for this opened joystick */ @@ -361,6 +374,19 @@ extern DECLSPEC int SDLCALL SDL_JoystickGetBall(SDL_Joystick * joystick, extern DECLSPEC Uint8 SDLCALL SDL_JoystickGetButton(SDL_Joystick * joystick, int button); +/** + * Trigger a rumble effect + * Each call to this function cancels any previous rumble effect, and calling it with 0 intensity stops any rumbling. + * + * \param joystick The joystick to vibrate + * \param low_frequency_rumble The intensity of the low frequency (left) rumble motor, from 0 to 0xFFFF + * \param high_frequency_rumble The intensity of the high frequency (right) rumble motor, from 0 to 0xFFFF + * \param duration_ms The duration of the rumble effect, in milliseconds + * + * \return 0, or -1 if rumble isn't supported on this joystick + */ +extern DECLSPEC int SDLCALL SDL_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms); + /** * Close a joystick previously opened with SDL_JoystickOpen(). */ diff --git a/Engine/lib/sdl/include/SDL_revision.h b/Engine/lib/sdl/include/SDL_revision.h index dbe9b97d5..92fbe67b1 100644 --- a/Engine/lib/sdl/include/SDL_revision.h +++ b/Engine/lib/sdl/include/SDL_revision.h @@ -1,2 +1,2 @@ -#define SDL_REVISION "hg-11914:f1084c419f33" -#define SDL_REVISION_NUMBER 11914 +#define SDL_REVISION "hg-12373:8feb5da6f2fb" +#define SDL_REVISION_NUMBER 12373 diff --git a/Engine/lib/sdl/include/SDL_sensor.h b/Engine/lib/sdl/include/SDL_sensor.h new file mode 100644 index 000000000..ac163a8cd --- /dev/null +++ b/Engine/lib/sdl/include/SDL_sensor.h @@ -0,0 +1,251 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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. +*/ + +/** + * \file SDL_sensor.h + * + * Include file for SDL sensor event handling + * + */ + +#ifndef _SDL_sensor_h +#define _SDL_sensor_h + +#include "SDL_stdinc.h" +#include "SDL_error.h" + +#include "begin_code.h" +/* Set up for C function definitions, even when using C++ */ +#ifdef __cplusplus +/* *INDENT-OFF* */ +extern "C" { +/* *INDENT-ON* */ +#endif + +/** + * \brief SDL_sensor.h + * + * In order to use these functions, SDL_Init() must have been called + * with the ::SDL_INIT_SENSOR flag. This causes SDL to scan the system + * for sensors, and load appropriate drivers. + */ + +struct _SDL_Sensor; +typedef struct _SDL_Sensor SDL_Sensor; + +/** + * This is a unique ID for a sensor for the time it is connected to the system, + * and is never reused for the lifetime of the application. + * + * The ID value starts at 0 and increments from there. The value -1 is an invalid ID. + */ +typedef Sint32 SDL_SensorID; + +/* The different sensors defined by SDL + * + * Additional sensors may be available, using platform dependent semantics. + * + * Hare are the additional Android sensors: + * https://developer.android.com/reference/android/hardware/SensorEvent.html#values + */ +typedef enum +{ + SDL_SENSOR_INVALID = -1, /**< Returned for an invalid sensor */ + SDL_SENSOR_UNKNOWN, /**< Unknown sensor type */ + SDL_SENSOR_ACCEL, /**< Accelerometer */ + SDL_SENSOR_GYRO /**< Gyroscope */ +} SDL_SensorType; + +/** + * Accelerometer sensor + * + * The accelerometer returns the current acceleration in SI meters per + * second squared. This includes gravity, so a device at rest will have + * an acceleration of SDL_STANDARD_GRAVITY straight down. + * + * values[0]: Acceleration on the x axis + * values[1]: Acceleration on the y axis + * values[2]: Acceleration on the z axis + * + * For phones held in portrait mode, the axes are defined as follows: + * -X ... +X : left ... right + * -Y ... +Y : bottom ... top + * -Z ... +Z : farther ... closer + * + * The axis data is not changed when the phone is rotated. + * + * \sa SDL_GetDisplayOrientation() + */ +#define SDL_STANDARD_GRAVITY 9.80665f + +/** + * Gyroscope sensor + * + * The gyroscope returns the current rate of rotation in radians per second. + * The rotation is positive in the counter-clockwise direction. That is, + * an observer looking from a positive location on one of the axes would + * see positive rotation on that axis when it appeared to be rotating + * counter-clockwise. + * + * values[0]: Angular speed around the x axis + * values[1]: Angular speed around the y axis + * values[2]: Angular speed around the z axis + * + * For phones held in portrait mode, the axes are defined as follows: + * -X ... +X : left ... right + * -Y ... +Y : bottom ... top + * -Z ... +Z : farther ... closer + * + * The axis data is not changed when the phone is rotated. + * + * \sa SDL_GetDisplayOrientation() + */ + +/* Function prototypes */ + +/** + * \brief Count the number of sensors attached to the system right now + */ +extern DECLSPEC int SDLCALL SDL_NumSensors(void); + +/** + * \brief Get the implementation dependent name of a sensor. + * + * This can be called before any sensors are opened. + * + * \return The sensor name, or NULL if device_index is out of range. + */ +extern DECLSPEC const char *SDLCALL SDL_SensorGetDeviceName(int device_index); + +/** + * \brief Get the type of a sensor. + * + * This can be called before any sensors are opened. + * + * \return The sensor type, or SDL_SENSOR_INVALID if device_index is out of range. + */ +extern DECLSPEC SDL_SensorType SDLCALL SDL_SensorGetDeviceType(int device_index); + +/** + * \brief Get the platform dependent type of a sensor. + * + * This can be called before any sensors are opened. + * + * \return The sensor platform dependent type, or -1 if device_index is out of range. + */ +extern DECLSPEC int SDLCALL SDL_SensorGetDeviceNonPortableType(int device_index); + +/** + * \brief Get the instance ID of a sensor. + * + * This can be called before any sensors are opened. + * + * \return The sensor instance ID, or -1 if device_index is out of range. + */ +extern DECLSPEC SDL_SensorID SDLCALL SDL_SensorGetDeviceInstanceID(int device_index); + +/** + * \brief Open a sensor for use. + * + * The index passed as an argument refers to the N'th sensor on the system. + * + * \return A sensor identifier, or NULL if an error occurred. + */ +extern DECLSPEC SDL_Sensor *SDLCALL SDL_SensorOpen(int device_index); + +/** + * Return the SDL_Sensor associated with an instance id. + */ +extern DECLSPEC SDL_Sensor *SDLCALL SDL_SensorFromInstanceID(SDL_SensorID instance_id); + +/** + * \brief Get the implementation dependent name of a sensor. + * + * \return The sensor name, or NULL if the sensor is NULL. + */ +extern DECLSPEC const char *SDLCALL SDL_SensorGetName(SDL_Sensor *sensor); + +/** + * \brief Get the type of a sensor. + * + * This can be called before any sensors are opened. + * + * \return The sensor type, or SDL_SENSOR_INVALID if the sensor is NULL. + */ +extern DECLSPEC SDL_SensorType SDLCALL SDL_SensorGetType(SDL_Sensor *sensor); + +/** + * \brief Get the platform dependent type of a sensor. + * + * This can be called before any sensors are opened. + * + * \return The sensor platform dependent type, or -1 if the sensor is NULL. + */ +extern DECLSPEC int SDLCALL SDL_SensorGetNonPortableType(SDL_Sensor *sensor); + +/** + * \brief Get the instance ID of a sensor. + * + * This can be called before any sensors are opened. + * + * \return The sensor instance ID, or -1 if the sensor is NULL. + */ +extern DECLSPEC SDL_SensorID SDLCALL SDL_SensorGetInstanceID(SDL_Sensor *sensor); + +/** + * Get the current state of an opened sensor. + * + * The number of values and interpretation of the data is sensor dependent. + * + * \param sensor The sensor to query + * \param data A pointer filled with the current sensor state + * \param num_values The number of values to write to data + * + * \return 0 or -1 if an error occurred. + */ +extern DECLSPEC int SDLCALL SDL_SensorGetData(SDL_Sensor * sensor, float *data, int num_values); + +/** + * Close a sensor previously opened with SDL_SensorOpen() + */ +extern DECLSPEC void SDLCALL SDL_SensorClose(SDL_Sensor * sensor); + +/** + * Update the current state of the open sensors. + * + * This is called automatically by the event loop if sensor events are enabled. + * + * This needs to be called from the thread that initialized the sensor subsystem. + */ +extern DECLSPEC void SDLCALL SDL_SensorUpdate(void); + + +/* Ends C function definitions when using C++ */ +#ifdef __cplusplus +/* *INDENT-OFF* */ +} +/* *INDENT-ON* */ +#endif +#include "close_code.h" + +#endif /* _SDL_sensor_h */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/include/SDL_stdinc.h b/Engine/lib/sdl/include/SDL_stdinc.h index 111a0645e..e373bc380 100644 --- a/Engine/lib/sdl/include/SDL_stdinc.h +++ b/Engine/lib/sdl/include/SDL_stdinc.h @@ -86,6 +86,28 @@ #ifdef HAVE_FLOAT_H # include #endif +#if defined(HAVE_ALLOCA) && !defined(alloca) +# if defined(HAVE_ALLOCA_H) +# include +# elif defined(__GNUC__) +# define alloca __builtin_alloca +# elif defined(_MSC_VER) +# include +# define alloca _alloca +# elif defined(__WATCOMC__) +# include +# elif defined(__BORLANDC__) +# include +# elif defined(__DMC__) +# include +# elif defined(__AIX__) +#pragma alloca +# elif defined(__MRC__) +void *alloca(unsigned); +# else +char *alloca(); +# endif +#endif /** * The number of elements in an array. @@ -328,28 +350,6 @@ SDL_COMPILE_TIME_ASSERT(enum, sizeof(SDL_DUMMY_ENUM) == sizeof(int)); extern "C" { #endif -#if defined(HAVE_ALLOCA) && !defined(alloca) -# if defined(HAVE_ALLOCA_H) -# include -# elif defined(__GNUC__) -# define alloca __builtin_alloca -# elif defined(_MSC_VER) -# include -# define alloca _alloca -# elif defined(__WATCOMC__) -# include -# elif defined(__BORLANDC__) -# include -# elif defined(__DMC__) -# include -# elif defined(__AIX__) -#pragma alloca -# elif defined(__MRC__) -void *alloca(unsigned); -# else -char *alloca(); -# endif -#endif #ifdef HAVE_ALLOCA #define SDL_stack_alloc(type, count) (type*)alloca(sizeof(type)*(count)) #define SDL_stack_free(data) @@ -445,12 +445,12 @@ SDL_FORCE_INLINE void SDL_memset4(void *dst, Uint32 val, size_t dwords) #endif } - extern DECLSPEC void *SDLCALL SDL_memcpy(SDL_OUT_BYTECAP(len) void *dst, SDL_IN_BYTECAP(len) const void *src, size_t len); extern DECLSPEC void *SDLCALL SDL_memmove(SDL_OUT_BYTECAP(len) void *dst, SDL_IN_BYTECAP(len) const void *src, size_t len); extern DECLSPEC int SDLCALL SDL_memcmp(const void *s1, const void *s2, size_t len); +extern DECLSPEC wchar_t *SDLCALL SDL_wcsdup(const wchar_t *wstr); extern DECLSPEC size_t SDLCALL SDL_wcslen(const wchar_t *wstr); extern DECLSPEC size_t SDLCALL SDL_wcslcpy(SDL_OUT_Z_CAP(maxlen) wchar_t *dst, const wchar_t *src, size_t maxlen); extern DECLSPEC size_t SDLCALL SDL_wcslcat(SDL_INOUT_Z_CAP(maxlen) wchar_t *dst, const wchar_t *src, size_t maxlen); @@ -514,6 +514,8 @@ extern DECLSPEC double SDLCALL SDL_copysign(double x, double y); extern DECLSPEC float SDLCALL SDL_copysignf(float x, float y); extern DECLSPEC double SDLCALL SDL_cos(double x); extern DECLSPEC float SDLCALL SDL_cosf(float x); +extern DECLSPEC double SDLCALL SDL_exp(double x); +extern DECLSPEC float SDLCALL SDL_expf(float x); extern DECLSPEC double SDLCALL SDL_fabs(double x); extern DECLSPEC float SDLCALL SDL_fabsf(float x); extern DECLSPEC double SDLCALL SDL_floor(double x); diff --git a/Engine/lib/sdl/include/SDL_surface.h b/Engine/lib/sdl/include/SDL_surface.h index 45e5366fe..730d49fc8 100644 --- a/Engine/lib/sdl/include/SDL_surface.h +++ b/Engine/lib/sdl/include/SDL_surface.h @@ -248,6 +248,13 @@ extern DECLSPEC int SDLCALL SDL_SetSurfaceRLE(SDL_Surface * surface, extern DECLSPEC int SDLCALL SDL_SetColorKey(SDL_Surface * surface, int flag, Uint32 key); +/** + * \brief Returns whether the surface has a color key + * + * \return SDL_TRUE if the surface has a color key, or SDL_FALSE if the surface is NULL or has no color key + */ +extern DECLSPEC SDL_bool SDLCALL SDL_HasColorKey(SDL_Surface * surface); + /** * \brief Gets the color key (transparent pixel) in a blittable surface. * diff --git a/Engine/lib/sdl/include/SDL_system.h b/Engine/lib/sdl/include/SDL_system.h index 7b776fdf1..4dc372d6b 100644 --- a/Engine/lib/sdl/include/SDL_system.h +++ b/Engine/lib/sdl/include/SDL_system.h @@ -76,6 +76,18 @@ extern DECLSPEC SDL_bool SDLCALL SDL_DXGIGetOutputInfo( int displayIndex, int *a #endif /* __WIN32__ */ +/* Platform specific functions for Linux */ +#ifdef __LINUX__ + +/** + \brief Sets the UNIX nice value for a thread, using setpriority() if possible, and RealtimeKit if available. + + \return 0 on success, or -1 on error. + */ +extern DECLSPEC int SDLCALL SDL_LinuxSetThreadPriority(Sint64 threadID, int priority); + +#endif /* __LINUX__ */ + /* Platform specific functions for iOS */ #if defined(__IPHONEOS__) && __IPHONEOS__ @@ -113,6 +125,21 @@ extern DECLSPEC void * SDLCALL SDL_AndroidGetActivity(void); */ extern DECLSPEC SDL_bool SDLCALL SDL_IsAndroidTV(void); +/** + \brief Return true if the application is running on a Chromebook + */ +extern DECLSPEC SDL_bool SDLCALL SDL_IsChromebook(void); + +/** + \brief Return true is the application is running on a Samsung DeX docking station + */ +extern DECLSPEC SDL_bool SDLCALL SDL_IsDeXMode(void); + +/** + \brief Trigger the Android system back button behavior. + */ +extern DECLSPEC void SDLCALL SDL_AndroidBackButton(void); + /** See the official Android developer guide for more information: http://developer.android.com/guide/topics/data/data-storage.html @@ -236,6 +263,11 @@ extern DECLSPEC SDL_WinRT_DeviceFamily SDLCALL SDL_WinRTGetDeviceFamily(); #endif /* __WINRT__ */ +/** + \brief Return true if the current device is a tablet. + */ +extern DECLSPEC SDL_bool SDLCALL SDL_IsTablet(void); + /* Ends C function definitions when using C++ */ #ifdef __cplusplus } diff --git a/Engine/lib/sdl/include/SDL_syswm.h b/Engine/lib/sdl/include/SDL_syswm.h index 8aa4a39ec..f1c4021cc 100644 --- a/Engine/lib/sdl/include/SDL_syswm.h +++ b/Engine/lib/sdl/include/SDL_syswm.h @@ -33,12 +33,6 @@ #include "SDL_video.h" #include "SDL_version.h" -#include "begin_code.h" -/* Set up for C function definitions, even when using C++ */ -#ifdef __cplusplus -extern "C" { -#endif - /** * \file SDL_syswm.h * @@ -110,6 +104,12 @@ typedef void *EGLSurface; #include "SDL_egl.h" #endif +#include "begin_code.h" +/* Set up for C function definitions, even when using C++ */ +#ifdef __cplusplus +extern "C" { +#endif + /** * These are the various supported windowing subsystems */ diff --git a/Engine/lib/sdl/include/SDL_thread.h b/Engine/lib/sdl/include/SDL_thread.h index 82a43fc03..554dd0b61 100644 --- a/Engine/lib/sdl/include/SDL_thread.h +++ b/Engine/lib/sdl/include/SDL_thread.h @@ -54,12 +54,13 @@ typedef unsigned int SDL_TLSID; /** * The SDL thread priority. * - * \note On many systems you require special privileges to set high priority. + * \note On many systems you require special privileges to set high or time critical priority. */ typedef enum { SDL_THREAD_PRIORITY_LOW, SDL_THREAD_PRIORITY_NORMAL, - SDL_THREAD_PRIORITY_HIGH + SDL_THREAD_PRIORITY_HIGH, + SDL_THREAD_PRIORITY_TIME_CRITICAL } SDL_ThreadPriority; /** @@ -105,14 +106,24 @@ SDL_CreateThread(SDL_ThreadFunction fn, const char *name, void *data, pfnSDL_CurrentBeginThread pfnBeginThread, pfnSDL_CurrentEndThread pfnEndThread); +extern DECLSPEC SDL_Thread *SDLCALL +SDL_CreateThreadWithStackSize(int (SDLCALL * fn) (void *), + const char *name, const size_t stacksize, void *data, + pfnSDL_CurrentBeginThread pfnBeginThread, + pfnSDL_CurrentEndThread pfnEndThread); + + /** * Create a thread. */ #if defined(SDL_CreateThread) && SDL_DYNAMIC_API #undef SDL_CreateThread #define SDL_CreateThread(fn, name, data) SDL_CreateThread_REAL(fn, name, data, (pfnSDL_CurrentBeginThread)_beginthreadex, (pfnSDL_CurrentEndThread)_endthreadex) +#undef SDL_CreateThreadWithStackSize +#define SDL_CreateThreadWithStackSize(fn, name, stacksize, data) SDL_CreateThreadWithStackSize_REAL(fn, name, stacksize, data, (pfnSDL_CurrentBeginThread)_beginthreadex, (pfnSDL_CurrentEndThread)_endthreadex) #else #define SDL_CreateThread(fn, name, data) SDL_CreateThread(fn, name, data, (pfnSDL_CurrentBeginThread)_beginthreadex, (pfnSDL_CurrentEndThread)_endthreadex) +#define SDL_CreateThreadWithStackSize(fn, name, stacksize, data) SDL_CreateThreadWithStackSize(fn, name, data, (pfnSDL_CurrentBeginThread)_beginthreadex, (pfnSDL_CurrentEndThread)_endthreadex) #endif #elif defined(__OS2__) @@ -132,15 +143,31 @@ extern DECLSPEC SDL_Thread *SDLCALL SDL_CreateThread(SDL_ThreadFunction fn, const char *name, void *data, pfnSDL_CurrentBeginThread pfnBeginThread, pfnSDL_CurrentEndThread pfnEndThread); +extern DECLSPEC SDL_Thread *SDLCALL +SDL_CreateThreadWithStackSize(SDL_ThreadFunction fn, const char *name, const size_t stacksize, void *data, + pfnSDL_CurrentBeginThread pfnBeginThread, + pfnSDL_CurrentEndThread pfnEndThread); #if defined(SDL_CreateThread) && SDL_DYNAMIC_API #undef SDL_CreateThread #define SDL_CreateThread(fn, name, data) SDL_CreateThread_REAL(fn, name, data, (pfnSDL_CurrentBeginThread)_beginthread, (pfnSDL_CurrentEndThread)_endthread) +#undef SDL_CreateThreadWithStackSize +#define SDL_CreateThreadWithStackSize(fn, name, stacksize, data) SDL_CreateThreadWithStackSize_REAL(fn, name, data, (pfnSDL_CurrentBeginThread)_beginthread, (pfnSDL_CurrentEndThread)_endthread) #else #define SDL_CreateThread(fn, name, data) SDL_CreateThread(fn, name, data, (pfnSDL_CurrentBeginThread)_beginthread, (pfnSDL_CurrentEndThread)_endthread) +#define SDL_CreateThreadWithStackSize(fn, name, stacksize, data) SDL_CreateThreadWithStackSize(fn, name, stacksize, data, (pfnSDL_CurrentBeginThread)_beginthread, (pfnSDL_CurrentEndThread)_endthread) #endif #else +/** + * Create a thread with a default stack size. + * + * This is equivalent to calling: + * SDL_CreateThreadWithStackSize(fn, name, 0, data); + */ +extern DECLSPEC SDL_Thread *SDLCALL +SDL_CreateThread(SDL_ThreadFunction fn, const char *name, void *data); + /** * Create a thread. * @@ -158,9 +185,17 @@ SDL_CreateThread(SDL_ThreadFunction fn, const char *name, void *data, * If a system imposes requirements, SDL will try to munge the string for * it (truncate, etc), but the original string contents will be available * from SDL_GetThreadName(). + * + * The size (in bytes) of the new stack can be specified. Zero means "use + * the system default" which might be wildly different between platforms + * (x86 Linux generally defaults to eight megabytes, an embedded device + * might be a few kilobytes instead). + * + * In SDL 2.1, stacksize will be folded into the original SDL_CreateThread + * function. */ extern DECLSPEC SDL_Thread *SDLCALL -SDL_CreateThread(SDL_ThreadFunction fn, const char *name, void *data); +SDL_CreateThreadWithStackSize(SDL_ThreadFunction fn, const char *name, const size_t stacksize, void *data); #endif diff --git a/Engine/lib/sdl/include/SDL_version.h b/Engine/lib/sdl/include/SDL_version.h index 584b48c7a..31443e149 100644 --- a/Engine/lib/sdl/include/SDL_version.h +++ b/Engine/lib/sdl/include/SDL_version.h @@ -59,7 +59,7 @@ typedef struct SDL_version */ #define SDL_MAJOR_VERSION 2 #define SDL_MINOR_VERSION 0 -#define SDL_PATCHLEVEL 8 +#define SDL_PATCHLEVEL 9 /** * \brief Macro to determine SDL version program was compiled against. diff --git a/Engine/lib/sdl/include/SDL_video.h b/Engine/lib/sdl/include/SDL_video.h index 83f49faae..461f13805 100644 --- a/Engine/lib/sdl/include/SDL_video.h +++ b/Engine/lib/sdl/include/SDL_video.h @@ -169,6 +169,24 @@ typedef enum SDL_WINDOWEVENT_HIT_TEST /**< Window had a hit test that wasn't SDL_HITTEST_NORMAL. */ } SDL_WindowEventID; +/** + * \brief Event subtype for display events + */ +typedef enum +{ + SDL_DISPLAYEVENT_NONE, /**< Never used */ + SDL_DISPLAYEVENT_ORIENTATION /**< Display orientation has changed to data1 */ +} SDL_DisplayEventID; + +typedef enum +{ + SDL_ORIENTATION_UNKNOWN, /**< The display orientation can't be determined */ + SDL_ORIENTATION_LANDSCAPE, /**< The display is in landscape mode, with the right side up, relative to portrait mode */ + SDL_ORIENTATION_LANDSCAPE_FLIPPED, /**< The display is in landscape mode, with the left side up, relative to portrait mode */ + SDL_ORIENTATION_PORTRAIT, /**< The display is in portrait mode */ + SDL_ORIENTATION_PORTRAIT_FLIPPED /**< The display is in portrait mode, upside down */ +} SDL_DisplayOrientation; + /** * \brief An opaque handle to an OpenGL context. */ @@ -316,18 +334,6 @@ extern DECLSPEC const char * SDLCALL SDL_GetDisplayName(int displayIndex); */ extern DECLSPEC int SDLCALL SDL_GetDisplayBounds(int displayIndex, SDL_Rect * rect); -/** - * \brief Get the dots/pixels-per-inch for a display - * - * \note Diagonal, horizontal and vertical DPI can all be optionally - * returned if the parameter is non-NULL. - * - * \return 0 on success, or -1 if no DPI information is available or the index is out of range. - * - * \sa SDL_GetNumVideoDisplays() - */ -extern DECLSPEC int SDLCALL SDL_GetDisplayDPI(int displayIndex, float * ddpi, float * hdpi, float * vdpi); - /** * \brief Get the usable desktop area represented by a display, with the * primary display located at 0,0 @@ -347,6 +353,27 @@ extern DECLSPEC int SDLCALL SDL_GetDisplayDPI(int displayIndex, float * ddpi, fl */ extern DECLSPEC int SDLCALL SDL_GetDisplayUsableBounds(int displayIndex, SDL_Rect * rect); +/** + * \brief Get the dots/pixels-per-inch for a display + * + * \note Diagonal, horizontal and vertical DPI can all be optionally + * returned if the parameter is non-NULL. + * + * \return 0 on success, or -1 if no DPI information is available or the index is out of range. + * + * \sa SDL_GetNumVideoDisplays() + */ +extern DECLSPEC int SDLCALL SDL_GetDisplayDPI(int displayIndex, float * ddpi, float * hdpi, float * vdpi); + +/** + * \brief Get the orientation of a display + * + * \return The orientation of the display, or SDL_ORIENTATION_UNKNOWN if it isn't available. + * + * \sa SDL_GetNumVideoDisplays() + */ +extern DECLSPEC SDL_DisplayOrientation SDLCALL SDL_GetDisplayOrientation(int displayIndex); + /** * \brief Returns the number of available display modes. * diff --git a/Engine/lib/sdl/include/SDL_vulkan.h b/Engine/lib/sdl/include/SDL_vulkan.h index f04c21adb..972cca4d7 100644 --- a/Engine/lib/sdl/include/SDL_vulkan.h +++ b/Engine/lib/sdl/include/SDL_vulkan.h @@ -135,11 +135,11 @@ extern DECLSPEC void SDLCALL SDL_Vulkan_UnloadLibrary(void); * \brief Get the names of the Vulkan instance extensions needed to create * a surface with \c SDL_Vulkan_CreateSurface(). * - * \param [in] window Window for which the required Vulkan instance + * \param [in] \c NULL or window Window for which the required Vulkan instance * extensions should be retrieved - * \param [in,out] count pointer to an \c unsigned related to the number of + * \param [in,out] pCount pointer to an \c unsigned related to the number of * required Vulkan instance extensions - * \param [out] names \c NULL or a pointer to an array to be filled with the + * \param [out] pNames \c NULL or a pointer to an array to be filled with the * required Vulkan instance extensions * * \return \c SDL_TRUE on success, \c SDL_FALSE on error. @@ -153,6 +153,10 @@ extern DECLSPEC void SDLCALL SDL_Vulkan_UnloadLibrary(void); * is smaller than the number of required extensions, \c SDL_FALSE will be * returned instead of \c SDL_TRUE, to indicate that not all the required * extensions were returned. + * + * \note If \c window is not NULL, it will be checked against its creation + * flags to ensure that the Vulkan flag is present. This parameter + * will be removed in a future major release. * * \note The returned list of extensions will contain \c VK_KHR_surface * and zero or more platform specific extensions @@ -160,12 +164,13 @@ extern DECLSPEC void SDLCALL SDL_Vulkan_UnloadLibrary(void); * \note The extension names queried here must be enabled when calling * VkCreateInstance, otherwise surface creation will fail. * - * \note \c window should have been created with the \c SDL_WINDOW_VULKAN flag. + * \note \c window should have been created with the \c SDL_WINDOW_VULKAN flag + * or be \c NULL * * \code * unsigned int count; * // get count of required extensions - * if(!SDL_Vulkan_GetInstanceExtensions(window, &count, NULL)) + * if(!SDL_Vulkan_GetInstanceExtensions(NULL, &count, NULL)) * handle_error(); * * static const char *const additionalExtensions[] = @@ -179,7 +184,7 @@ extern DECLSPEC void SDLCALL SDL_Vulkan_UnloadLibrary(void); * handle_error(); * * // get names of required extensions - * if(!SDL_Vulkan_GetInstanceExtensions(window, &count, names)) + * if(!SDL_Vulkan_GetInstanceExtensions(NULL, &count, names)) * handle_error(); * * // copy additional extensions after required extensions diff --git a/Engine/lib/sdl/src/SDL.c b/Engine/lib/sdl/src/SDL.c index 0e552791f..6d7e16662 100644 --- a/Engine/lib/sdl/src/SDL.c +++ b/Engine/lib/sdl/src/SDL.c @@ -33,6 +33,7 @@ #include "events/SDL_events_c.h" #include "haptic/SDL_haptic_c.h" #include "joystick/SDL_joystick_c.h" +#include "sensor/SDL_sensor_c.h" /* Initialization/Cleanup routines */ #if !SDL_TIMERS_DISABLED @@ -123,11 +124,11 @@ SDL_InitSubSystem(Uint32 flags) } #if SDL_VIDEO_DRIVER_WINDOWS - if ((flags & (SDL_INIT_HAPTIC|SDL_INIT_JOYSTICK))) { - if (SDL_HelperWindowCreate() < 0) { - return -1; - } - } + if ((flags & (SDL_INIT_HAPTIC|SDL_INIT_JOYSTICK))) { + if (SDL_HelperWindowCreate() < 0) { + return -1; + } + } #endif #if !SDL_TIMERS_DISABLED @@ -232,6 +233,20 @@ SDL_InitSubSystem(Uint32 flags) #endif } + /* Initialize the sensor subsystem */ + if ((flags & SDL_INIT_SENSOR)){ +#if !SDL_SENSOR_DISABLED + if (SDL_PrivateShouldInitSubsystem(SDL_INIT_SENSOR)) { + if (SDL_SensorInit() < 0) { + return (-1); + } + } + SDL_PrivateSubsystemRefCountIncr(SDL_INIT_SENSOR); +#else + return SDL_SetError("SDL not built with sensor support"); +#endif + } + return (0); } @@ -245,6 +260,15 @@ void SDL_QuitSubSystem(Uint32 flags) { /* Shut down requested initialized subsystems */ +#if !SDL_SENSOR_DISABLED + if ((flags & SDL_INIT_SENSOR)) { + if (SDL_PrivateShouldQuitSubsystem(SDL_INIT_SENSOR)) { + SDL_SensorQuit(); + } + SDL_PrivateSubsystemRefCountDecr(SDL_INIT_SENSOR); + } +#endif + #if !SDL_JOYSTICK_DISABLED if ((flags & SDL_INIT_GAMECONTROLLER)) { /* game controller implies joystick */ @@ -451,6 +475,20 @@ SDL_GetPlatform() #endif } +SDL_bool +SDL_IsTablet() +{ +#if __ANDROID__ + extern SDL_bool SDL_IsAndroidTablet(void); + return SDL_IsAndroidTablet(); +#elif __IPHONEOS__ + extern SDL_bool SDL_IsIPad(void); + return SDL_IsIPad(); +#else + return SDL_FALSE; +#endif +} + #if defined(__WIN32__) #if (!defined(HAVE_LIBC) || defined(__WATCOMC__)) && !defined(SDL_STATIC_LIB) diff --git a/Engine/lib/sdl/src/SDL_assert.c b/Engine/lib/sdl/src/SDL_assert.c index 76f5d604d..1ca083ad4 100644 --- a/Engine/lib/sdl/src/SDL_assert.c +++ b/Engine/lib/sdl/src/SDL_assert.c @@ -120,10 +120,17 @@ static void SDL_GenerateAssertionReport(void) } -static SDL_NORETURN void SDL_ExitProcess(int exitcode) +#if defined(__WATCOMC__) +#pragma aux SDL_ExitProcess aborts; +#endif +static void SDL_ExitProcess(int exitcode) { #ifdef __WIN32__ - ExitProcess(exitcode); + /* "if you do not know the state of all threads in your process, it is + better to call TerminateProcess than ExitProcess" + https://msdn.microsoft.com/en-us/library/windows/desktop/ms682658(v=vs.85).aspx */ + TerminateProcess(GetCurrentProcess(), exitcode); + #elif defined(__EMSCRIPTEN__) emscripten_cancel_main_loop(); /* this should "kill" the app. */ emscripten_force_exit(exitcode); /* this should "kill" the app. */ @@ -134,7 +141,10 @@ static SDL_NORETURN void SDL_ExitProcess(int exitcode) } -static SDL_NORETURN void SDL_AbortAssertion(void) +#if defined(__WATCOMC__) +#pragma aux SDL_AbortAssertion aborts; +#endif +static void SDL_AbortAssertion(void) { SDL_Quit(); SDL_ExitProcess(42); diff --git a/Engine/lib/sdl/src/SDL_assert_c.h b/Engine/lib/sdl/src/SDL_assert_c.h index aa690a308..93263d6a1 100644 --- a/Engine/lib/sdl/src/SDL_assert_c.h +++ b/Engine/lib/sdl/src/SDL_assert_c.h @@ -19,6 +19,11 @@ 3. This notice may not be removed or altered from any source distribution. */ +#ifndef SDL_assert_c_h_ +#define SDL_assert_c_h_ + extern void SDL_AssertionsQuit(void); +#endif /* SDL_assert_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/atomic/SDL_atomic.c b/Engine/lib/sdl/src/atomic/SDL_atomic.c index df4920139..d0022cdc1 100644 --- a/Engine/lib/sdl/src/atomic/SDL_atomic.c +++ b/Engine/lib/sdl/src/atomic/SDL_atomic.c @@ -53,10 +53,11 @@ #endif #if defined(__WATCOMC__) && defined(__386__) +SDL_COMPILE_TIME_ASSERT(intsize, 4==sizeof(int)); #define HAVE_WATCOM_ATOMICS extern _inline int _SDL_xchg_watcom(volatile int *a, int v); #pragma aux _SDL_xchg_watcom = \ - "xchg [ecx], eax" \ + "lock xchg [ecx], eax" \ parm [ecx] [eax] \ value [eax] \ modify exact [eax]; diff --git a/Engine/lib/sdl/src/atomic/SDL_spinlock.c b/Engine/lib/sdl/src/atomic/SDL_spinlock.c index 1ebc71887..6a7b14a98 100644 --- a/Engine/lib/sdl/src/atomic/SDL_spinlock.c +++ b/Engine/lib/sdl/src/atomic/SDL_spinlock.c @@ -32,11 +32,15 @@ #include #endif +#if defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64)) +#include +#endif + #if defined(__WATCOMC__) && defined(__386__) SDL_COMPILE_TIME_ASSERT(locksize, 4==sizeof(SDL_SpinLock)); extern _inline int _SDL_xchg_watcom(volatile int *a, int v); #pragma aux _SDL_xchg_watcom = \ - "xchg [ecx], eax" \ + "lock xchg [ecx], eax" \ parm [ecx] [eax] \ value [eax] \ modify exact [eax]; @@ -116,12 +120,32 @@ SDL_AtomicTryLock(SDL_SpinLock *lock) #endif } +/* "REP NOP" is PAUSE, coded for tools that don't know it by that name. */ +#if (defined(__GNUC__) || defined(__clang__)) && (defined(__i386__) || defined(__x86_64__)) + #define PAUSE_INSTRUCTION() __asm__ __volatile__("pause\n") /* Some assemblers can't do REP NOP, so go with PAUSE. */ +#elif defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64)) + #define PAUSE_INSTRUCTION() _mm_pause() /* this is actually "rep nop" and not a SIMD instruction. No inline asm in MSVC x86-64! */ +#elif defined(__WATCOMC__) && defined(__386__) + /* watcom assembler rejects PAUSE if CPU < i686, and it refuses REP NOP as an invalid combination. Hardcode the bytes. */ + extern _inline void PAUSE_INSTRUCTION(void); + #pragma aux PAUSE_INSTRUCTION = "db 0f3h,90h" +#else + #define PAUSE_INSTRUCTION() +#endif + void SDL_AtomicLock(SDL_SpinLock *lock) { + int iterations = 0; /* FIXME: Should we have an eventual timeout? */ while (!SDL_AtomicTryLock(lock)) { - SDL_Delay(0); + if (iterations < 32) { + iterations++; + PAUSE_INSTRUCTION(); + } else { + /* !!! FIXME: this doesn't definitely give up the current timeslice, it does different things on various platforms. */ + SDL_Delay(0); + } } } diff --git a/Engine/lib/sdl/src/audio/SDL_audio.c b/Engine/lib/sdl/src/audio/SDL_audio.c index dcaebea6d..f4999f136 100644 --- a/Engine/lib/sdl/src/audio/SDL_audio.c +++ b/Engine/lib/sdl/src/audio/SDL_audio.c @@ -378,21 +378,57 @@ static int add_audio_device(const char *name, void *handle, SDL_AudioDeviceItem **devices, int *devCount) { int retval = -1; - const size_t size = sizeof (SDL_AudioDeviceItem) + SDL_strlen(name) + 1; - SDL_AudioDeviceItem *item = (SDL_AudioDeviceItem *) SDL_malloc(size); - if (item == NULL) { - return -1; - } + SDL_AudioDeviceItem *item; + const SDL_AudioDeviceItem *i; + int dupenum = 0; SDL_assert(handle != NULL); /* we reserve NULL, audio backends can't use it. */ + SDL_assert(name != NULL); + item = (SDL_AudioDeviceItem *) SDL_malloc(sizeof (SDL_AudioDeviceItem)); + if (!item) { + return SDL_OutOfMemory(); + } + + item->original_name = SDL_strdup(name); + if (!item->original_name) { + SDL_free(item); + return SDL_OutOfMemory(); + } + + item->dupenum = 0; + item->name = item->original_name; item->handle = handle; - SDL_strlcpy(item->name, name, size - sizeof (SDL_AudioDeviceItem)); SDL_LockMutex(current_audio.detectionLock); + + for (i = *devices; i != NULL; i = i->next) { + if (SDL_strcmp(name, i->original_name) == 0) { + dupenum = i->dupenum + 1; + break; /* stop at the highest-numbered dupe. */ + } + } + + if (dupenum) { + const size_t len = SDL_strlen(name) + 16; + char *replacement = (char *) SDL_malloc(len); + if (!replacement) { + SDL_UnlockMutex(current_audio.detectionLock); + SDL_free(item->original_name); + SDL_free(item); + SDL_OutOfMemory(); + return -1; + } + + SDL_snprintf(replacement, len, "%s (%d)", name, dupenum + 1); + item->dupenum = dupenum; + item->name = replacement; + } + item->next = *devices; *devices = item; - retval = (*devCount)++; + retval = (*devCount)++; /* !!! FIXME: this should be an atomic increment */ + SDL_UnlockMutex(current_audio.detectionLock); return retval; @@ -420,6 +456,11 @@ free_device_list(SDL_AudioDeviceItem **devices, int *devCount) if (item->handle != NULL) { current_audio.impl.FreeDeviceHandle(item->handle); } + /* these two pointers are the same if not a duplicate devname */ + if (item->name != item->original_name) { + SDL_free(item->name); + } + SDL_free(item->original_name); SDL_free(item); } *devices = NULL; @@ -451,7 +492,11 @@ void SDL_OpenedAudioDeviceDisconnected(SDL_AudioDevice *device) SDL_assert(get_audio_device(device->id) == device); if (!SDL_AtomicGet(&device->enabled)) { - return; + return; /* don't report disconnects more than once. */ + } + + if (SDL_AtomicGet(&device->shutdown)) { + return; /* don't report disconnect if we're trying to close device. */ } /* Ends the audio callback and mark the device as STOPPED, but the @@ -651,7 +696,7 @@ SDL_RunAudio(void *devicep) SDL_assert(!device->iscapture); /* The audio mixing is always a high priority thread */ - SDL_SetThreadPriority(SDL_THREAD_PRIORITY_HIGH); + SDL_SetThreadPriority(SDL_THREAD_PRIORITY_TIME_CRITICAL); /* Perform any thread setup */ device->threadid = SDL_ThreadID(); @@ -832,6 +877,8 @@ SDL_CaptureAudio(void *devicep) } } + current_audio.impl.PrepareToClose(device); + current_audio.impl.FlushCapture(device); current_audio.impl.ThreadDeinit(device); @@ -971,6 +1018,11 @@ clean_out_device_list(SDL_AudioDeviceItem **devices, int *devCount, SDL_bool *re } else { *devices = next; } + /* these two pointers are the same if not a duplicate devname */ + if (item->name != item->original_name) { + SDL_free(item->name); + } + SDL_free(item->original_name); SDL_free(item); } item = next; @@ -997,7 +1049,6 @@ SDL_GetNumAudioDevices(int iscapture) if (!iscapture && current_audio.outputDevicesRemoved) { clean_out_device_list(¤t_audio.outputDevices, ¤t_audio.outputDeviceCount, ¤t_audio.outputDevicesRemoved); - current_audio.outputDevicesRemoved = SDL_FALSE; } retval = iscapture ? current_audio.inputDeviceCount : current_audio.outputDeviceCount; @@ -1054,16 +1105,14 @@ close_audio_device(SDL_AudioDevice * device) return; } - if (device->id > 0) { - SDL_AudioDevice *opendev = open_devices[device->id - 1]; - SDL_assert((opendev == device) || (opendev == NULL)); - if (opendev == device) { - open_devices[device->id - 1] = NULL; - } - } - + /* make sure the device is paused before we do anything else, so the + audio callback definitely won't fire again. */ + current_audio.impl.LockDevice(device); + SDL_AtomicSet(&device->paused, 1); SDL_AtomicSet(&device->shutdown, 1); SDL_AtomicSet(&device->enabled, 0); + current_audio.impl.UnlockDevice(device); + if (device->thread != NULL) { SDL_WaitThread(device->thread, NULL); } @@ -1074,6 +1123,14 @@ close_audio_device(SDL_AudioDevice * device) SDL_free(device->work_buffer); SDL_FreeAudioStream(device->stream); + if (device->id > 0) { + SDL_AudioDevice *opendev = open_devices[device->id - 1]; + SDL_assert((opendev == device) || (opendev == NULL)); + if (opendev == device) { + open_devices[device->id - 1] = NULL; + } + } + if (device->hidden != NULL) { current_audio.impl.CloseDevice(device); } @@ -1118,8 +1175,9 @@ prepare_audiospec(const SDL_AudioSpec * orig, SDL_AudioSpec * prepared) } case 1: /* Mono */ case 2: /* Stereo */ - case 4: /* surround */ - case 6: /* surround with center and lfe */ + case 4: /* Quadrophonic */ + case 6: /* 5.1 surround */ + case 8: /* 7.1 surround */ break; default: SDL_SetError("Unsupported number of audio channels."); @@ -1312,15 +1370,12 @@ open_audio_device(const char *devname, int iscapture, build_stream = SDL_TRUE; } } - - /* !!! FIXME in 2.1: add SDL_AUDIO_ALLOW_SAMPLES_CHANGE flag? - As of 2.0.6, we will build a stream to buffer the difference between - what the app wants to feed and the device wants to eat, so everyone - gets their way. In prior releases, SDL would force the callback to - feed at the rate the device requested, adjusted for resampling. - */ if (device->spec.samples != obtained->samples) { - build_stream = SDL_TRUE; + if (allowed_changes & SDL_AUDIO_ALLOW_SAMPLES_CHANGE) { + obtained->samples = device->spec.samples; + } else { + build_stream = SDL_TRUE; + } } SDL_CalculateAudioSpec(obtained); /* recalc after possible changes. */ diff --git a/Engine/lib/sdl/src/audio/SDL_audiocvt.c b/Engine/lib/sdl/src/audio/SDL_audiocvt.c index 7fde2b93c..ee0ba321b 100644 --- a/Engine/lib/sdl/src/audio/SDL_audiocvt.c +++ b/Engine/lib/sdl/src/audio/SDL_audiocvt.c @@ -724,7 +724,7 @@ SDL_ResampleCVT(SDL_AudioCVT *cvt, const int chans, const SDL_AudioFormat format SDL_assert(format == AUDIO_F32SYS); /* we keep no streaming state here, so pad with silence on both ends. */ - padding = (float *) SDL_calloc(paddingsamples, sizeof (float)); + padding = (float *) SDL_calloc(paddingsamples ? paddingsamples : 1, sizeof (float)); if (!padding) { SDL_OutOfMemory(); return; @@ -1291,7 +1291,7 @@ SDL_NewAudioStream(const SDL_AudioFormat src_format, retval->packetlen = packetlen; retval->rate_incr = ((double) dst_rate) / ((double) src_rate); retval->resampler_padding_samples = ResamplerPadding(retval->src_rate, retval->dst_rate) * pre_resample_channels; - retval->resampler_padding = (float *) SDL_calloc(retval->resampler_padding_samples, sizeof (float)); + retval->resampler_padding = (float *) SDL_calloc(retval->resampler_padding_samples ? retval->resampler_padding_samples : 1, sizeof (float)); if (retval->resampler_padding == NULL) { SDL_FreeAudioStream(retval); diff --git a/Engine/lib/sdl/src/audio/SDL_audiodev_c.h b/Engine/lib/sdl/src/audio/SDL_audiodev_c.h index 15928d10a..2d3b0eac9 100644 --- a/Engine/lib/sdl/src/audio/SDL_audiodev_c.h +++ b/Engine/lib/sdl/src/audio/SDL_audiodev_c.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_audiodev_c_h_ +#define SDL_audiodev_c_h_ + #include "SDL.h" #include "../SDL_internal.h" #include "SDL_sysaudio.h" @@ -35,4 +39,6 @@ extern void SDL_EnumUnixAudioDevices(const int classic, int (*test)(int)); +#endif /* SDL_audiodev_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/audio/SDL_audiotypecvt.c b/Engine/lib/sdl/src/audio/SDL_audiotypecvt.c index 2fbd916e5..5f8cc22da 100644 --- a/Engine/lib/sdl/src/audio/SDL_audiotypecvt.c +++ b/Engine/lib/sdl/src/audio/SDL_audiotypecvt.c @@ -25,8 +25,10 @@ #include "SDL_cpuinfo.h" #include "SDL_assert.h" -/* !!! FIXME: write NEON code. */ -#define HAVE_NEON_INTRINSICS 0 +/* !!! FIXME: disabled until we fix https://bugzilla.libsdl.org/show_bug.cgi?id=4186 */ +#if 0 /*def __ARM_NEON__*/ +#define HAVE_NEON_INTRINSICS 1 +#endif #ifdef __SSE2__ #define HAVE_SSE2_INTRINSICS 1 @@ -62,7 +64,7 @@ SDL_AudioFilter SDL_Convert_F32_to_S32 = NULL; #define DIVBY128 0.0078125f #define DIVBY32768 0.000030517578125f -#define DIVBY2147483648 0.00000000046566128730773926 +#define DIVBY8388607 0.00000011920930376163766f #if NEED_SCALAR_CONVERTER_FALLBACKS @@ -152,7 +154,7 @@ SDL_Convert_S32_to_F32_Scalar(SDL_AudioCVT *cvt, SDL_AudioFormat format) LOG_DEBUG_CONVERT("AUDIO_S32", "AUDIO_F32"); for (i = cvt->len_cvt / sizeof (Sint32); i; --i, ++src, ++dst) { - *dst = (float) (((double) *src) * DIVBY2147483648); + *dst = ((float) (*src>>8)) * DIVBY8388607; } if (cvt->filters[++cvt->filter_index]) { @@ -171,10 +173,10 @@ SDL_Convert_F32_to_S8_Scalar(SDL_AudioCVT *cvt, SDL_AudioFormat format) for (i = cvt->len_cvt / sizeof (float); i; --i, ++src, ++dst) { const float sample = *src; - if (sample > 1.0f) { + if (sample >= 1.0f) { *dst = 127; - } else if (sample < -1.0f) { - *dst = -127; + } else if (sample <= -1.0f) { + *dst = -128; } else { *dst = (Sint8)(sample * 127.0f); } @@ -197,9 +199,9 @@ SDL_Convert_F32_to_U8_Scalar(SDL_AudioCVT *cvt, SDL_AudioFormat format) for (i = cvt->len_cvt / sizeof (float); i; --i, ++src, ++dst) { const float sample = *src; - if (sample > 1.0f) { + if (sample >= 1.0f) { *dst = 255; - } else if (sample < -1.0f) { + } else if (sample <= -1.0f) { *dst = 0; } else { *dst = (Uint8)((sample + 1.0f) * 127.0f); @@ -223,10 +225,10 @@ SDL_Convert_F32_to_S16_Scalar(SDL_AudioCVT *cvt, SDL_AudioFormat format) for (i = cvt->len_cvt / sizeof (float); i; --i, ++src, ++dst) { const float sample = *src; - if (sample > 1.0f) { + if (sample >= 1.0f) { *dst = 32767; - } else if (sample < -1.0f) { - *dst = -32767; + } else if (sample <= -1.0f) { + *dst = -32768; } else { *dst = (Sint16)(sample * 32767.0f); } @@ -249,9 +251,9 @@ SDL_Convert_F32_to_U16_Scalar(SDL_AudioCVT *cvt, SDL_AudioFormat format) for (i = cvt->len_cvt / sizeof (float); i; --i, ++src, ++dst) { const float sample = *src; - if (sample > 1.0f) { - *dst = 65534; - } else if (sample < -1.0f) { + if (sample >= 1.0f) { + *dst = 65535; + } else if (sample <= -1.0f) { *dst = 0; } else { *dst = (Uint16)((sample + 1.0f) * 32767.0f); @@ -275,12 +277,12 @@ SDL_Convert_F32_to_S32_Scalar(SDL_AudioCVT *cvt, SDL_AudioFormat format) for (i = cvt->len_cvt / sizeof (float); i; --i, ++src, ++dst) { const float sample = *src; - if (sample > 1.0f) { + if (sample >= 1.0f) { *dst = 2147483647; - } else if (sample < -1.0f) { - *dst = -2147483647; + } else if (sample <= -1.0f) { + *dst = (Sint32) -2147483648LL; } else { - *dst = (Sint32)((double)sample * 2147483647.0); + *dst = ((Sint32)(sample * 8388607.0f)) << 8; } } @@ -509,16 +511,6 @@ SDL_Convert_U16_to_F32_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) } } -#if defined(__GNUC__) && (__GNUC__ < 4) -/* these were added as of gcc-4.0: http://gcc.gnu.org/bugzilla/show_bug.cgi?id=19418 */ -static inline __m128 _mm_castsi128_ps(__m128i __A) { - return (__m128) __A; -} -static inline __m128i _mm_castps_si128(__m128 __A) { - return (__m128i) __A; -} -#endif - static void SDLCALL SDL_Convert_S32_to_F32_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) { @@ -530,7 +522,7 @@ SDL_Convert_S32_to_F32_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Get dst aligned to 16 bytes */ for (i = cvt->len_cvt / sizeof (Sint32); i && (((size_t) dst) & 15); --i, ++src, ++dst) { - *dst = (float) (((double) *src) * DIVBY2147483648); + *dst = ((float) (*src>>8)) * DIVBY8388607; } SDL_assert(!i || ((((size_t) dst) & 15) == 0)); @@ -538,15 +530,11 @@ SDL_Convert_S32_to_F32_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) { /* Aligned! Do SSE blocks as long as we have 16 bytes available. */ - const __m128d divby2147483648 = _mm_set1_pd(DIVBY2147483648); + const __m128 divby8388607 = _mm_set1_ps(DIVBY8388607); const __m128i *mmsrc = (const __m128i *) src; while (i >= 4) { /* 4 * sint32 */ - const __m128i ints = _mm_load_si128(mmsrc); - /* bitshift the whole register over, so _mm_cvtepi32_pd can read the top ints in the bottom of the vector. */ - const __m128d doubles1 = _mm_mul_pd(_mm_cvtepi32_pd(_mm_srli_si128(ints, 8)), divby2147483648); - const __m128d doubles2 = _mm_mul_pd(_mm_cvtepi32_pd(ints), divby2147483648); - /* convert to float32, bitshift/or to get these into a vector to store. */ - _mm_store_ps(dst, _mm_castsi128_ps(_mm_or_si128(_mm_slli_si128(_mm_castps_si128(_mm_cvtpd_ps(doubles1)), 8), _mm_castps_si128(_mm_cvtpd_ps(doubles2))))); + /* shift out lowest bits so int fits in a float32. Small precision loss, but much faster. */ + _mm_store_ps(dst, _mm_mul_ps(_mm_cvtepi32_ps(_mm_srai_epi32(_mm_load_si128(mmsrc), 8)), divby8388607)); i -= 4; mmsrc++; dst += 4; } src = (const Sint32 *) mmsrc; @@ -554,7 +542,7 @@ SDL_Convert_S32_to_F32_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Finish off any leftovers with scalar operations. */ while (i) { - *dst = (float) (((double) *src) * DIVBY2147483648); + *dst = ((float) (*src>>8)) * DIVBY8388607; i--; src++; dst++; } @@ -574,7 +562,14 @@ SDL_Convert_F32_to_S8_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Get dst aligned to 16 bytes */ for (i = cvt->len_cvt / sizeof (float); i && (((size_t) dst) & 15); --i, ++src, ++dst) { - *dst = (Sint8) (*src * 127.0f); + const float sample = *src; + if (sample >= 1.0f) { + *dst = 127; + } else if (sample <= -1.0f) { + *dst = -128; + } else { + *dst = (Sint8)(sample * 127.0f); + } } SDL_assert(!i || ((((size_t) dst) & 15) == 0)); @@ -582,13 +577,15 @@ SDL_Convert_F32_to_S8_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Make sure src is aligned too. */ if ((((size_t) src) & 15) == 0) { /* Aligned! Do SSE blocks as long as we have 16 bytes available. */ + const __m128 one = _mm_set1_ps(1.0f); + const __m128 negone = _mm_set1_ps(-1.0f); const __m128 mulby127 = _mm_set1_ps(127.0f); __m128i *mmdst = (__m128i *) dst; while (i >= 16) { /* 16 * float32 */ - const __m128i ints1 = _mm_cvtps_epi32(_mm_mul_ps(_mm_load_ps(src), mulby127)); /* load 4 floats, convert to sint32 */ - const __m128i ints2 = _mm_cvtps_epi32(_mm_mul_ps(_mm_load_ps(src+4), mulby127)); /* load 4 floats, convert to sint32 */ - const __m128i ints3 = _mm_cvtps_epi32(_mm_mul_ps(_mm_load_ps(src+8), mulby127)); /* load 4 floats, convert to sint32 */ - const __m128i ints4 = _mm_cvtps_epi32(_mm_mul_ps(_mm_load_ps(src+12), mulby127)); /* load 4 floats, convert to sint32 */ + const __m128i ints1 = _mm_cvtps_epi32(_mm_mul_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src)), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ + const __m128i ints2 = _mm_cvtps_epi32(_mm_mul_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src+4)), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ + const __m128i ints3 = _mm_cvtps_epi32(_mm_mul_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src+8)), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ + const __m128i ints4 = _mm_cvtps_epi32(_mm_mul_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src+12)), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ _mm_store_si128(mmdst, _mm_packs_epi16(_mm_packs_epi32(ints1, ints2), _mm_packs_epi32(ints3, ints4))); /* pack down, store out. */ i -= 16; src += 16; mmdst++; } @@ -597,7 +594,14 @@ SDL_Convert_F32_to_S8_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Finish off any leftovers with scalar operations. */ while (i) { - *dst = (Sint8) (*src * 127.0f); + const float sample = *src; + if (sample >= 1.0f) { + *dst = 127; + } else if (sample <= -1.0f) { + *dst = -128; + } else { + *dst = (Sint8)(sample * 127.0f); + } i--; src++; dst++; } @@ -618,7 +622,14 @@ SDL_Convert_F32_to_U8_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Get dst aligned to 16 bytes */ for (i = cvt->len_cvt / sizeof (float); i && (((size_t) dst) & 15); --i, ++src, ++dst) { - *dst = (Uint8) ((*src + 1.0f) * 127.0f); + const float sample = *src; + if (sample >= 1.0f) { + *dst = 255; + } else if (sample <= -1.0f) { + *dst = 0; + } else { + *dst = (Uint8)((sample + 1.0f) * 127.0f); + } } SDL_assert(!i || ((((size_t) dst) & 15) == 0)); @@ -626,14 +637,15 @@ SDL_Convert_F32_to_U8_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Make sure src is aligned too. */ if ((((size_t) src) & 15) == 0) { /* Aligned! Do SSE blocks as long as we have 16 bytes available. */ - const __m128 add1 = _mm_set1_ps(1.0f); + const __m128 one = _mm_set1_ps(1.0f); + const __m128 negone = _mm_set1_ps(-1.0f); const __m128 mulby127 = _mm_set1_ps(127.0f); __m128i *mmdst = (__m128i *) dst; while (i >= 16) { /* 16 * float32 */ - const __m128i ints1 = _mm_cvtps_epi32(_mm_mul_ps(_mm_add_ps(_mm_load_ps(src), add1), mulby127)); /* load 4 floats, convert to sint32 */ - const __m128i ints2 = _mm_cvtps_epi32(_mm_mul_ps(_mm_add_ps(_mm_load_ps(src+4), add1), mulby127)); /* load 4 floats, convert to sint32 */ - const __m128i ints3 = _mm_cvtps_epi32(_mm_mul_ps(_mm_add_ps(_mm_load_ps(src+8), add1), mulby127)); /* load 4 floats, convert to sint32 */ - const __m128i ints4 = _mm_cvtps_epi32(_mm_mul_ps(_mm_add_ps(_mm_load_ps(src+12), add1), mulby127)); /* load 4 floats, convert to sint32 */ + const __m128i ints1 = _mm_cvtps_epi32(_mm_mul_ps(_mm_add_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src)), one), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ + const __m128i ints2 = _mm_cvtps_epi32(_mm_mul_ps(_mm_add_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src+4)), one), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ + const __m128i ints3 = _mm_cvtps_epi32(_mm_mul_ps(_mm_add_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src+8)), one), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ + const __m128i ints4 = _mm_cvtps_epi32(_mm_mul_ps(_mm_add_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src+12)), one), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ _mm_store_si128(mmdst, _mm_packus_epi16(_mm_packs_epi32(ints1, ints2), _mm_packs_epi32(ints3, ints4))); /* pack down, store out. */ i -= 16; src += 16; mmdst++; } @@ -642,7 +654,14 @@ SDL_Convert_F32_to_U8_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Finish off any leftovers with scalar operations. */ while (i) { - *dst = (Uint8) ((*src + 1.0f) * 127.0f); + const float sample = *src; + if (sample >= 1.0f) { + *dst = 255; + } else if (sample <= -1.0f) { + *dst = 0; + } else { + *dst = (Uint8)((sample + 1.0f) * 127.0f); + } i--; src++; dst++; } @@ -663,7 +682,14 @@ SDL_Convert_F32_to_S16_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Get dst aligned to 16 bytes */ for (i = cvt->len_cvt / sizeof (float); i && (((size_t) dst) & 15); --i, ++src, ++dst) { - *dst = (Sint16) (*src * 32767.0f); + const float sample = *src; + if (sample >= 1.0f) { + *dst = 32767; + } else if (sample <= -1.0f) { + *dst = -32768; + } else { + *dst = (Sint16)(sample * 32767.0f); + } } SDL_assert(!i || ((((size_t) dst) & 15) == 0)); @@ -671,11 +697,13 @@ SDL_Convert_F32_to_S16_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Make sure src is aligned too. */ if ((((size_t) src) & 15) == 0) { /* Aligned! Do SSE blocks as long as we have 16 bytes available. */ + const __m128 one = _mm_set1_ps(1.0f); + const __m128 negone = _mm_set1_ps(-1.0f); const __m128 mulby32767 = _mm_set1_ps(32767.0f); __m128i *mmdst = (__m128i *) dst; while (i >= 8) { /* 8 * float32 */ - const __m128i ints1 = _mm_cvtps_epi32(_mm_mul_ps(_mm_load_ps(src), mulby32767)); /* load 4 floats, convert to sint32 */ - const __m128i ints2 = _mm_cvtps_epi32(_mm_mul_ps(_mm_load_ps(src+4), mulby32767)); /* load 4 floats, convert to sint32 */ + const __m128i ints1 = _mm_cvtps_epi32(_mm_mul_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src)), one), mulby32767)); /* load 4 floats, clamp, convert to sint32 */ + const __m128i ints2 = _mm_cvtps_epi32(_mm_mul_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src+4)), one), mulby32767)); /* load 4 floats, clamp, convert to sint32 */ _mm_store_si128(mmdst, _mm_packs_epi32(ints1, ints2)); /* pack to sint16, store out. */ i -= 8; src += 8; mmdst++; } @@ -684,7 +712,14 @@ SDL_Convert_F32_to_S16_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Finish off any leftovers with scalar operations. */ while (i) { - *dst = (Sint16) (*src * 32767.0f); + const float sample = *src; + if (sample >= 1.0f) { + *dst = 32767; + } else if (sample <= -1.0f) { + *dst = -32768; + } else { + *dst = (Sint16)(sample * 32767.0f); + } i--; src++; dst++; } @@ -705,7 +740,14 @@ SDL_Convert_F32_to_U16_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Get dst aligned to 16 bytes */ for (i = cvt->len_cvt / sizeof (float); i && (((size_t) dst) & 15); --i, ++src, ++dst) { - *dst = (Uint16) ((*src + 1.0f) * 32767.0f); + const float sample = *src; + if (sample >= 1.0f) { + *dst = 65535; + } else if (sample <= -1.0f) { + *dst = 0; + } else { + *dst = (Uint16)((sample + 1.0f) * 32767.0f); + } } SDL_assert(!i || ((((size_t) dst) & 15) == 0)); @@ -722,10 +764,12 @@ SDL_Convert_F32_to_U16_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) though it looks like dark magic. */ const __m128 mulby32767 = _mm_set1_ps(32767.0f); const __m128i topbit = _mm_set1_epi16(-32768); + const __m128 one = _mm_set1_ps(1.0f); + const __m128 negone = _mm_set1_ps(-1.0f); __m128i *mmdst = (__m128i *) dst; while (i >= 8) { /* 8 * float32 */ - const __m128i ints1 = _mm_cvtps_epi32(_mm_mul_ps(_mm_load_ps(src), mulby32767)); /* load 4 floats, convert to sint32 */ - const __m128i ints2 = _mm_cvtps_epi32(_mm_mul_ps(_mm_load_ps(src+4), mulby32767)); /* load 4 floats, convert to sint32 */ + const __m128i ints1 = _mm_cvtps_epi32(_mm_mul_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src)), one), mulby32767)); /* load 4 floats, clamp, convert to sint32 */ + const __m128i ints2 = _mm_cvtps_epi32(_mm_mul_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src+4)), one), mulby32767)); /* load 4 floats, clamp, convert to sint32 */ _mm_store_si128(mmdst, _mm_xor_si128(_mm_packs_epi32(ints1, ints2), topbit)); /* pack to sint16, xor top bit, store out. */ i -= 8; src += 8; mmdst++; } @@ -734,7 +778,14 @@ SDL_Convert_F32_to_U16_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Finish off any leftovers with scalar operations. */ while (i) { - *dst = (Uint16) ((*src + 1.0f) * 32767.0f); + const float sample = *src; + if (sample >= 1.0f) { + *dst = 65535; + } else if (sample <= -1.0f) { + *dst = 0; + } else { + *dst = (Uint16)((sample + 1.0f) * 32767.0f); + } i--; src++; dst++; } @@ -755,7 +806,14 @@ SDL_Convert_F32_to_S32_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Get dst aligned to 16 bytes */ for (i = cvt->len_cvt / sizeof (float); i && (((size_t) dst) & 15); --i, ++src, ++dst) { - *dst = (Sint32) (((double) *src) * 2147483647.0); + const float sample = *src; + if (sample >= 1.0f) { + *dst = 2147483647; + } else if (sample <= -1.0f) { + *dst = (Sint32) -2147483648LL; + } else { + *dst = ((Sint32)(sample * 8388607.0f)) << 8; + } } SDL_assert(!i || ((((size_t) dst) & 15) == 0)); @@ -763,14 +821,12 @@ SDL_Convert_F32_to_S32_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) { /* Aligned! Do SSE blocks as long as we have 16 bytes available. */ - const __m128d mulby2147483647 = _mm_set1_pd(2147483647.0); + const __m128 one = _mm_set1_ps(1.0f); + const __m128 negone = _mm_set1_ps(-1.0f); + const __m128 mulby8388607 = _mm_set1_ps(8388607.0f); __m128i *mmdst = (__m128i *) dst; while (i >= 4) { /* 4 * float32 */ - const __m128 floats = _mm_load_ps(src); - /* bitshift the whole register over, so _mm_cvtps_pd can read the top floats in the bottom of the vector. */ - const __m128d doubles1 = _mm_mul_pd(_mm_cvtps_pd(_mm_castsi128_ps(_mm_srli_si128(_mm_castps_si128(floats), 8))), mulby2147483647); - const __m128d doubles2 = _mm_mul_pd(_mm_cvtps_pd(floats), mulby2147483647); - _mm_store_si128(mmdst, _mm_or_si128(_mm_slli_si128(_mm_cvtpd_epi32(doubles1), 8), _mm_cvtpd_epi32(doubles2))); + _mm_store_si128(mmdst, _mm_slli_epi32(_mm_cvtps_epi32(_mm_mul_ps(_mm_min_ps(_mm_max_ps(negone, _mm_load_ps(src)), one), mulby8388607)), 8)); /* load 4 floats, clamp, convert to sint32 */ i -= 4; src += 4; mmdst++; } dst = (Sint32 *) mmdst; @@ -778,7 +834,14 @@ SDL_Convert_F32_to_S32_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) /* Finish off any leftovers with scalar operations. */ while (i) { - *dst = (Sint32) (((double) *src) * 2147483647.0); + const float sample = *src; + if (sample >= 1.0f) { + *dst = 2147483647; + } else if (sample <= -1.0f) { + *dst = (Sint32) -2147483648LL; + } else { + *dst = ((Sint32)(sample * 8388607.0f)) << 8; + } i--; src++; dst++; } @@ -789,6 +852,538 @@ SDL_Convert_F32_to_S32_SSE2(SDL_AudioCVT *cvt, SDL_AudioFormat format) #endif +#if HAVE_NEON_INTRINSICS +static void SDLCALL +SDL_Convert_S8_to_F32_NEON(SDL_AudioCVT *cvt, SDL_AudioFormat format) +{ + const Sint8 *src = ((const Sint8 *) (cvt->buf + cvt->len_cvt)) - 1; + float *dst = ((float *) (cvt->buf + cvt->len_cvt * 4)) - 1; + int i; + + LOG_DEBUG_CONVERT("AUDIO_S8", "AUDIO_F32 (using NEON)"); + + /* Get dst aligned to 16 bytes (since buffer is growing, we don't have to worry about overreading from src) */ + for (i = cvt->len_cvt; i && (((size_t) (dst-15)) & 15); --i, --src, --dst) { + *dst = ((float) *src) * DIVBY128; + } + + src -= 15; dst -= 15; /* adjust to read NEON blocks from the start. */ + SDL_assert(!i || ((((size_t) dst) & 15) == 0)); + + /* Make sure src is aligned too. */ + if ((((size_t) src) & 15) == 0) { + /* Aligned! Do NEON blocks as long as we have 16 bytes available. */ + const int8_t *mmsrc = (const int8_t *) src; + const float32x4_t divby128 = vdupq_n_f32(DIVBY128); + while (i >= 16) { /* 16 * 8-bit */ + const int8x16_t bytes = vld1q_s8(mmsrc); /* get 16 sint8 into a NEON register. */ + const int16x8_t int16hi = vmovl_s8(vget_high_s8(bytes)); /* convert top 8 bytes to 8 int16 */ + const int16x8_t int16lo = vmovl_s8(vget_low_s8(bytes)); /* convert bottom 8 bytes to 8 int16 */ + /* split int16 to two int32, then convert to float, then multiply to normalize, store. */ + vst1q_f32(dst, vmulq_f32(vcvtq_f32_s32(vmovl_s16(vget_high_s16(int16hi))), divby128)); + vst1q_f32(dst+4, vmulq_f32(vcvtq_f32_s32(vmovl_s16(vget_low_s16(int16hi))), divby128)); + vst1q_f32(dst+8, vmulq_f32(vcvtq_f32_s32(vmovl_s16(vget_high_s16(int16lo))), divby128)); + vst1q_f32(dst+12, vmulq_f32(vcvtq_f32_s32(vmovl_s16(vget_low_s16(int16lo))), divby128)); + i -= 16; mmsrc -= 16; dst -= 16; + } + + src = (const Sint8 *) mmsrc; + } + + src += 15; dst += 15; /* adjust for any scalar finishing. */ + + /* Finish off any leftovers with scalar operations. */ + while (i) { + *dst = ((float) *src) * DIVBY128; + i--; src--; dst--; + } + + cvt->len_cvt *= 4; + if (cvt->filters[++cvt->filter_index]) { + cvt->filters[cvt->filter_index](cvt, AUDIO_F32SYS); + } +} + +static void SDLCALL +SDL_Convert_U8_to_F32_NEON(SDL_AudioCVT *cvt, SDL_AudioFormat format) +{ + const Uint8 *src = ((const Uint8 *) (cvt->buf + cvt->len_cvt)) - 1; + float *dst = ((float *) (cvt->buf + cvt->len_cvt * 4)) - 1; + int i; + + LOG_DEBUG_CONVERT("AUDIO_U8", "AUDIO_F32 (using NEON)"); + + /* Get dst aligned to 16 bytes (since buffer is growing, we don't have to worry about overreading from src) */ + for (i = cvt->len_cvt; i && (((size_t) (dst-15)) & 15); --i, --src, --dst) { + *dst = (((float) *src) * DIVBY128) - 1.0f; + } + + src -= 15; dst -= 15; /* adjust to read NEON blocks from the start. */ + SDL_assert(!i || ((((size_t) dst) & 15) == 0)); + + /* Make sure src is aligned too. */ + if ((((size_t) src) & 15) == 0) { + /* Aligned! Do NEON blocks as long as we have 16 bytes available. */ + const uint8_t *mmsrc = (const uint8_t *) src; + const float32x4_t divby128 = vdupq_n_f32(DIVBY128); + const float32x4_t one = vdupq_n_f32(1.0f); + while (i >= 16) { /* 16 * 8-bit */ + const uint8x16_t bytes = vld1q_u8(mmsrc); /* get 16 uint8 into a NEON register. */ + const uint16x8_t uint16hi = vmovl_u8(vget_high_u8(bytes)); /* convert top 8 bytes to 8 uint16 */ + const uint16x8_t uint16lo = vmovl_u8(vget_low_u8(bytes)); /* convert bottom 8 bytes to 8 uint16 */ + /* split uint16 to two uint32, then convert to float, then multiply to normalize, subtract to adjust for sign, store. */ + vst1q_f32(dst, vmlsq_f32(vcvtq_f32_u32(vmovl_u16(vget_high_u16(uint16hi))), divby128, one)); + vst1q_f32(dst+4, vmlsq_f32(vcvtq_f32_u32(vmovl_u16(vget_low_u16(uint16hi))), divby128, one)); + vst1q_f32(dst+8, vmlsq_f32(vcvtq_f32_u32(vmovl_u16(vget_high_u16(uint16lo))), divby128, one)); + vst1q_f32(dst+12, vmlsq_f32(vcvtq_f32_u32(vmovl_u16(vget_low_u16(uint16lo))), divby128, one)); + i -= 16; mmsrc -= 16; dst -= 16; + } + + src = (const Uint8 *) mmsrc; + } + + src += 15; dst += 15; /* adjust for any scalar finishing. */ + + /* Finish off any leftovers with scalar operations. */ + while (i) { + *dst = (((float) *src) * DIVBY128) - 1.0f; + i--; src--; dst--; + } + + cvt->len_cvt *= 4; + if (cvt->filters[++cvt->filter_index]) { + cvt->filters[cvt->filter_index](cvt, AUDIO_F32SYS); + } +} + +static void SDLCALL +SDL_Convert_S16_to_F32_NEON(SDL_AudioCVT *cvt, SDL_AudioFormat format) +{ + const Sint16 *src = ((const Sint16 *) (cvt->buf + cvt->len_cvt)) - 1; + float *dst = ((float *) (cvt->buf + cvt->len_cvt * 2)) - 1; + int i; + + LOG_DEBUG_CONVERT("AUDIO_S16", "AUDIO_F32 (using NEON)"); + + /* Get dst aligned to 16 bytes (since buffer is growing, we don't have to worry about overreading from src) */ + for (i = cvt->len_cvt / sizeof (Sint16); i && (((size_t) (dst-7)) & 15); --i, --src, --dst) { + *dst = ((float) *src) * DIVBY32768; + } + + src -= 7; dst -= 7; /* adjust to read NEON blocks from the start. */ + SDL_assert(!i || ((((size_t) dst) & 15) == 0)); + + /* Make sure src is aligned too. */ + if ((((size_t) src) & 15) == 0) { + /* Aligned! Do NEON blocks as long as we have 16 bytes available. */ + const float32x4_t divby32768 = vdupq_n_f32(DIVBY32768); + while (i >= 8) { /* 8 * 16-bit */ + const int16x8_t ints = vld1q_s16((int16_t const *) src); /* get 8 sint16 into a NEON register. */ + /* split int16 to two int32, then convert to float, then multiply to normalize, store. */ + vst1q_f32(dst, vmulq_f32(vcvtq_f32_s32(vmovl_s16(vget_low_s16(ints))), divby32768)); + vst1q_f32(dst+4, vmulq_f32(vcvtq_f32_s32(vmovl_s16(vget_high_s16(ints))), divby32768)); + i -= 8; src -= 8; dst -= 8; + } + } + + src += 7; dst += 7; /* adjust for any scalar finishing. */ + + /* Finish off any leftovers with scalar operations. */ + while (i) { + *dst = ((float) *src) * DIVBY32768; + i--; src--; dst--; + } + + cvt->len_cvt *= 2; + if (cvt->filters[++cvt->filter_index]) { + cvt->filters[cvt->filter_index](cvt, AUDIO_F32SYS); + } +} + +static void SDLCALL +SDL_Convert_U16_to_F32_NEON(SDL_AudioCVT *cvt, SDL_AudioFormat format) +{ + const Uint16 *src = ((const Uint16 *) (cvt->buf + cvt->len_cvt)) - 1; + float *dst = ((float *) (cvt->buf + cvt->len_cvt * 2)) - 1; + int i; + + LOG_DEBUG_CONVERT("AUDIO_U16", "AUDIO_F32 (using NEON)"); + + /* Get dst aligned to 16 bytes (since buffer is growing, we don't have to worry about overreading from src) */ + for (i = cvt->len_cvt / sizeof (Sint16); i && (((size_t) (dst-7)) & 15); --i, --src, --dst) { + *dst = (((float) *src) * DIVBY32768) - 1.0f; + } + + src -= 7; dst -= 7; /* adjust to read NEON blocks from the start. */ + SDL_assert(!i || ((((size_t) dst) & 15) == 0)); + + /* Make sure src is aligned too. */ + if ((((size_t) src) & 15) == 0) { + /* Aligned! Do NEON blocks as long as we have 16 bytes available. */ + const float32x4_t divby32768 = vdupq_n_f32(DIVBY32768); + const float32x4_t one = vdupq_n_f32(1.0f); + while (i >= 8) { /* 8 * 16-bit */ + const uint16x8_t uints = vld1q_u16((uint16_t const *) src); /* get 8 uint16 into a NEON register. */ + /* split uint16 to two int32, then convert to float, then multiply to normalize, subtract for sign, store. */ + vst1q_f32(dst, vmlsq_f32(one, vcvtq_f32_u32(vmovl_u16(vget_low_u16(uints))), divby32768)); + vst1q_f32(dst+4, vmlsq_f32(one, vcvtq_f32_u32(vmovl_u16(vget_high_u16(uints))), divby32768)); + i -= 8; src -= 8; dst -= 8; + } + } + + src += 7; dst += 7; /* adjust for any scalar finishing. */ + + /* Finish off any leftovers with scalar operations. */ + while (i) { + *dst = (((float) *src) * DIVBY32768) - 1.0f; + i--; src--; dst--; + } + + cvt->len_cvt *= 2; + if (cvt->filters[++cvt->filter_index]) { + cvt->filters[cvt->filter_index](cvt, AUDIO_F32SYS); + } +} + +static void SDLCALL +SDL_Convert_S32_to_F32_NEON(SDL_AudioCVT *cvt, SDL_AudioFormat format) +{ + const Sint32 *src = (const Sint32 *) cvt->buf; + float *dst = (float *) cvt->buf; + int i; + + LOG_DEBUG_CONVERT("AUDIO_S32", "AUDIO_F32 (using NEON)"); + + /* Get dst aligned to 16 bytes */ + for (i = cvt->len_cvt / sizeof (Sint32); i && (((size_t) dst) & 15); --i, ++src, ++dst) { + *dst = ((float) (*src>>8)) * DIVBY8388607; + } + + SDL_assert(!i || ((((size_t) dst) & 15) == 0)); + SDL_assert(!i || ((((size_t) src) & 15) == 0)); + + { + /* Aligned! Do NEON blocks as long as we have 16 bytes available. */ + const float32x4_t divby8388607 = vdupq_n_f32(DIVBY8388607); + const int32_t *mmsrc = (const int32_t *) src; + while (i >= 4) { /* 4 * sint32 */ + /* shift out lowest bits so int fits in a float32. Small precision loss, but much faster. */ + vst1q_f32(dst, vmulq_f32(vcvtq_f32_s32(vshrq_n_s32(vld1q_s32(mmsrc), 8)), divby8388607)); + i -= 4; mmsrc += 4; dst += 4; + } + src = (const Sint32 *) mmsrc; + } + + /* Finish off any leftovers with scalar operations. */ + while (i) { + *dst = ((float) (*src>>8)) * DIVBY8388607; + i--; src++; dst++; + } + + if (cvt->filters[++cvt->filter_index]) { + cvt->filters[cvt->filter_index](cvt, AUDIO_F32SYS); + } +} + +static void SDLCALL +SDL_Convert_F32_to_S8_NEON(SDL_AudioCVT *cvt, SDL_AudioFormat format) +{ + const float *src = (const float *) cvt->buf; + Sint8 *dst = (Sint8 *) cvt->buf; + int i; + + LOG_DEBUG_CONVERT("AUDIO_F32", "AUDIO_S8 (using NEON)"); + + /* Get dst aligned to 16 bytes */ + for (i = cvt->len_cvt / sizeof (float); i && (((size_t) dst) & 15); --i, ++src, ++dst) { + const float sample = *src; + if (sample >= 1.0f) { + *dst = 127; + } else if (sample <= -1.0f) { + *dst = -128; + } else { + *dst = (Sint8)(sample * 127.0f); + } + } + + SDL_assert(!i || ((((size_t) dst) & 15) == 0)); + + /* Make sure src is aligned too. */ + if ((((size_t) src) & 15) == 0) { + /* Aligned! Do NEON blocks as long as we have 16 bytes available. */ + const float32x4_t one = vdupq_n_f32(1.0f); + const float32x4_t negone = vdupq_n_f32(-1.0f); + const float32x4_t mulby127 = vdupq_n_f32(127.0f); + int8_t *mmdst = (int8_t *) dst; + while (i >= 16) { /* 16 * float32 */ + const int32x4_t ints1 = vcvtq_s32_f32(vmulq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src)), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ + const int32x4_t ints2 = vcvtq_s32_f32(vmulq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src+4)), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ + const int32x4_t ints3 = vcvtq_s32_f32(vmulq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src+8)), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ + const int32x4_t ints4 = vcvtq_s32_f32(vmulq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src+12)), one), mulby127)); /* load 4 floats, clamp, convert to sint32 */ + const int8x8_t i8lo = vmovn_s16(vcombine_s16(vmovn_s32(ints1), vmovn_s32(ints2))); /* narrow to sint16, combine, narrow to sint8 */ + const int8x8_t i8hi = vmovn_s16(vcombine_s16(vmovn_s32(ints3), vmovn_s32(ints4))); /* narrow to sint16, combine, narrow to sint8 */ + vst1q_s8(mmdst, vcombine_s8(i8lo, i8hi)); /* combine to int8x16_t, store out */ + i -= 16; src += 16; mmdst += 16; + } + dst = (Sint8 *) mmdst; + } + + /* Finish off any leftovers with scalar operations. */ + while (i) { + const float sample = *src; + if (sample >= 1.0f) { + *dst = 127; + } else if (sample <= -1.0f) { + *dst = -128; + } else { + *dst = (Sint8)(sample * 127.0f); + } + i--; src++; dst++; + } + + cvt->len_cvt /= 4; + if (cvt->filters[++cvt->filter_index]) { + cvt->filters[cvt->filter_index](cvt, AUDIO_S8); + } +} + +static void SDLCALL +SDL_Convert_F32_to_U8_NEON(SDL_AudioCVT *cvt, SDL_AudioFormat format) +{ + const float *src = (const float *) cvt->buf; + Uint8 *dst = (Uint8 *) cvt->buf; + int i; + + LOG_DEBUG_CONVERT("AUDIO_F32", "AUDIO_U8 (using NEON)"); + + /* Get dst aligned to 16 bytes */ + for (i = cvt->len_cvt / sizeof (float); i && (((size_t) dst) & 15); --i, ++src, ++dst) { + const float sample = *src; + if (sample >= 1.0f) { + *dst = 255; + } else if (sample <= -1.0f) { + *dst = 0; + } else { + *dst = (Uint8)((sample + 1.0f) * 127.0f); + } + } + + SDL_assert(!i || ((((size_t) dst) & 15) == 0)); + + /* Make sure src is aligned too. */ + if ((((size_t) src) & 15) == 0) { + /* Aligned! Do NEON blocks as long as we have 16 bytes available. */ + const float32x4_t one = vdupq_n_f32(1.0f); + const float32x4_t negone = vdupq_n_f32(-1.0f); + const float32x4_t mulby127 = vdupq_n_f32(127.0f); + uint8_t *mmdst = (uint8_t *) dst; + while (i >= 16) { /* 16 * float32 */ + const uint32x4_t uints1 = vcvtq_u32_f32(vmulq_f32(vaddq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src)), one), one), mulby127)); /* load 4 floats, clamp, convert to uint32 */ + const uint32x4_t uints2 = vcvtq_u32_f32(vmulq_f32(vaddq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src+4)), one), one), mulby127)); /* load 4 floats, clamp, convert to uint32 */ + const uint32x4_t uints3 = vcvtq_u32_f32(vmulq_f32(vaddq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src+8)), one), one), mulby127)); /* load 4 floats, clamp, convert to uint32 */ + const uint32x4_t uints4 = vcvtq_u32_f32(vmulq_f32(vaddq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src+12)), one), one), mulby127)); /* load 4 floats, clamp, convert to uint32 */ + const uint8x8_t ui8lo = vmovn_u16(vcombine_u16(vmovn_u32(uints1), vmovn_u32(uints2))); /* narrow to uint16, combine, narrow to uint8 */ + const uint8x8_t ui8hi = vmovn_u16(vcombine_u16(vmovn_u32(uints3), vmovn_u32(uints4))); /* narrow to uint16, combine, narrow to uint8 */ + vst1q_u8(mmdst, vcombine_u8(ui8lo, ui8hi)); /* combine to uint8x16_t, store out */ + i -= 16; src += 16; mmdst += 16; + } + + dst = (Uint8 *) mmdst; + } + + /* Finish off any leftovers with scalar operations. */ + while (i) { + const float sample = *src; + if (sample >= 1.0f) { + *dst = 255; + } else if (sample <= -1.0f) { + *dst = 0; + } else { + *dst = (Uint8)((sample + 1.0f) * 127.0f); + } + i--; src++; dst++; + } + + cvt->len_cvt /= 4; + if (cvt->filters[++cvt->filter_index]) { + cvt->filters[cvt->filter_index](cvt, AUDIO_U8); + } +} + +static void SDLCALL +SDL_Convert_F32_to_S16_NEON(SDL_AudioCVT *cvt, SDL_AudioFormat format) +{ + const float *src = (const float *) cvt->buf; + Sint16 *dst = (Sint16 *) cvt->buf; + int i; + + LOG_DEBUG_CONVERT("AUDIO_F32", "AUDIO_S16 (using NEON)"); + + /* Get dst aligned to 16 bytes */ + for (i = cvt->len_cvt / sizeof (float); i && (((size_t) dst) & 15); --i, ++src, ++dst) { + const float sample = *src; + if (sample >= 1.0f) { + *dst = 32767; + } else if (sample <= -1.0f) { + *dst = -32768; + } else { + *dst = (Sint16)(sample * 32767.0f); + } + } + + SDL_assert(!i || ((((size_t) dst) & 15) == 0)); + + /* Make sure src is aligned too. */ + if ((((size_t) src) & 15) == 0) { + /* Aligned! Do NEON blocks as long as we have 16 bytes available. */ + const float32x4_t one = vdupq_n_f32(1.0f); + const float32x4_t negone = vdupq_n_f32(-1.0f); + const float32x4_t mulby32767 = vdupq_n_f32(32767.0f); + int16_t *mmdst = (int16_t *) dst; + while (i >= 8) { /* 8 * float32 */ + const int32x4_t ints1 = vcvtq_s32_f32(vmulq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src)), one), mulby32767)); /* load 4 floats, clamp, convert to sint32 */ + const int32x4_t ints2 = vcvtq_s32_f32(vmulq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src+4)), one), mulby32767)); /* load 4 floats, clamp, convert to sint32 */ + vst1q_s16(mmdst, vcombine_s16(vmovn_s32(ints1), vmovn_s32(ints2))); /* narrow to sint16, combine, store out. */ + i -= 8; src += 8; mmdst += 8; + } + dst = (Sint16 *) mmdst; + } + + /* Finish off any leftovers with scalar operations. */ + while (i) { + const float sample = *src; + if (sample >= 1.0f) { + *dst = 32767; + } else if (sample <= -1.0f) { + *dst = -32768; + } else { + *dst = (Sint16)(sample * 32767.0f); + } + i--; src++; dst++; + } + + cvt->len_cvt /= 2; + if (cvt->filters[++cvt->filter_index]) { + cvt->filters[cvt->filter_index](cvt, AUDIO_S16SYS); + } +} + +static void SDLCALL +SDL_Convert_F32_to_U16_NEON(SDL_AudioCVT *cvt, SDL_AudioFormat format) +{ + const float *src = (const float *) cvt->buf; + Uint16 *dst = (Uint16 *) cvt->buf; + int i; + + LOG_DEBUG_CONVERT("AUDIO_F32", "AUDIO_U16 (using NEON)"); + + /* Get dst aligned to 16 bytes */ + for (i = cvt->len_cvt / sizeof (float); i && (((size_t) dst) & 15); --i, ++src, ++dst) { + const float sample = *src; + if (sample >= 1.0f) { + *dst = 65535; + } else if (sample <= -1.0f) { + *dst = 0; + } else { + *dst = (Uint16)((sample + 1.0f) * 32767.0f); + } + } + + SDL_assert(!i || ((((size_t) dst) & 15) == 0)); + + /* Make sure src is aligned too. */ + if ((((size_t) src) & 15) == 0) { + /* Aligned! Do NEON blocks as long as we have 16 bytes available. */ + const float32x4_t one = vdupq_n_f32(1.0f); + const float32x4_t negone = vdupq_n_f32(-1.0f); + const float32x4_t mulby32767 = vdupq_n_f32(32767.0f); + uint16_t *mmdst = (uint16_t *) dst; + while (i >= 8) { /* 8 * float32 */ + const uint32x4_t uints1 = vcvtq_u32_f32(vmulq_f32(vaddq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src)), one), one), mulby32767)); /* load 4 floats, clamp, convert to uint32 */ + const uint32x4_t uints2 = vcvtq_u32_f32(vmulq_f32(vaddq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src+4)), one), one), mulby32767)); /* load 4 floats, clamp, convert to uint32 */ + vst1q_u16(mmdst, vcombine_u16(vmovn_u32(uints1), vmovn_u32(uints2))); /* narrow to uint16, combine, store out. */ + i -= 8; src += 8; mmdst += 8; + } + dst = (Uint16 *) mmdst; + } + + /* Finish off any leftovers with scalar operations. */ + while (i) { + const float sample = *src; + if (sample >= 1.0f) { + *dst = 65535; + } else if (sample <= -1.0f) { + *dst = 0; + } else { + *dst = (Uint16)((sample + 1.0f) * 32767.0f); + } + i--; src++; dst++; + } + + cvt->len_cvt /= 2; + if (cvt->filters[++cvt->filter_index]) { + cvt->filters[cvt->filter_index](cvt, AUDIO_U16SYS); + } +} + +static void SDLCALL +SDL_Convert_F32_to_S32_NEON(SDL_AudioCVT *cvt, SDL_AudioFormat format) +{ + const float *src = (const float *) cvt->buf; + Sint32 *dst = (Sint32 *) cvt->buf; + int i; + + LOG_DEBUG_CONVERT("AUDIO_F32", "AUDIO_S32 (using NEON)"); + + /* Get dst aligned to 16 bytes */ + for (i = cvt->len_cvt / sizeof (float); i && (((size_t) dst) & 15); --i, ++src, ++dst) { + const float sample = *src; + if (sample >= 1.0f) { + *dst = 2147483647; + } else if (sample <= -1.0f) { + *dst = -2147483648; + } else { + *dst = ((Sint32)(sample * 8388607.0f)) << 8; + } + } + + SDL_assert(!i || ((((size_t) dst) & 15) == 0)); + SDL_assert(!i || ((((size_t) src) & 15) == 0)); + + { + /* Aligned! Do NEON blocks as long as we have 16 bytes available. */ + const float32x4_t one = vdupq_n_f32(1.0f); + const float32x4_t negone = vdupq_n_f32(-1.0f); + const float32x4_t mulby8388607 = vdupq_n_f32(8388607.0f); + int32_t *mmdst = (int32_t *) dst; + while (i >= 4) { /* 4 * float32 */ + vst1q_s32(mmdst, vshlq_n_s32(vcvtq_s32_f32(vmulq_f32(vminq_f32(vmaxq_f32(negone, vld1q_f32(src)), one), mulby8388607)), 8)); + i -= 4; src += 4; mmdst += 4; + } + dst = (Sint32 *) mmdst; + } + + /* Finish off any leftovers with scalar operations. */ + while (i) { + const float sample = *src; + if (sample >= 1.0f) { + *dst = 2147483647; + } else if (sample <= -1.0f) { + *dst = -2147483648; + } else { + *dst = ((Sint32)(sample * 8388607.0f)) << 8; + } + i--; src++; dst++; + } + + if (cvt->filters[++cvt->filter_index]) { + cvt->filters[cvt->filter_index](cvt, AUDIO_S32SYS); + } +} +#endif + + + void SDL_ChooseAudioConverters(void) { static SDL_bool converters_chosen = SDL_FALSE; @@ -817,6 +1412,13 @@ void SDL_ChooseAudioConverters(void) } #endif +#if HAVE_NEON_INTRINSICS + if (SDL_HasNEON()) { + SET_CONVERTER_FUNCS(NEON); + return; + } +#endif + #if NEED_SCALAR_CONVERTER_FALLBACKS SET_CONVERTER_FUNCS(Scalar); #endif diff --git a/Engine/lib/sdl/src/audio/SDL_sysaudio.h b/Engine/lib/sdl/src/audio/SDL_sysaudio.h index f0e1f3dad..579dea5b4 100644 --- a/Engine/lib/sdl/src/audio/SDL_sysaudio.h +++ b/Engine/lib/sdl/src/audio/SDL_sysaudio.h @@ -98,8 +98,10 @@ typedef struct SDL_AudioDriverImpl typedef struct SDL_AudioDeviceItem { void *handle; + char *name; + char *original_name; + int dupenum; struct SDL_AudioDeviceItem *next; - char name[SDL_VARIABLE_LENGTH_ARRAY]; } SDL_AudioDeviceItem; diff --git a/Engine/lib/sdl/src/audio/alsa/SDL_alsa_audio.c b/Engine/lib/sdl/src/audio/alsa/SDL_alsa_audio.c index 2dba1ff46..eff192b7e 100644 --- a/Engine/lib/sdl/src/audio/alsa/SDL_alsa_audio.c +++ b/Engine/lib/sdl/src/audio/alsa/SDL_alsa_audio.c @@ -22,6 +22,10 @@ #if SDL_AUDIO_DRIVER_ALSA +#ifndef SDL_ALSA_NON_BLOCKING +#define SDL_ALSA_NON_BLOCKING 0 +#endif + /* Allow access to a raw mixing buffer */ #include @@ -90,6 +94,7 @@ static int (*ALSA_snd_pcm_reset)(snd_pcm_t *); static int (*ALSA_snd_device_name_hint) (int, const char *, void ***); static char* (*ALSA_snd_device_name_get_hint) (const void *, const char *); static int (*ALSA_snd_device_name_free_hint) (void **); +static snd_pcm_sframes_t (*ALSA_snd_pcm_avail)(snd_pcm_t *); #ifdef SND_CHMAP_API_VERSION static snd_pcm_chmap_t* (*ALSA_snd_pcm_get_chmap) (snd_pcm_t *); static int (*ALSA_snd_pcm_chmap_print) (const snd_pcm_chmap_t *map, size_t maxlen, char *buf); @@ -158,6 +163,7 @@ load_alsa_syms(void) SDL_ALSA_SYM(snd_device_name_hint); SDL_ALSA_SYM(snd_device_name_get_hint); SDL_ALSA_SYM(snd_device_name_free_hint); + SDL_ALSA_SYM(snd_pcm_avail); #ifdef SND_CHMAP_API_VERSION SDL_ALSA_SYM(snd_pcm_get_chmap); SDL_ALSA_SYM(snd_pcm_chmap_print); @@ -243,7 +249,24 @@ get_audio_device(void *handle, const int channels) static void ALSA_WaitDevice(_THIS) { - /* We're in blocking mode, so there's nothing to do here */ +#if SDL_ALSA_NON_BLOCKING + const snd_pcm_sframes_t needed = (snd_pcm_sframes_t) this->spec.samples; + while (SDL_AtomicGet(&this->enabled)) { + const snd_pcm_sframes_t rc = ALSA_snd_pcm_avail(this->hidden->pcm_handle); + if ((rc < 0) && (rc != -EAGAIN)) { + /* Hmm, not much we can do - abort */ + fprintf(stderr, "ALSA snd_pcm_avail failed (unrecoverable): %s\n", + ALSA_snd_strerror(rc)); + SDL_OpenedAudioDeviceDisconnected(this); + return; + } else if (rc < needed) { + const Uint32 delay = ((needed - (SDL_max(rc, 0))) * 1000) / this->spec.freq; + SDL_Delay(SDL_max(delay, 10)); + } else { + break; /* ready to go! */ + } + } +#endif } @@ -422,7 +445,7 @@ static void ALSA_CloseDevice(_THIS) { if (this->hidden->pcm_handle) { - /* Wait for the submitted audio to drain + /* Wait for the submitted audio to drain ALSA_snd_pcm_drop() can hang, so don't use that. */ Uint32 delay = ((this->spec.samples * 1000) / this->spec.freq) * 2; @@ -435,10 +458,32 @@ ALSA_CloseDevice(_THIS) } static int -ALSA_finalize_hardware(_THIS, snd_pcm_hw_params_t *hwparams, int override) +ALSA_set_buffer_size(_THIS, snd_pcm_hw_params_t *params) { int status; + snd_pcm_hw_params_t *hwparams; snd_pcm_uframes_t bufsize; + snd_pcm_uframes_t persize; + + /* Copy the hardware parameters for this setup */ + snd_pcm_hw_params_alloca(&hwparams); + ALSA_snd_pcm_hw_params_copy(hwparams, params); + + /* Prioritize matching the period size to the requested buffer size */ + persize = this->spec.samples; + status = ALSA_snd_pcm_hw_params_set_period_size_near( + this->hidden->pcm_handle, hwparams, &persize, NULL); + if ( status < 0 ) { + return(-1); + } + + /* Next try to restrict the parameters to having only two periods */ + bufsize = this->spec.samples * 2; + status = ALSA_snd_pcm_hw_params_set_buffer_size_near( + this->hidden->pcm_handle, hwparams, &bufsize); + if ( status < 0 ) { + return(-1); + } /* "set" the hardware with the desired parameters */ status = ALSA_snd_pcm_hw_params(this->hidden->pcm_handle, hwparams); @@ -446,24 +491,12 @@ ALSA_finalize_hardware(_THIS, snd_pcm_hw_params_t *hwparams, int override) return(-1); } - /* Get samples for the actual buffer size */ - status = ALSA_snd_pcm_hw_params_get_buffer_size(hwparams, &bufsize); - if ( status < 0 ) { - return(-1); - } - if ( !override && bufsize != this->spec.samples * 2 ) { - return(-1); - } - - /* !!! FIXME: Is this safe to do? */ - this->spec.samples = bufsize / 2; + this->spec.samples = persize; /* This is useful for debugging */ if ( SDL_getenv("SDL_AUDIO_ALSA_DEBUG") ) { - snd_pcm_uframes_t persize = 0; unsigned int periods = 0; - ALSA_snd_pcm_hw_params_get_period_size(hwparams, &persize, NULL); ALSA_snd_pcm_hw_params_get_periods(hwparams, &periods, NULL); fprintf(stderr, @@ -474,78 +507,6 @@ ALSA_finalize_hardware(_THIS, snd_pcm_hw_params_t *hwparams, int override) return(0); } -static int -ALSA_set_period_size(_THIS, snd_pcm_hw_params_t *params, int override) -{ - const char *env; - int status; - snd_pcm_hw_params_t *hwparams; - snd_pcm_uframes_t frames; - unsigned int periods; - - /* Copy the hardware parameters for this setup */ - snd_pcm_hw_params_alloca(&hwparams); - ALSA_snd_pcm_hw_params_copy(hwparams, params); - - if ( !override ) { - env = SDL_getenv("SDL_AUDIO_ALSA_SET_PERIOD_SIZE"); - if ( env ) { - override = SDL_atoi(env); - if ( override == 0 ) { - return(-1); - } - } - } - - frames = this->spec.samples; - status = ALSA_snd_pcm_hw_params_set_period_size_near( - this->hidden->pcm_handle, hwparams, &frames, NULL); - if ( status < 0 ) { - return(-1); - } - - periods = 2; - status = ALSA_snd_pcm_hw_params_set_periods_near( - this->hidden->pcm_handle, hwparams, &periods, NULL); - if ( status < 0 ) { - return(-1); - } - - return ALSA_finalize_hardware(this, hwparams, override); -} - -static int -ALSA_set_buffer_size(_THIS, snd_pcm_hw_params_t *params, int override) -{ - const char *env; - int status; - snd_pcm_hw_params_t *hwparams; - snd_pcm_uframes_t frames; - - /* Copy the hardware parameters for this setup */ - snd_pcm_hw_params_alloca(&hwparams); - ALSA_snd_pcm_hw_params_copy(hwparams, params); - - if ( !override ) { - env = SDL_getenv("SDL_AUDIO_ALSA_SET_BUFFER_SIZE"); - if ( env ) { - override = SDL_atoi(env); - if ( override == 0 ) { - return(-1); - } - } - } - - frames = this->spec.samples * 2; - status = ALSA_snd_pcm_hw_params_set_buffer_size_near( - this->hidden->pcm_handle, hwparams, &frames); - if ( status < 0 ) { - return(-1); - } - - return ALSA_finalize_hardware(this, hwparams, override); -} - static int ALSA_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) { @@ -692,14 +653,11 @@ ALSA_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) this->spec.freq = rate; /* Set the buffer size, in samples */ - if ( ALSA_set_period_size(this, hwparams, 0) < 0 && - ALSA_set_buffer_size(this, hwparams, 0) < 0 ) { - /* Failed to set desired buffer size, do the best you can... */ - status = ALSA_set_period_size(this, hwparams, 1); - if (status < 0) { - return SDL_SetError("Couldn't set hardware audio parameters: %s", ALSA_snd_strerror(status)); - } + status = ALSA_set_buffer_size(this, hwparams); + if (status < 0) { + return SDL_SetError("Couldn't set hardware audio parameters: %s", ALSA_snd_strerror(status)); } + /* Set the software parameters */ snd_pcm_sw_params_alloca(&swparams); status = ALSA_snd_pcm_sw_params_current(pcm_handle, swparams); @@ -737,9 +695,11 @@ ALSA_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) SDL_memset(this->hidden->mixbuf, this->spec.silence, this->hidden->mixlen); } + #if !SDL_ALSA_NON_BLOCKING if (!iscapture) { ALSA_snd_pcm_nonblock(pcm_handle, 0); } + #endif /* We're ready to rock and roll. :-) */ return 0; diff --git a/Engine/lib/sdl/src/audio/android/SDL_androidaudio.c b/Engine/lib/sdl/src/audio/android/SDL_androidaudio.c index 7a2542461..77a5f0da8 100644 --- a/Engine/lib/sdl/src/audio/android/SDL_androidaudio.c +++ b/Engine/lib/sdl/src/audio/android/SDL_androidaudio.c @@ -57,7 +57,9 @@ ANDROIDAUDIO_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) test_format = SDL_FirstAudioFormat(this->spec.format); while (test_format != 0) { /* no "UNKNOWN" constant */ - if ((test_format == AUDIO_U8) || (test_format == AUDIO_S16LSB)) { + if ((test_format == AUDIO_U8) || + (test_format == AUDIO_S16) || + (test_format == AUDIO_F32)) { this->spec.format = test_format; break; } @@ -69,25 +71,8 @@ ANDROIDAUDIO_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) return SDL_SetError("No compatible audio format!"); } - if (this->spec.channels > 1) { - this->spec.channels = 2; - } else { - this->spec.channels = 1; - } - - if (this->spec.freq < 8000) { - this->spec.freq = 8000; - } - if (this->spec.freq > 48000) { - this->spec.freq = 48000; - } - - /* TODO: pass in/return a (Java) device ID */ - this->spec.samples = Android_JNI_OpenAudioDevice(iscapture, this->spec.freq, this->spec.format == AUDIO_U8 ? 0 : 1, this->spec.channels, this->spec.samples); - - if (this->spec.samples == 0) { - /* Init failed? */ - return SDL_SetError("Java-side initialization failed!"); + if (Android_JNI_OpenAudioDevice(iscapture, &this->spec) < 0) { + return -1; } SDL_CalculateAudioSpec(&this->spec); diff --git a/Engine/lib/sdl/src/audio/arts/SDL_artsaudio.c b/Engine/lib/sdl/src/audio/arts/SDL_artsaudio.c index 4e3ebf2ce..47bad4bd4 100644 --- a/Engine/lib/sdl/src/audio/arts/SDL_artsaudio.c +++ b/Engine/lib/sdl/src/audio/arts/SDL_artsaudio.c @@ -39,7 +39,7 @@ #include "SDL_name.h" #include "SDL_loadso.h" #else -#define SDL_NAME(X) X +#define SDL_NAME(X) X #endif #ifdef SDL_AUDIO_DRIVER_ARTS_DYNAMIC diff --git a/Engine/lib/sdl/src/audio/coreaudio/SDL_coreaudio.h b/Engine/lib/sdl/src/audio/coreaudio/SDL_coreaudio.h index 7ce8b8dfa..dcce3f7c6 100644 --- a/Engine/lib/sdl/src/audio/coreaudio/SDL_coreaudio.h +++ b/Engine/lib/sdl/src/audio/coreaudio/SDL_coreaudio.h @@ -45,16 +45,14 @@ struct SDL_PrivateAudioData { - SDL_Thread *thread; AudioQueueRef audioQueue; + int numAudioBuffers; AudioQueueBufferRef *audioBuffer; void *buffer; - UInt32 bufferOffset; UInt32 bufferSize; AudioStreamBasicDescription strdesc; - SDL_sem *ready_semaphore; - char *thread_error; - SDL_atomic_t shutdown; + SDL_bool refill; + SDL_AudioStream *capturestream; #if MACOSX_COREAUDIO AudioDeviceID deviceID; #else diff --git a/Engine/lib/sdl/src/audio/coreaudio/SDL_coreaudio.m b/Engine/lib/sdl/src/audio/coreaudio/SDL_coreaudio.m index 92f5f12a0..59242f935 100644 --- a/Engine/lib/sdl/src/audio/coreaudio/SDL_coreaudio.m +++ b/Engine/lib/sdl/src/audio/coreaudio/SDL_coreaudio.m @@ -26,6 +26,7 @@ #include "SDL_audio.h" #include "SDL_hints.h" +#include "SDL_timer.h" #include "../SDL_audio_c.h" #include "../SDL_sysaudio.h" #include "SDL_coreaudio.h" @@ -354,7 +355,7 @@ static BOOL update_audio_session(_THIS, SDL_bool open) return NO; } - if (open_playback_devices + open_capture_devices == 1) { + if (open && (open_playback_devices + open_capture_devices) == 1) { if (![session setActive:YES error:&err]) { NSString *desc = err.description; SDL_SetError("Could not activate Audio Session: %s", desc.UTF8String); @@ -391,10 +392,10 @@ static BOOL update_audio_session(_THIS, SDL_bool open) if (this->hidden->interruption_listener != NULL) { SDLInterruptionListener *listener = nil; listener = (SDLInterruptionListener *) CFBridgingRelease(this->hidden->interruption_listener); + [center removeObserver:listener]; @synchronized (listener) { listener.device = NULL; } - [center removeObserver:listener]; } } } @@ -409,43 +410,27 @@ static void outputCallback(void *inUserData, AudioQueueRef inAQ, AudioQueueBufferRef inBuffer) { SDL_AudioDevice *this = (SDL_AudioDevice *) inUserData; - if (SDL_AtomicGet(&this->hidden->shutdown)) { - return; /* don't do anything. */ - } - - if (!SDL_AtomicGet(&this->enabled) || SDL_AtomicGet(&this->paused)) { - /* Supply silence if audio is not enabled or paused */ - SDL_memset(inBuffer->mAudioData, this->spec.silence, inBuffer->mAudioDataBytesCapacity); - } else { - UInt32 remaining = inBuffer->mAudioDataBytesCapacity; - Uint8 *ptr = (Uint8 *) inBuffer->mAudioData; - - while (remaining > 0) { - UInt32 len; - if (this->hidden->bufferOffset >= this->hidden->bufferSize) { - /* Generate the data */ - SDL_LockMutex(this->mixer_lock); - (*this->callbackspec.callback)(this->callbackspec.userdata, - this->hidden->buffer, this->hidden->bufferSize); - SDL_UnlockMutex(this->mixer_lock); - this->hidden->bufferOffset = 0; - } - - len = this->hidden->bufferSize - this->hidden->bufferOffset; - if (len > remaining) { - len = remaining; - } - SDL_memcpy(ptr, (char *)this->hidden->buffer + - this->hidden->bufferOffset, len); - ptr = ptr + len; - remaining -= len; - this->hidden->bufferOffset += len; - } - } - + SDL_assert(inBuffer->mAudioDataBytesCapacity == this->hidden->bufferSize); + SDL_memcpy(inBuffer->mAudioData, this->hidden->buffer, this->hidden->bufferSize); + SDL_memset(this->hidden->buffer, '\0', this->hidden->bufferSize); /* zero out in case we have to fill again without new data. */ + inBuffer->mAudioDataByteSize = this->hidden->bufferSize; AudioQueueEnqueueBuffer(this->hidden->audioQueue, inBuffer, 0, NULL); + this->hidden->refill = SDL_TRUE; +} - inBuffer->mAudioDataByteSize = inBuffer->mAudioDataBytesCapacity; +static Uint8 * +COREAUDIO_GetDeviceBuf(_THIS) +{ + return this->hidden->buffer; +} + +static void +COREAUDIO_WaitDevice(_THIS) +{ + while (SDL_AtomicGet(&this->enabled) && !this->hidden->refill) { + CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0.10, 1); + } + this->hidden->refill = SDL_FALSE; } static void @@ -454,36 +439,46 @@ inputCallback(void *inUserData, AudioQueueRef inAQ, AudioQueueBufferRef inBuffer const AudioStreamPacketDescription *inPacketDescs ) { SDL_AudioDevice *this = (SDL_AudioDevice *) inUserData; - - if (SDL_AtomicGet(&this->shutdown)) { - return; /* don't do anything. */ - } - - /* ignore unless we're active. */ - if (!SDL_AtomicGet(&this->paused) && SDL_AtomicGet(&this->enabled) && !SDL_AtomicGet(&this->paused)) { - const Uint8 *ptr = (const Uint8 *) inBuffer->mAudioData; - UInt32 remaining = inBuffer->mAudioDataByteSize; - while (remaining > 0) { - UInt32 len = this->hidden->bufferSize - this->hidden->bufferOffset; - if (len > remaining) { - len = remaining; - } - - SDL_memcpy((char *)this->hidden->buffer + this->hidden->bufferOffset, ptr, len); - ptr += len; - remaining -= len; - this->hidden->bufferOffset += len; - - if (this->hidden->bufferOffset >= this->hidden->bufferSize) { - SDL_LockMutex(this->mixer_lock); - (*this->callbackspec.callback)(this->callbackspec.userdata, this->hidden->buffer, this->hidden->bufferSize); - SDL_UnlockMutex(this->mixer_lock); - this->hidden->bufferOffset = 0; - } + if (SDL_AtomicGet(&this->enabled)) { + SDL_AudioStream *stream = this->hidden->capturestream; + if (SDL_AudioStreamPut(stream, inBuffer->mAudioData, inBuffer->mAudioDataByteSize) == -1) { + /* yikes, out of memory or something. I guess drop the buffer. Our WASAPI target kills the device in this case, though */ } + AudioQueueEnqueueBuffer(this->hidden->audioQueue, inBuffer, 0, NULL); + this->hidden->refill = SDL_TRUE; + } +} + +static int +COREAUDIO_CaptureFromDevice(_THIS, void *buffer, int buflen) +{ + SDL_AudioStream *stream = this->hidden->capturestream; + while (SDL_AtomicGet(&this->enabled)) { + const int avail = SDL_AudioStreamAvailable(stream); + if (avail > 0) { + const int cpy = SDL_min(buflen, avail); + SDL_AudioStreamGet(stream, buffer, cpy); + return cpy; + } + + /* wait for more data, try again. */ + while (SDL_AtomicGet(&this->enabled) && !this->hidden->refill) { + CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0.10, 1); + } + this->hidden->refill = SDL_FALSE; } - AudioQueueEnqueueBuffer(this->hidden->audioQueue, inBuffer, 0, NULL); + return 0; /* not enabled, giving up. */ +} + +static void +COREAUDIO_FlushCapture(_THIS) +{ + while (CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0, 1) == kCFRunLoopRunHandledSource) { + /* spin. */ + } + this->hidden->refill = SDL_FALSE; + SDL_AudioStreamClear(this->hidden->capturestream); } @@ -541,25 +536,16 @@ COREAUDIO_CloseDevice(_THIS) update_audio_session(this, SDL_FALSE); #endif - /* if callback fires again, feed silence; don't call into the app. */ - SDL_AtomicSet(&this->paused, 1); - if (this->hidden->audioQueue) { AudioQueueDispose(this->hidden->audioQueue, 1); } - if (this->hidden->thread) { - SDL_AtomicSet(&this->hidden->shutdown, 1); - SDL_WaitThread(this->hidden->thread, NULL); - } - - if (this->hidden->ready_semaphore) { - SDL_DestroySemaphore(this->hidden->ready_semaphore); + if (this->hidden->capturestream) { + SDL_FreeAudioStream(this->hidden->capturestream); } /* AudioQueueDispose() frees the actual buffer objects. */ SDL_free(this->hidden->audioBuffer); - SDL_free(this->hidden->thread_error); SDL_free(this->hidden->buffer); SDL_free(this->hidden); @@ -625,6 +611,8 @@ prepare_device(_THIS, void *handle, int iscapture) } #endif + +/* this all happens in the audio thread, since it needs a separate runloop. */ static int prepare_audioqueue(_THIS) { @@ -664,19 +652,6 @@ prepare_audioqueue(_THIS) } #endif - /* Calculate the final parameters for this audio specification */ - SDL_CalculateAudioSpec(&this->spec); - - /* Allocate a sample buffer */ - this->hidden->bufferSize = this->spec.size; - this->hidden->bufferOffset = iscapture ? 0 : this->hidden->bufferSize; - - this->hidden->buffer = SDL_malloc(this->hidden->bufferSize); - if (this->hidden->buffer == NULL) { - SDL_OutOfMemory(); - return 0; - } - /* Make sure we can feed the device a minimum amount of time */ double MINIMUM_AUDIO_BUFFER_TIME_MS = 15.0; #if defined(__IPHONEOS__) @@ -691,6 +666,7 @@ prepare_audioqueue(_THIS) numAudioBuffers = ((int)SDL_ceil(MINIMUM_AUDIO_BUFFER_TIME_MS / msecs) * 2); } + this->hidden->numAudioBuffers = numAudioBuffers; this->hidden->audioBuffer = SDL_calloc(1, sizeof (AudioQueueBufferRef) * numAudioBuffers); if (this->hidden->audioBuffer == NULL) { SDL_OutOfMemory(); @@ -717,29 +693,23 @@ prepare_audioqueue(_THIS) return 1; } -static int -audioqueue_thread(void *arg) +static void +COREAUDIO_ThreadInit(_THIS) { - SDL_AudioDevice *this = (SDL_AudioDevice *) arg; const int rc = prepare_audioqueue(this); if (!rc) { - this->hidden->thread_error = SDL_strdup(SDL_GetError()); - SDL_SemPost(this->hidden->ready_semaphore); - return 0; + /* !!! FIXME: do this in RunAudio, and maybe block OpenDevice until ThreadInit finishes, too, to report an opening error */ + SDL_OpenedAudioDeviceDisconnected(this); /* oh well. */ } +} - /* init was successful, alert parent thread and start running... */ - SDL_SemPost(this->hidden->ready_semaphore); - while (!SDL_AtomicGet(&this->hidden->shutdown)) { - CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0.10, 1); - } - - if (!this->iscapture) { /* Drain off any pending playback. */ - const CFTimeInterval secs = (((this->spec.size / (SDL_AUDIO_BITSIZE(this->spec.format) / 8)) / this->spec.channels) / ((CFTimeInterval) this->spec.freq)) * 2.0; - CFRunLoopRunInMode(kCFRunLoopDefaultMode, secs, 0); - } - - return 0; +static void +COREAUDIO_PrepareToClose(_THIS) +{ + /* run long enough to queue some silence, so we know our actual audio + has been played */ + CFRunLoopRunInMode(kCFRunLoopDefaultMode, (((this->spec.samples * 1000) / this->spec.freq) * 2) / 1000.0f, 0); + AudioQueueStop(this->hidden->audioQueue, 1); } static int @@ -826,28 +796,23 @@ COREAUDIO_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) } #endif - /* This has to init in a new thread so it can get its own CFRunLoop. :/ */ - SDL_AtomicSet(&this->hidden->shutdown, 0); - this->hidden->ready_semaphore = SDL_CreateSemaphore(0); - if (!this->hidden->ready_semaphore) { - return -1; /* oh well. */ + /* Calculate the final parameters for this audio specification */ + SDL_CalculateAudioSpec(&this->spec); + + if (iscapture) { + this->hidden->capturestream = SDL_NewAudioStream(this->spec.format, this->spec.channels, this->spec.freq, this->spec.format, this->spec.channels, this->spec.freq); + if (!this->hidden->capturestream) { + return -1; /* already set SDL_Error */ + } + } else { + this->hidden->bufferSize = this->spec.size; + this->hidden->buffer = SDL_malloc(this->hidden->bufferSize); + if (this->hidden->buffer == NULL) { + return SDL_OutOfMemory(); + } } - this->hidden->thread = SDL_CreateThreadInternal(audioqueue_thread, "AudioQueue thread", 512 * 1024, this); - if (!this->hidden->thread) { - return -1; - } - - SDL_SemWait(this->hidden->ready_semaphore); - SDL_DestroySemaphore(this->hidden->ready_semaphore); - this->hidden->ready_semaphore = NULL; - - if ((this->hidden->thread != NULL) && (this->hidden->thread_error != NULL)) { - SDL_SetError("%s", this->hidden->thread_error); - return -1; - } - - return (this->hidden->thread != NULL) ? 0 : -1; + return 0; } static void @@ -867,6 +832,12 @@ COREAUDIO_Init(SDL_AudioDriverImpl * impl) impl->OpenDevice = COREAUDIO_OpenDevice; impl->CloseDevice = COREAUDIO_CloseDevice; impl->Deinitialize = COREAUDIO_Deinitialize; + impl->ThreadInit = COREAUDIO_ThreadInit; + impl->WaitDevice = COREAUDIO_WaitDevice; + impl->GetDeviceBuf = COREAUDIO_GetDeviceBuf; + impl->PrepareToClose = COREAUDIO_PrepareToClose; + impl->CaptureFromDevice = COREAUDIO_CaptureFromDevice; + impl->FlushCapture = COREAUDIO_FlushCapture; #if MACOSX_COREAUDIO impl->DetectDevices = COREAUDIO_DetectDevices; @@ -876,7 +847,6 @@ COREAUDIO_Init(SDL_AudioDriverImpl * impl) impl->OnlyHasDefaultCaptureDevice = 1; #endif - impl->ProvidesOwnCallbackThread = 1; impl->HasCaptureSupport = 1; return 1; /* this audio target is available. */ diff --git a/Engine/lib/sdl/src/audio/directsound/SDL_directsound.c b/Engine/lib/sdl/src/audio/directsound/SDL_directsound.c index 09b83aedf..a943ba29e 100644 --- a/Engine/lib/sdl/src/audio/directsound/SDL_directsound.c +++ b/Engine/lib/sdl/src/audio/directsound/SDL_directsound.c @@ -477,8 +477,8 @@ DSOUND_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) SDL_bool tried_format = SDL_FALSE; SDL_AudioFormat test_format = SDL_FirstAudioFormat(this->spec.format); LPGUID guid = (LPGUID) handle; - DWORD bufsize; - + DWORD bufsize; + /* Initialize all variables that we clean on shutdown */ this->hidden = (struct SDL_PrivateAudioData *) SDL_malloc((sizeof *this->hidden)); @@ -526,7 +526,7 @@ DSOUND_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) (int) (DSBSIZE_MAX / numchunks)); } else { int rc; - WAVEFORMATEX wfmt; + WAVEFORMATEX wfmt; SDL_zero(wfmt); if (SDL_AUDIO_ISFLOAT(this->spec.format)) { wfmt.wFormatTag = WAVE_FORMAT_IEEE_FLOAT; diff --git a/Engine/lib/sdl/src/audio/jack/SDL_jackaudio.c b/Engine/lib/sdl/src/audio/jack/SDL_jackaudio.c index a252da70e..76ff431c8 100644 --- a/Engine/lib/sdl/src/audio/jack/SDL_jackaudio.c +++ b/Engine/lib/sdl/src/audio/jack/SDL_jackaudio.c @@ -44,7 +44,9 @@ static const char ** (*JACK_jack_get_ports) (jack_client_t *, const char *, cons static jack_nframes_t (*JACK_jack_get_sample_rate) (jack_client_t *); static jack_nframes_t (*JACK_jack_get_buffer_size) (jack_client_t *); static jack_port_t * (*JACK_jack_port_register) (jack_client_t *, const char *, const char *, unsigned long, unsigned long); +static jack_port_t * (*JACK_jack_port_by_name) (jack_client_t *, const char *); static const char * (*JACK_jack_port_name) (const jack_port_t *); +static const char * (*JACK_jack_port_type) (const jack_port_t *); static int (*JACK_jack_connect) (jack_client_t *, const char *, const char *); static int (*JACK_jack_set_process_callback) (jack_client_t *, JackProcessCallback, void *); @@ -135,7 +137,9 @@ load_jack_syms(void) SDL_JACK_SYM(jack_get_sample_rate); SDL_JACK_SYM(jack_get_buffer_size); SDL_JACK_SYM(jack_port_register); + SDL_JACK_SYM(jack_port_by_name); SDL_JACK_SYM(jack_port_name); + SDL_JACK_SYM(jack_port_type); SDL_JACK_SYM(jack_connect); SDL_JACK_SYM(jack_set_process_callback); return 0; @@ -273,10 +277,6 @@ JACK_CloseDevice(_THIS) SDL_DestroySemaphore(this->hidden->iosem); } - if (this->hidden->devports) { - JACK_jack_free(this->hidden->devports); - } - SDL_free(this->hidden->iobuffer); } @@ -292,9 +292,11 @@ JACK_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) const JackProcessCallback callback = iscapture ? jackProcessCaptureCallback : jackProcessPlaybackCallback; const char *sdlportstr = iscapture ? "input" : "output"; const char **devports = NULL; + int *audio_ports; jack_client_t *client = NULL; jack_status_t status; int channels = 0; + int ports = 0; int i; /* Initialize all variables that we clean on shutdown */ @@ -311,15 +313,30 @@ JACK_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) } devports = JACK_jack_get_ports(client, NULL, NULL, JackPortIsPhysical | sysportflags); - this->hidden->devports = devports; if (!devports || !devports[0]) { return SDL_SetError("No physical JACK ports available"); } - while (devports[++channels]) { + while (devports[++ports]) { /* spin to count devports */ } + /* Filter out non-audio ports */ + audio_ports = SDL_calloc(ports, sizeof *audio_ports); + for (i = 0; i < ports; i++) { + const jack_port_t *dport = JACK_jack_port_by_name(client, devports[i]); + const char *type = JACK_jack_port_type(dport); + const int len = SDL_strlen(type); + /* See if type ends with "audio" */ + if (len >= 5 && !SDL_memcmp(type+len-5, "audio", 5)) { + audio_ports[channels++] = i; + } + } + if (channels == 0) { + return SDL_SetError("No physical JACK ports available"); + } + + /* !!! FIXME: docs say about buffer size: "This size may change, clients that depend on it must register a bufsize_callback so they will be notified if it does." */ /* Jack pretty much demands what it wants. */ @@ -368,16 +385,16 @@ JACK_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) /* once activated, we can connect all the ports. */ for (i = 0; i < channels; i++) { const char *sdlport = JACK_jack_port_name(this->hidden->sdlports[i]); - const char *srcport = iscapture ? devports[i] : sdlport; - const char *dstport = iscapture ? sdlport : devports[i]; + const char *srcport = iscapture ? devports[audio_ports[i]] : sdlport; + const char *dstport = iscapture ? sdlport : devports[audio_ports[i]]; if (JACK_jack_connect(client, srcport, dstport) != 0) { return SDL_SetError("Couldn't connect JACK ports: %s => %s", srcport, dstport); } } /* don't need these anymore. */ - this->hidden->devports = NULL; JACK_jack_free(devports); + SDL_free(audio_ports); /* We're ready to rock and roll. :-) */ return 0; diff --git a/Engine/lib/sdl/src/audio/jack/SDL_jackaudio.h b/Engine/lib/sdl/src/audio/jack/SDL_jackaudio.h index aab199ac2..5bc04bd89 100644 --- a/Engine/lib/sdl/src/audio/jack/SDL_jackaudio.h +++ b/Engine/lib/sdl/src/audio/jack/SDL_jackaudio.h @@ -33,7 +33,6 @@ struct SDL_PrivateAudioData jack_client_t *client; SDL_sem *iosem; float *iobuffer; - const char **devports; jack_port_t **sdlports; }; diff --git a/Engine/lib/sdl/src/audio/pulseaudio/SDL_pulseaudio.c b/Engine/lib/sdl/src/audio/pulseaudio/SDL_pulseaudio.c index 1e9858063..053a1c34a 100644 --- a/Engine/lib/sdl/src/audio/pulseaudio/SDL_pulseaudio.c +++ b/Engine/lib/sdl/src/audio/pulseaudio/SDL_pulseaudio.c @@ -109,7 +109,7 @@ static pa_operation * (*PULSEAUDIO_pa_stream_drain) (pa_stream *, pa_stream_success_cb_t, void *); static int (*PULSEAUDIO_pa_stream_peek) (pa_stream *, const void **, size_t *); static int (*PULSEAUDIO_pa_stream_drop) (pa_stream *); -static pa_operation * (*PULSEAUDIO_pa_stream_flush) (pa_stream *, +static pa_operation * (*PULSEAUDIO_pa_stream_flush) (pa_stream *, pa_stream_success_cb_t, void *); static int (*PULSEAUDIO_pa_stream_disconnect) (pa_stream *); static void (*PULSEAUDIO_pa_stream_unref) (pa_stream *); diff --git a/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi.c b/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi.c index b7c8dda5d..f51753965 100644 --- a/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi.c +++ b/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi.c @@ -725,6 +725,12 @@ WASAPI_ThreadDeinit(_THIS) WASAPI_PlatformThreadDeinit(this); } +void +WASAPI_BeginLoopIteration(_THIS) +{ + /* no-op. */ +} + static void WASAPI_Deinitialize(void) { diff --git a/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi_win32.c b/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi_win32.c index 8b55582c3..9d7c1591b 100644 --- a/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi_win32.c +++ b/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi_win32.c @@ -351,10 +351,42 @@ WASAPI_ActivateDevice(_THIS, const SDL_bool isrecovery) } +typedef struct +{ + LPWSTR devid; + char *devname; +} EndpointItem; + +static int sort_endpoints(const void *_a, const void *_b) +{ + LPWSTR a = ((const EndpointItem *) _a)->devid; + LPWSTR b = ((const EndpointItem *) _b)->devid; + if (!a && b) { + return -1; + } else if (a && !b) { + return 1; + } + + while (SDL_TRUE) { + if (*a < *b) { + return -1; + } else if (*a > *b) { + return 1; + } else if (*a == 0) { + break; + } + a++; + b++; + } + + return 0; +} + static void WASAPI_EnumerateEndpointsForFlow(const SDL_bool iscapture) { IMMDeviceCollection *collection = NULL; + EndpointItem *items; UINT i, total; /* Note that WASAPI separates "adapter devices" from "audio endpoint devices" @@ -369,22 +401,36 @@ WASAPI_EnumerateEndpointsForFlow(const SDL_bool iscapture) return; } + items = (EndpointItem *) SDL_calloc(total, sizeof (EndpointItem)); + if (!items) { + return; /* oh well. */ + } + for (i = 0; i < total; i++) { + EndpointItem *item = items + i; IMMDevice *device = NULL; if (SUCCEEDED(IMMDeviceCollection_Item(collection, i, &device))) { - LPWSTR devid = NULL; - if (SUCCEEDED(IMMDevice_GetId(device, &devid))) { - char *devname = GetWasapiDeviceName(device); - if (devname) { - WASAPI_AddDevice(iscapture, devname, devid); - SDL_free(devname); - } - CoTaskMemFree(devid); + if (SUCCEEDED(IMMDevice_GetId(device, &item->devid))) { + item->devname = GetWasapiDeviceName(device); } IMMDevice_Release(device); } } + /* sort the list of devices by their guid so list is consistent between runs */ + SDL_qsort(items, total, sizeof (*items), sort_endpoints); + + /* Send the sorted list on to the SDL's higher level. */ + for (i = 0; i < total; i++) { + EndpointItem *item = items + i; + if ((item->devid) && (item->devname)) { + WASAPI_AddDevice(iscapture, item->devname, item->devid); + } + SDL_free(item->devname); + CoTaskMemFree(item->devid); + } + + SDL_free(items); IMMDeviceCollection_Release(collection); } @@ -405,12 +451,6 @@ WASAPI_PlatformDeleteActivationHandler(void *handler) SDL_assert(!"This function should have only been called on WinRT."); } -void -WASAPI_BeginLoopIteration(_THIS) -{ - /* no-op. */ -} - #endif /* SDL_AUDIO_DRIVER_WASAPI && !defined(__WINRT__) */ /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi_winrt.cpp b/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi_winrt.cpp index 309ec6a78..2ca09de86 100644 --- a/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi_winrt.cpp +++ b/Engine/lib/sdl/src/audio/wasapi/SDL_wasapi_winrt.cpp @@ -185,20 +185,9 @@ struct SDL_WasapiActivationHandler : public RuntimeClass< RuntimeClassFlags< Cla HRESULT SDL_WasapiActivationHandler::ActivateCompleted(IActivateAudioInterfaceAsyncOperation *async) { - HRESULT result = S_OK; - IUnknown *iunknown = nullptr; - const HRESULT ret = async->GetActivateResult(&result, &iunknown); - - if (SUCCEEDED(ret) && SUCCEEDED(result)) { - iunknown->QueryInterface(IID_PPV_ARGS(&device->hidden->client)); - if (device->hidden->client) { - // Just set a flag, since we're probably in a different thread. We'll pick it up and init everything on our own thread to prevent races. - SDL_AtomicSet(&device->hidden->just_activated, 1); - } - } - + // Just set a flag, since we're probably in a different thread. We'll pick it up and init everything on our own thread to prevent races. + SDL_AtomicSet(&device->hidden->just_activated, 1); WASAPI_UnrefDevice(device); - return S_OK; } @@ -236,27 +225,47 @@ WASAPI_ActivateDevice(_THIS, const SDL_bool isrecovery) IActivateAudioInterfaceAsyncOperation *async = nullptr; const HRESULT ret = ActivateAudioInterfaceAsync(devid, __uuidof(IAudioClient), nullptr, handler.Get(), &async); - if (async != nullptr) { - async->Release(); - } - - if (FAILED(ret)) { + if (FAILED(ret) || async == nullptr) { + if (async != nullptr) { + async->Release(); + } handler.Get()->Release(); WASAPI_UnrefDevice(_this); return WIN_SetErrorFromHRESULT("WASAPI can't activate requested audio endpoint", ret); } - return 0; -} - -void -WASAPI_BeginLoopIteration(_THIS) -{ - if (SDL_AtomicCAS(&_this->hidden->just_activated, 1, 0)) { - if (WASAPI_PrepDevice(_this, SDL_TRUE) == -1) { - SDL_OpenedAudioDeviceDisconnected(_this); - } + /* Spin until the async operation is complete. + * If we don't PrepDevice before leaving this function, the bug list gets LONG: + * - device.spec is not filled with the correct information + * - The 'obtained' spec will be wrong for ALLOW_CHANGE properties + * - SDL_AudioStreams will/will not be allocated at the right time + * - SDL_assert(device->callbackspec.size == device->spec.size) will fail + * - When the assert is ignored, skipping or a buffer overflow will occur + */ + while (!SDL_AtomicCAS(&_this->hidden->just_activated, 1, 0)) { + SDL_Delay(1); } + + HRESULT activateRes = S_OK; + IUnknown *iunknown = nullptr; + const HRESULT getActivateRes = async->GetActivateResult(&activateRes, &iunknown); + async->Release(); + if (FAILED(getActivateRes)) { + return WIN_SetErrorFromHRESULT("Failed to get WASAPI activate result", getActivateRes); + } else if (FAILED(activateRes)) { + return WIN_SetErrorFromHRESULT("Failed to activate WASAPI device", activateRes); + } + + iunknown->QueryInterface(IID_PPV_ARGS(&_this->hidden->client)); + if (!_this->hidden->client) { + return SDL_SetError("Failed to query WASAPI client interface"); + } + + if (WASAPI_PrepDevice(_this, isrecovery) == -1) { + return -1; + } + + return 0; } void diff --git a/Engine/lib/sdl/src/audio/winmm/SDL_winmm.c b/Engine/lib/sdl/src/audio/winmm/SDL_winmm.c index 8e5c17ba1..20426f13a 100644 --- a/Engine/lib/sdl/src/audio/winmm/SDL_winmm.c +++ b/Engine/lib/sdl/src/audio/winmm/SDL_winmm.c @@ -78,7 +78,7 @@ static void DetectWave##typ##Devs(void) { \ capstyp##2W caps; \ UINT i; \ for (i = 0; i < devcount; i++) { \ - if (wave##typ##GetDevCaps(i,(LP##capstyp##W)&caps,sizeof(caps))==MMSYSERR_NOERROR) { \ + if (wave##typ##GetDevCaps(i,(LP##capstyp##W)&caps,sizeof(caps))==MMSYSERR_NOERROR) { \ char *name = WIN_LookupAudioDeviceName(caps.szPname,&caps.NameGuid); \ if (name != NULL) { \ SDL_AddAudioDevice((int) iscapture, name, (void *) ((size_t) i+1)); \ @@ -375,8 +375,7 @@ WINMM_OpenDevice(_THIS, void *handle, const char *devname, int iscapture) #endif /* Create the audio buffer semaphore */ - this->hidden->audio_sem = - CreateSemaphore(NULL, iscapture ? 0 : NUM_BUFFERS - 1, NUM_BUFFERS, NULL); + this->hidden->audio_sem = CreateSemaphore(NULL, iscapture ? 0 : NUM_BUFFERS - 1, NUM_BUFFERS, NULL); if (this->hidden->audio_sem == NULL) { return SDL_SetError("Couldn't create semaphore"); } diff --git a/Engine/lib/sdl/src/core/android/SDL_android.c b/Engine/lib/sdl/src/core/android/SDL_android.c index c40c6764f..a56575e09 100644 --- a/Engine/lib/sdl/src/core/android/SDL_android.c +++ b/Engine/lib/sdl/src/core/android/SDL_android.c @@ -61,6 +61,10 @@ #define SDL_JAVA_CONTROLLER_INTERFACE(function) CONCAT1(SDL_JAVA_PREFIX, SDLControllerManager, function) #define SDL_JAVA_INTERFACE_INPUT_CONNECTION(function) CONCAT1(SDL_JAVA_PREFIX, SDLInputConnection, function) +/* Audio encoding definitions */ +#define ENCODING_PCM_8BIT 3 +#define ENCODING_PCM_16BIT 2 +#define ENCODING_PCM_FLOAT 4 /* Java class SDLActivity */ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(nativeSetupJNI)( @@ -76,7 +80,8 @@ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeDropFile)( JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeResize)( JNIEnv* env, jclass jcls, - jint width, jint height, jint format, jfloat rate); + jint surfaceWidth, jint surfaceHeight, + jint deviceWidth, jint deviceHeight, jint format, jfloat rate); JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeSurfaceChanged)( JNIEnv* env, jclass jcls); @@ -102,7 +107,7 @@ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeTouch)( JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeMouse)( JNIEnv* env, jclass jcls, - jint button, jint action, jfloat x, jfloat y); + jint button, jint action, jfloat x, jfloat y, jboolean relative); JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeAccel)( JNIEnv* env, jclass jcls, @@ -134,11 +139,19 @@ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(nativeSetenv)( JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(nativeEnvironmentVariablesSet)( JNIEnv* env, jclass cls); +JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeOrientationChanged)( + JNIEnv* env, jclass cls, + jint orientation); + /* Java class SDLInputConnection */ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE_INPUT_CONNECTION(nativeCommitText)( JNIEnv* env, jclass cls, jstring text, jint newCursorPosition); +JNIEXPORT void JNICALL SDL_JAVA_INTERFACE_INPUT_CONNECTION(nativeGenerateScancodeForUnichar)( + JNIEnv* env, jclass cls, + jchar chUnicode); + JNIEXPORT void JNICALL SDL_JAVA_INTERFACE_INPUT_CONNECTION(nativeSetComposingText)( JNIEnv* env, jclass cls, jstring text, jint newCursorPosition); @@ -169,8 +182,8 @@ JNIEXPORT void JNICALL SDL_JAVA_CONTROLLER_INTERFACE(onNativeHat)( JNIEXPORT jint JNICALL SDL_JAVA_CONTROLLER_INTERFACE(nativeAddJoystick)( JNIEnv* env, jclass jcls, - jint device_id, jstring device_name, jstring device_desc, jint is_accelerometer, - jint nbuttons, jint naxes, jint nhats, jint nballs); + jint device_id, jstring device_name, jstring device_desc, jint vendor_id, jint product_id, + jboolean is_accelerometer, jint button_mask, jint naxes, jint nhats, jint nballs); JNIEXPORT jint JNICALL SDL_JAVA_CONTROLLER_INTERFACE(nativeRemoveJoystick)( JNIEnv* env, jclass jcls, @@ -190,6 +203,7 @@ JNIEXPORT jint JNICALL SDL_JAVA_CONTROLLER_INTERFACE(nativeRemoveHaptic)( /* #define DEBUG_JNI */ static void Android_JNI_ThreadDestroyed(void*); +static void checkJNIReady(void); /******************************************************************************* This file links the Java side of Android with libsdl @@ -212,7 +226,11 @@ static jmethodID midSetActivityTitle; static jmethodID midSetWindowStyle; static jmethodID midSetOrientation; static jmethodID midGetContext; +static jmethodID midIsTablet; static jmethodID midIsAndroidTV; +static jmethodID midIsChromebook; +static jmethodID midIsDeXMode; +static jmethodID midManualBackButton; static jmethodID midInputGetInputDeviceIds; static jmethodID midSendMessage; static jmethodID midShowTextInput; @@ -223,18 +241,25 @@ static jmethodID midClipboardHasText; static jmethodID midOpenAPKExpansionInputStream; static jmethodID midGetManifestEnvironmentVariables; static jmethodID midGetDisplayDPI; +static jmethodID midCreateCustomCursor; +static jmethodID midSetCustomCursor; +static jmethodID midSetSystemCursor; +static jmethodID midSupportsRelativeMouse; +static jmethodID midSetRelativeMouseEnabled; /* audio manager */ static jclass mAudioManagerClass; /* method signatures */ static jmethodID midAudioOpen; -static jmethodID midAudioWriteShortBuffer; static jmethodID midAudioWriteByteBuffer; +static jmethodID midAudioWriteShortBuffer; +static jmethodID midAudioWriteFloatBuffer; static jmethodID midAudioClose; static jmethodID midCaptureOpen; -static jmethodID midCaptureReadShortBuffer; static jmethodID midCaptureReadByteBuffer; +static jmethodID midCaptureReadShortBuffer; +static jmethodID midCaptureReadFloatBuffer; static jmethodID midCaptureClose; /* controller manager */ @@ -244,6 +269,7 @@ static jclass mControllerManagerClass; static jmethodID midPollInputDevices; static jmethodID midPollHapticDevices; static jmethodID midHapticRun; +static jmethodID midHapticStop; /* static fields */ static jfieldID fidSeparateMouseAndTouch; @@ -309,8 +335,16 @@ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(nativeSetupJNI)(JNIEnv* mEnv, jclass c "setOrientation","(IIZLjava/lang/String;)V"); midGetContext = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, "getContext","()Landroid/content/Context;"); + midIsTablet = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, + "isTablet", "()Z"); midIsAndroidTV = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, "isAndroidTV","()Z"); + midIsChromebook = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, + "isChromebook", "()Z"); + midIsDeXMode = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, + "isDeXMode", "()Z"); + midManualBackButton = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, + "manualBackButton", "()V"); midInputGetInputDeviceIds = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, "inputGetInputDeviceIds", "(I)[I"); midSendMessage = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, @@ -332,13 +366,21 @@ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(nativeSetupJNI)(JNIEnv* mEnv, jclass c "getManifestEnvironmentVariables", "()Z"); midGetDisplayDPI = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, "getDisplayDPI", "()Landroid/util/DisplayMetrics;"); - midGetDisplayDPI = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, "getDisplayDPI", "()Landroid/util/DisplayMetrics;"); + midCreateCustomCursor = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, "createCustomCursor", "([IIIII)I"); + midSetCustomCursor = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, "setCustomCursor", "(I)Z"); + midSetSystemCursor = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, "setSystemCursor", "(I)Z"); + + midSupportsRelativeMouse = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, "supportsRelativeMouse", "()Z"); + midSetRelativeMouseEnabled = (*mEnv)->GetStaticMethodID(mEnv, mActivityClass, "setRelativeMouseEnabled", "(Z)Z"); + if (!midGetNativeSurface || - !midSetActivityTitle || !midSetWindowStyle || !midSetOrientation || !midGetContext || !midIsAndroidTV || !midInputGetInputDeviceIds || + !midSetActivityTitle || !midSetWindowStyle || !midSetOrientation || !midGetContext || !midIsTablet || !midIsAndroidTV || !midInputGetInputDeviceIds || !midSendMessage || !midShowTextInput || !midIsScreenKeyboardShown || !midClipboardSetText || !midClipboardGetText || !midClipboardHasText || - !midOpenAPKExpansionInputStream || !midGetManifestEnvironmentVariables|| !midGetDisplayDPI) { + !midOpenAPKExpansionInputStream || !midGetManifestEnvironmentVariables || !midGetDisplayDPI || + !midCreateCustomCursor || !midSetCustomCursor || !midSetSystemCursor || !midSupportsRelativeMouse || !midSetRelativeMouseEnabled || + !midIsChromebook || !midIsDeXMode || !midManualBackButton) { __android_log_print(ANDROID_LOG_WARN, "SDL", "Missing some Java callbacks, do you have the latest version of SDLActivity.java?"); } @@ -361,24 +403,28 @@ JNIEXPORT void JNICALL SDL_JAVA_AUDIO_INTERFACE(nativeSetupJNI)(JNIEnv* mEnv, jc mAudioManagerClass = (jclass)((*mEnv)->NewGlobalRef(mEnv, cls)); midAudioOpen = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, - "audioOpen", "(IZZI)I"); - midAudioWriteShortBuffer = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, - "audioWriteShortBuffer", "([S)V"); + "audioOpen", "(IIII)[I"); midAudioWriteByteBuffer = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, "audioWriteByteBuffer", "([B)V"); + midAudioWriteShortBuffer = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, + "audioWriteShortBuffer", "([S)V"); + midAudioWriteFloatBuffer = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, + "audioWriteFloatBuffer", "([F)V"); midAudioClose = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, "audioClose", "()V"); midCaptureOpen = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, - "captureOpen", "(IZZI)I"); - midCaptureReadShortBuffer = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, - "captureReadShortBuffer", "([SZ)I"); + "captureOpen", "(IIII)[I"); midCaptureReadByteBuffer = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, "captureReadByteBuffer", "([BZ)I"); + midCaptureReadShortBuffer = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, + "captureReadShortBuffer", "([SZ)I"); + midCaptureReadFloatBuffer = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, + "captureReadFloatBuffer", "([FZ)I"); midCaptureClose = (*mEnv)->GetStaticMethodID(mEnv, mAudioManagerClass, "captureClose", "()V"); - if (!midAudioOpen || !midAudioWriteShortBuffer || !midAudioWriteByteBuffer || !midAudioClose || - !midCaptureOpen || !midCaptureReadShortBuffer || !midCaptureReadByteBuffer || !midCaptureClose) { + if (!midAudioOpen || !midAudioWriteByteBuffer || !midAudioWriteShortBuffer || !midAudioWriteFloatBuffer || !midAudioClose || + !midCaptureOpen || !midCaptureReadByteBuffer || !midCaptureReadShortBuffer || !midCaptureReadFloatBuffer || !midCaptureClose) { __android_log_print(ANDROID_LOG_WARN, "SDL", "Missing some Java callbacks, do you have the latest version of SDLAudioManager.java?"); } @@ -399,9 +445,11 @@ JNIEXPORT void JNICALL SDL_JAVA_CONTROLLER_INTERFACE(nativeSetupJNI)(JNIEnv* mEn midPollHapticDevices = (*mEnv)->GetStaticMethodID(mEnv, mControllerManagerClass, "pollHapticDevices", "()V"); midHapticRun = (*mEnv)->GetStaticMethodID(mEnv, mControllerManagerClass, - "hapticRun", "(II)V"); + "hapticRun", "(IFI)V"); + midHapticStop = (*mEnv)->GetStaticMethodID(mEnv, mControllerManagerClass, + "hapticStop", "(I)V"); - if (!midPollInputDevices || !midPollHapticDevices || !midHapticRun) { + if (!midPollInputDevices || !midPollHapticDevices || !midHapticRun || !midHapticStop) { __android_log_print(ANDROID_LOG_WARN, "SDL", "Missing some Java callbacks, do you have the latest version of SDLControllerManager.java?"); } @@ -503,9 +551,18 @@ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeDropFile)( /* Resize */ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeResize)( JNIEnv* env, jclass jcls, - jint width, jint height, jint format, jfloat rate) + jint surfaceWidth, jint surfaceHeight, + jint deviceWidth, jint deviceHeight, jint format, jfloat rate) { - Android_SetScreenResolution(width, height, format, rate); + Android_SetScreenResolution(surfaceWidth, surfaceHeight, deviceWidth, deviceHeight, format, rate); +} + +JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeOrientationChanged)( + JNIEnv *env, jclass jcls, + jint orientation) +{ + SDL_VideoDisplay *display = SDL_GetDisplay(0); + SDL_SendDisplayEvent(display, SDL_DISPLAYEVENT_ORIENTATION, orientation); } /* Paddown */ @@ -543,14 +600,15 @@ JNIEXPORT void JNICALL SDL_JAVA_CONTROLLER_INTERFACE(onNativeHat)( JNIEXPORT jint JNICALL SDL_JAVA_CONTROLLER_INTERFACE(nativeAddJoystick)( JNIEnv* env, jclass jcls, - jint device_id, jstring device_name, jstring device_desc, jint is_accelerometer, - jint nbuttons, jint naxes, jint nhats, jint nballs) + jint device_id, jstring device_name, jstring device_desc, + jint vendor_id, jint product_id, jboolean is_accelerometer, + jint button_mask, jint naxes, jint nhats, jint nballs) { int retval; const char *name = (*env)->GetStringUTFChars(env, device_name, NULL); const char *desc = (*env)->GetStringUTFChars(env, device_desc, NULL); - retval = Android_AddJoystick(device_id, name, desc, (SDL_bool) is_accelerometer, nbuttons, naxes, nhats, nballs); + retval = Android_AddJoystick(device_id, name, desc, vendor_id, product_id, is_accelerometer ? SDL_TRUE : SDL_FALSE, button_mask, naxes, nhats, nballs); (*env)->ReleaseStringUTFChars(env, device_name, name); (*env)->ReleaseStringUTFChars(env, device_desc, desc); @@ -675,9 +733,9 @@ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeTouch)( /* Mouse */ JNIEXPORT void JNICALL SDL_JAVA_INTERFACE(onNativeMouse)( JNIEnv* env, jclass jcls, - jint button, jint action, jfloat x, jfloat y) + jint button, jint action, jfloat x, jfloat y, jboolean relative) { - Android_OnMouse(button, action, x, y); + Android_OnMouse(button, action, x, y, relative); } /* Accelerometer */ @@ -995,17 +1053,19 @@ int Android_JNI_SetupThread(void) /* * Audio support */ -static jboolean audioBuffer16Bit = JNI_FALSE; +static int audioBufferFormat = 0; static jobject audioBuffer = NULL; static void* audioBufferPinned = NULL; -static jboolean captureBuffer16Bit = JNI_FALSE; +static int captureBufferFormat = 0; static jobject captureBuffer = NULL; -int Android_JNI_OpenAudioDevice(int iscapture, int sampleRate, int is16Bit, int channelCount, int desiredBufferFrames) +int Android_JNI_OpenAudioDevice(int iscapture, SDL_AudioSpec *spec) { - jboolean audioBufferStereo; - int audioBufferFrames; + int audioformat; + int numBufferFrames; jobject jbufobj = NULL; + jobject result; + int *resultElements; jboolean isCopy; JNIEnv *env = Android_JNI_GetEnv(); @@ -1015,74 +1075,123 @@ int Android_JNI_OpenAudioDevice(int iscapture, int sampleRate, int is16Bit, int } Android_JNI_SetupThread(); - audioBufferStereo = channelCount > 1; + switch (spec->format) { + case AUDIO_U8: + audioformat = ENCODING_PCM_8BIT; + break; + case AUDIO_S16: + audioformat = ENCODING_PCM_16BIT; + break; + case AUDIO_F32: + audioformat = ENCODING_PCM_FLOAT; + break; + default: + return SDL_SetError("Unsupported audio format: 0x%x", spec->format); + } if (iscapture) { __android_log_print(ANDROID_LOG_VERBOSE, "SDL", "SDL audio: opening device for capture"); - captureBuffer16Bit = is16Bit; - if ((*env)->CallStaticIntMethod(env, mAudioManagerClass, midCaptureOpen, sampleRate, audioBuffer16Bit, audioBufferStereo, desiredBufferFrames) != 0) { - /* Error during audio initialization */ - __android_log_print(ANDROID_LOG_WARN, "SDL", "SDL audio: error on AudioRecord initialization!"); - return 0; - } + result = (*env)->CallStaticObjectMethod(env, mAudioManagerClass, midCaptureOpen, spec->freq, audioformat, spec->channels, spec->samples); } else { __android_log_print(ANDROID_LOG_VERBOSE, "SDL", "SDL audio: opening device for output"); - audioBuffer16Bit = is16Bit; - if ((*env)->CallStaticIntMethod(env, mAudioManagerClass, midAudioOpen, sampleRate, audioBuffer16Bit, audioBufferStereo, desiredBufferFrames) != 0) { - /* Error during audio initialization */ - __android_log_print(ANDROID_LOG_WARN, "SDL", "SDL audio: error on AudioTrack initialization!"); - return 0; - } + result = (*env)->CallStaticObjectMethod(env, mAudioManagerClass, midAudioOpen, spec->freq, audioformat, spec->channels, spec->samples); } + if (result == NULL) { + /* Error during audio initialization, error printed from Java */ + return SDL_SetError("Java-side initialization failed"); + } + + if ((*env)->GetArrayLength(env, (jintArray)result) != 4) { + return SDL_SetError("Unexpected results from Java, expected 4, got %d", (*env)->GetArrayLength(env, (jintArray)result)); + } + isCopy = JNI_FALSE; + resultElements = (*env)->GetIntArrayElements(env, (jintArray)result, &isCopy); + spec->freq = resultElements[0]; + audioformat = resultElements[1]; + switch (audioformat) { + case ENCODING_PCM_8BIT: + spec->format = AUDIO_U8; + break; + case ENCODING_PCM_16BIT: + spec->format = AUDIO_S16; + break; + case ENCODING_PCM_FLOAT: + spec->format = AUDIO_F32; + break; + default: + return SDL_SetError("Unexpected audio format from Java: %d\n", audioformat); + } + spec->channels = resultElements[2]; + spec->samples = resultElements[3]; + (*env)->ReleaseIntArrayElements(env, (jintArray)result, resultElements, JNI_ABORT); + (*env)->DeleteLocalRef(env, result); /* Allocating the audio buffer from the Java side and passing it as the return value for audioInit no longer works on * Android >= 4.2 due to a "stale global reference" error. So now we allocate this buffer directly from this side. */ - - if (is16Bit) { - jshortArray audioBufferLocal = (*env)->NewShortArray(env, desiredBufferFrames * (audioBufferStereo ? 2 : 1)); - if (audioBufferLocal) { - jbufobj = (*env)->NewGlobalRef(env, audioBufferLocal); - (*env)->DeleteLocalRef(env, audioBufferLocal); + switch (audioformat) { + case ENCODING_PCM_8BIT: + { + jbyteArray audioBufferLocal = (*env)->NewByteArray(env, spec->samples * spec->channels); + if (audioBufferLocal) { + jbufobj = (*env)->NewGlobalRef(env, audioBufferLocal); + (*env)->DeleteLocalRef(env, audioBufferLocal); + } } - } - else { - jbyteArray audioBufferLocal = (*env)->NewByteArray(env, desiredBufferFrames * (audioBufferStereo ? 2 : 1)); - if (audioBufferLocal) { - jbufobj = (*env)->NewGlobalRef(env, audioBufferLocal); - (*env)->DeleteLocalRef(env, audioBufferLocal); + break; + case ENCODING_PCM_16BIT: + { + jshortArray audioBufferLocal = (*env)->NewShortArray(env, spec->samples * spec->channels); + if (audioBufferLocal) { + jbufobj = (*env)->NewGlobalRef(env, audioBufferLocal); + (*env)->DeleteLocalRef(env, audioBufferLocal); + } } + break; + case ENCODING_PCM_FLOAT: + { + jfloatArray audioBufferLocal = (*env)->NewFloatArray(env, spec->samples * spec->channels); + if (audioBufferLocal) { + jbufobj = (*env)->NewGlobalRef(env, audioBufferLocal); + (*env)->DeleteLocalRef(env, audioBufferLocal); + } + } + break; + default: + return SDL_SetError("Unexpected audio format from Java: %d\n", audioformat); } if (jbufobj == NULL) { - __android_log_print(ANDROID_LOG_WARN, "SDL", "SDL audio: could not allocate an audio buffer!"); - return 0; + __android_log_print(ANDROID_LOG_WARN, "SDL", "SDL audio: could not allocate an audio buffer"); + return SDL_OutOfMemory(); } if (iscapture) { + captureBufferFormat = audioformat; captureBuffer = jbufobj; } else { + audioBufferFormat = audioformat; audioBuffer = jbufobj; } + numBufferFrames = (*env)->GetArrayLength(env, (jarray)jbufobj); - isCopy = JNI_FALSE; + if (!iscapture) { + isCopy = JNI_FALSE; - if (is16Bit) { - if (!iscapture) { - audioBufferPinned = (*env)->GetShortArrayElements(env, (jshortArray)audioBuffer, &isCopy); - } - audioBufferFrames = (*env)->GetArrayLength(env, (jshortArray)audioBuffer); - } else { - if (!iscapture) { + switch (audioformat) { + case ENCODING_PCM_8BIT: audioBufferPinned = (*env)->GetByteArrayElements(env, (jbyteArray)audioBuffer, &isCopy); + break; + case ENCODING_PCM_16BIT: + audioBufferPinned = (*env)->GetShortArrayElements(env, (jshortArray)audioBuffer, &isCopy); + break; + case ENCODING_PCM_FLOAT: + audioBufferPinned = (*env)->GetFloatArrayElements(env, (jfloatArray)audioBuffer, &isCopy); + break; + default: + return SDL_SetError("Unexpected audio format from Java: %d\n", audioformat); } - audioBufferFrames = (*env)->GetArrayLength(env, (jbyteArray)audioBuffer); } - - if (audioBufferStereo) { - audioBufferFrames /= 2; - } - - return audioBufferFrames; + return 0; } int Android_JNI_GetDisplayDPI(float *ddpi, float *xdpi, float *ydpi) @@ -1126,12 +1235,22 @@ void Android_JNI_WriteAudioBuffer(void) { JNIEnv *mAudioEnv = Android_JNI_GetEnv(); - if (audioBuffer16Bit) { - (*mAudioEnv)->ReleaseShortArrayElements(mAudioEnv, (jshortArray)audioBuffer, (jshort *)audioBufferPinned, JNI_COMMIT); - (*mAudioEnv)->CallStaticVoidMethod(mAudioEnv, mAudioManagerClass, midAudioWriteShortBuffer, (jshortArray)audioBuffer); - } else { + switch (audioBufferFormat) { + case ENCODING_PCM_8BIT: (*mAudioEnv)->ReleaseByteArrayElements(mAudioEnv, (jbyteArray)audioBuffer, (jbyte *)audioBufferPinned, JNI_COMMIT); (*mAudioEnv)->CallStaticVoidMethod(mAudioEnv, mAudioManagerClass, midAudioWriteByteBuffer, (jbyteArray)audioBuffer); + break; + case ENCODING_PCM_16BIT: + (*mAudioEnv)->ReleaseShortArrayElements(mAudioEnv, (jshortArray)audioBuffer, (jshort *)audioBufferPinned, JNI_COMMIT); + (*mAudioEnv)->CallStaticVoidMethod(mAudioEnv, mAudioManagerClass, midAudioWriteShortBuffer, (jshortArray)audioBuffer); + break; + case ENCODING_PCM_FLOAT: + (*mAudioEnv)->ReleaseFloatArrayElements(mAudioEnv, (jfloatArray)audioBuffer, (jfloat *)audioBufferPinned, JNI_COMMIT); + (*mAudioEnv)->CallStaticVoidMethod(mAudioEnv, mAudioManagerClass, midAudioWriteFloatBuffer, (jfloatArray)audioBuffer); + break; + default: + __android_log_print(ANDROID_LOG_WARN, "SDL", "SDL audio: unhandled audio buffer format"); + break; } /* JNI_COMMIT means the changes are committed to the VM but the buffer remains pinned */ @@ -1143,16 +1262,8 @@ int Android_JNI_CaptureAudioBuffer(void *buffer, int buflen) jboolean isCopy = JNI_FALSE; jint br; - if (captureBuffer16Bit) { - SDL_assert((*env)->GetArrayLength(env, (jshortArray)captureBuffer) == (buflen / 2)); - br = (*env)->CallStaticIntMethod(env, mAudioManagerClass, midCaptureReadShortBuffer, (jshortArray)captureBuffer, JNI_TRUE); - if (br > 0) { - jshort *ptr = (*env)->GetShortArrayElements(env, (jshortArray)captureBuffer, &isCopy); - br *= 2; - SDL_memcpy(buffer, ptr, br); - (*env)->ReleaseShortArrayElements(env, (jshortArray)captureBuffer, (jshort *)ptr, JNI_ABORT); - } - } else { + switch (captureBufferFormat) { + case ENCODING_PCM_8BIT: SDL_assert((*env)->GetArrayLength(env, (jshortArray)captureBuffer) == buflen); br = (*env)->CallStaticIntMethod(env, mAudioManagerClass, midCaptureReadByteBuffer, (jbyteArray)captureBuffer, JNI_TRUE); if (br > 0) { @@ -1160,27 +1271,75 @@ int Android_JNI_CaptureAudioBuffer(void *buffer, int buflen) SDL_memcpy(buffer, ptr, br); (*env)->ReleaseByteArrayElements(env, (jbyteArray)captureBuffer, (jbyte *)ptr, JNI_ABORT); } + break; + case ENCODING_PCM_16BIT: + SDL_assert((*env)->GetArrayLength(env, (jshortArray)captureBuffer) == (buflen / sizeof(Sint16))); + br = (*env)->CallStaticIntMethod(env, mAudioManagerClass, midCaptureReadShortBuffer, (jshortArray)captureBuffer, JNI_TRUE); + if (br > 0) { + jshort *ptr = (*env)->GetShortArrayElements(env, (jshortArray)captureBuffer, &isCopy); + br *= sizeof(Sint16); + SDL_memcpy(buffer, ptr, br); + (*env)->ReleaseShortArrayElements(env, (jshortArray)captureBuffer, (jshort *)ptr, JNI_ABORT); + } + break; + case ENCODING_PCM_FLOAT: + SDL_assert((*env)->GetArrayLength(env, (jfloatArray)captureBuffer) == (buflen / sizeof(float))); + br = (*env)->CallStaticIntMethod(env, mAudioManagerClass, midCaptureReadFloatBuffer, (jfloatArray)captureBuffer, JNI_TRUE); + if (br > 0) { + jfloat *ptr = (*env)->GetFloatArrayElements(env, (jfloatArray)captureBuffer, &isCopy); + br *= sizeof(float); + SDL_memcpy(buffer, ptr, br); + (*env)->ReleaseFloatArrayElements(env, (jfloatArray)captureBuffer, (jfloat *)ptr, JNI_ABORT); + } + break; + default: + __android_log_print(ANDROID_LOG_WARN, "SDL", "SDL audio: unhandled capture buffer format"); + break; } - - return (int) br; + return br; } void Android_JNI_FlushCapturedAudio(void) { JNIEnv *env = Android_JNI_GetEnv(); #if 0 /* !!! FIXME: this needs API 23, or it'll do blocking reads and never end. */ - if (captureBuffer16Bit) { - const jint len = (*env)->GetArrayLength(env, (jshortArray)captureBuffer); - while ((*env)->CallStaticIntMethod(env, mActivityClass, midCaptureReadShortBuffer, (jshortArray)captureBuffer, JNI_FALSE) == len) { /* spin */ } - } else { - const jint len = (*env)->GetArrayLength(env, (jbyteArray)captureBuffer); - while ((*env)->CallStaticIntMethod(env, mActivityClass, midCaptureReadByteBuffer, (jbyteArray)captureBuffer, JNI_FALSE) == len) { /* spin */ } + switch (captureBufferFormat) { + case ENCODING_PCM_8BIT: + { + const jint len = (*env)->GetArrayLength(env, (jbyteArray)captureBuffer); + while ((*env)->CallStaticIntMethod(env, mActivityClass, midCaptureReadByteBuffer, (jbyteArray)captureBuffer, JNI_FALSE) == len) { /* spin */ } + } + break; + case ENCODING_PCM_16BIT: + { + const jint len = (*env)->GetArrayLength(env, (jshortArray)captureBuffer); + while ((*env)->CallStaticIntMethod(env, mActivityClass, midCaptureReadShortBuffer, (jshortArray)captureBuffer, JNI_FALSE) == len) { /* spin */ } + } + break; + case ENCODING_PCM_FLOAT: + { + const jint len = (*env)->GetArrayLength(env, (jfloatArray)captureBuffer); + while ((*env)->CallStaticIntMethod(env, mActivityClass, midCaptureReadFloatBuffer, (jfloatArray)captureBuffer, JNI_FALSE) == len) { /* spin */ } + } + break; + default: + __android_log_print(ANDROID_LOG_WARN, "SDL", "SDL audio: flushing unhandled capture buffer format"); + break; } #else - if (captureBuffer16Bit) { - (*env)->CallStaticIntMethod(env, mAudioManagerClass, midCaptureReadShortBuffer, (jshortArray)captureBuffer, JNI_FALSE); - } else { + switch (captureBufferFormat) { + case ENCODING_PCM_8BIT: (*env)->CallStaticIntMethod(env, mAudioManagerClass, midCaptureReadByteBuffer, (jbyteArray)captureBuffer, JNI_FALSE); + break; + case ENCODING_PCM_16BIT: + (*env)->CallStaticIntMethod(env, mAudioManagerClass, midCaptureReadShortBuffer, (jshortArray)captureBuffer, JNI_FALSE); + break; + case ENCODING_PCM_FLOAT: + (*env)->CallStaticIntMethod(env, mAudioManagerClass, midCaptureReadFloatBuffer, (jfloatArray)captureBuffer, JNI_FALSE); + break; + default: + __android_log_print(ANDROID_LOG_WARN, "SDL", "SDL audio: flushing unhandled capture buffer format"); + break; } #endif } @@ -1846,12 +2005,17 @@ void Android_JNI_PollHapticDevices(void) (*env)->CallStaticVoidMethod(env, mControllerManagerClass, midPollHapticDevices); } -void Android_JNI_HapticRun(int device_id, int length) +void Android_JNI_HapticRun(int device_id, float intensity, int length) { JNIEnv *env = Android_JNI_GetEnv(); - (*env)->CallStaticVoidMethod(env, mControllerManagerClass, midHapticRun, device_id, length); + (*env)->CallStaticVoidMethod(env, mControllerManagerClass, midHapticRun, device_id, intensity, length); } +void Android_JNI_HapticStop(int device_id) +{ + JNIEnv *env = Android_JNI_GetEnv(); + (*env)->CallStaticVoidMethod(env, mControllerManagerClass, midHapticStop, device_id); +} /* See SDLActivity.java for constants. */ #define COMMAND_SET_KEEP_SCREEN_ON 5 @@ -2006,12 +2170,36 @@ void *SDL_AndroidGetActivity(void) return (*env)->CallStaticObjectMethod(env, mActivityClass, midGetContext); } +SDL_bool SDL_IsAndroidTablet(void) +{ + JNIEnv *env = Android_JNI_GetEnv(); + return (*env)->CallStaticBooleanMethod(env, mActivityClass, midIsTablet); +} + SDL_bool SDL_IsAndroidTV(void) { JNIEnv *env = Android_JNI_GetEnv(); return (*env)->CallStaticBooleanMethod(env, mActivityClass, midIsAndroidTV); } +SDL_bool SDL_IsChromebook(void) +{ + JNIEnv *env = Android_JNI_GetEnv(); + return (*env)->CallStaticBooleanMethod(env, mActivityClass, midIsChromebook); +} + +SDL_bool SDL_IsDeXMode(void) +{ + JNIEnv *env = Android_JNI_GetEnv(); + return (*env)->CallStaticBooleanMethod(env, mActivityClass, midIsDeXMode); +} + +void SDL_AndroidBackButton(void) +{ + JNIEnv *env = Android_JNI_GetEnv(); + return (*env)->CallStaticVoidMethod(env, mActivityClass, midManualBackButton); +} + const char * SDL_AndroidGetInternalStoragePath(void) { static char *s_AndroidInternalFilesPath = NULL; @@ -2166,6 +2354,48 @@ void Android_JNI_GetManifestEnvironmentVariables(void) } } +int Android_JNI_CreateCustomCursor(SDL_Surface *surface, int hot_x, int hot_y) +{ + JNIEnv *mEnv = Android_JNI_GetEnv(); + int custom_cursor = 0; + jintArray pixels; + pixels = (*mEnv)->NewIntArray(mEnv, surface->w * surface->h); + if (pixels) { + (*mEnv)->SetIntArrayRegion(mEnv, pixels, 0, surface->w * surface->h, (int *)surface->pixels); + custom_cursor = (*mEnv)->CallStaticIntMethod(mEnv, mActivityClass, midCreateCustomCursor, pixels, surface->w, surface->h, hot_x, hot_y); + (*mEnv)->DeleteLocalRef(mEnv, pixels); + } else { + SDL_OutOfMemory(); + } + return custom_cursor; +} + + +SDL_bool Android_JNI_SetCustomCursor(int cursorID) +{ + JNIEnv *mEnv = Android_JNI_GetEnv(); + return (*mEnv)->CallStaticBooleanMethod(mEnv, mActivityClass, midSetCustomCursor, cursorID); +} + +SDL_bool Android_JNI_SetSystemCursor(int cursorID) +{ + JNIEnv *mEnv = Android_JNI_GetEnv(); + return (*mEnv)->CallStaticBooleanMethod(mEnv, mActivityClass, midSetSystemCursor, cursorID); +} + +SDL_bool Android_JNI_SupportsRelativeMouse() +{ + JNIEnv *mEnv = Android_JNI_GetEnv(); + return (*mEnv)->CallStaticBooleanMethod(mEnv, mActivityClass, midSupportsRelativeMouse); +} + +SDL_bool Android_JNI_SetRelativeMouseEnabled(SDL_bool enabled) +{ + JNIEnv *mEnv = Android_JNI_GetEnv(); + return (*mEnv)->CallStaticBooleanMethod(mEnv, mActivityClass, midSetRelativeMouseEnabled, (enabled == 1)); +} + + #endif /* __ANDROID__ */ /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/core/android/SDL_android.h b/Engine/lib/sdl/src/core/android/SDL_android.h index c800dc651..b2ff32ea6 100644 --- a/Engine/lib/sdl/src/core/android/SDL_android.h +++ b/Engine/lib/sdl/src/core/android/SDL_android.h @@ -19,6 +19,7 @@ 3. This notice may not be removed or altered from any source distribution. */ #include "../../SDL_internal.h" +#include "SDL_system.h" /* Set up for C function definitions, even when using C++ */ #ifdef __cplusplus @@ -30,6 +31,7 @@ extern "C" { #include #include +#include "SDL_audio.h" #include "SDL_rect.h" /* Interface from the SDL library into the Android Java activity */ @@ -46,13 +48,17 @@ extern ANativeWindow* Android_JNI_GetNativeWindow(void); extern int Android_JNI_GetDisplayDPI(float *ddpi, float *xdpi, float *ydpi); /* Audio support */ -extern int Android_JNI_OpenAudioDevice(int iscapture, int sampleRate, int is16Bit, int channelCount, int desiredBufferFrames); +extern int Android_JNI_OpenAudioDevice(int iscapture, SDL_AudioSpec *spec); extern void* Android_JNI_GetAudioBuffer(void); extern void Android_JNI_WriteAudioBuffer(void); extern int Android_JNI_CaptureAudioBuffer(void *buffer, int buflen); extern void Android_JNI_FlushCapturedAudio(void); extern void Android_JNI_CloseAudioDevice(const int iscapture); +/* Detecting device type */ +extern SDL_bool Android_IsDeXMode(); +extern SDL_bool Android_IsChromebook(); + #include "SDL_rwops.h" int Android_JNI_FileOpen(SDL_RWops* ctx, const char* fileName, const char* mode); @@ -78,14 +84,16 @@ void Android_JNI_PollInputDevices(void); /* Haptic support */ void Android_JNI_PollHapticDevices(void); -void Android_JNI_HapticRun(int device_id, int length); +void Android_JNI_HapticRun(int device_id, float intensity, int length); +void Android_JNI_HapticStop(int device_id); /* Video */ void Android_JNI_SuspendScreenSaver(SDL_bool suspend); /* Touch support */ -int Android_JNI_GetTouchDeviceIds(int **ids); +int Android_JNI_InitTouch(void); void Android_JNI_SetSeparateMouseAndTouch(SDL_bool new_value); +int Android_JNI_GetTouchDeviceIds(int **ids); /* Threads */ #include @@ -102,6 +110,21 @@ JNIEXPORT void JNICALL SDL_Android_Init(JNIEnv* mEnv, jclass cls); #include "SDL_messagebox.h" int Android_JNI_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid); +/* Cursor support */ +int Android_JNI_CreateCustomCursor(SDL_Surface *surface, int hot_x, int hot_y); +SDL_bool Android_JNI_SetCustomCursor(int cursorID); +SDL_bool Android_JNI_SetSystemCursor(int cursorID); + +/* Relative mouse support */ +SDL_bool Android_JNI_SupportsRelativeMouse(void); +SDL_bool Android_JNI_SetRelativeMouseEnabled(SDL_bool enabled); + + +SDL_bool SDL_IsAndroidTablet(void); +SDL_bool SDL_IsAndroidTV(void); +SDL_bool SDL_IsChromebook(void); +SDL_bool SDL_IsDeXMode(void); + /* Ends C function definitions when using C++ */ #ifdef __cplusplus /* *INDENT-OFF* */ diff --git a/Engine/lib/sdl/src/core/linux/SDL_dbus.c b/Engine/lib/sdl/src/core/linux/SDL_dbus.c index ff4f0fe67..e0d99725d 100644 --- a/Engine/lib/sdl/src/core/linux/SDL_dbus.c +++ b/Engine/lib/sdl/src/core/linux/SDL_dbus.c @@ -173,17 +173,29 @@ SDL_DBus_CallMethodInternal(DBusConnection *conn, const char *node, const char * if (conn) { DBusMessage *msg = dbus.message_new_method_call(node, path, interface, method); if (msg) { - int firstarg = va_arg(ap, int); + int firstarg; + va_list ap_reply; + va_copy(ap_reply, ap); /* copy the arg list so we don't compete with D-Bus for it */ + firstarg = va_arg(ap, int); if ((firstarg == DBUS_TYPE_INVALID) || dbus.message_append_args_valist(msg, firstarg, ap)) { DBusMessage *reply = dbus.connection_send_with_reply_and_block(conn, msg, 300, NULL); if (reply) { - firstarg = va_arg(ap, int); - if ((firstarg == DBUS_TYPE_INVALID) || dbus.message_get_args_valist(reply, NULL, firstarg, ap)) { + /* skip any input args, get to output args. */ + while ((firstarg = va_arg(ap_reply, int)) != DBUS_TYPE_INVALID) { + /* we assume D-Bus already validated all this. */ + { void *dumpptr = va_arg(ap_reply, void*); (void) dumpptr; } + if (firstarg == DBUS_TYPE_ARRAY) { + { const int dumpint = va_arg(ap_reply, int); (void) dumpint; } + } + } + firstarg = va_arg(ap_reply, int); + if ((firstarg == DBUS_TYPE_INVALID) || dbus.message_get_args_valist(reply, NULL, firstarg, ap_reply)) { retval = SDL_TRUE; } dbus.message_unref(reply); } } + va_end(ap_reply); dbus.message_unref(msg); } } diff --git a/Engine/lib/sdl/src/core/linux/SDL_dbus.h b/Engine/lib/sdl/src/core/linux/SDL_dbus.h index 062543d46..aa787f223 100644 --- a/Engine/lib/sdl/src/core/linux/SDL_dbus.h +++ b/Engine/lib/sdl/src/core/linux/SDL_dbus.h @@ -39,9 +39,8 @@ typedef struct SDL_DBusContext { void (*bus_add_match)(DBusConnection *, const char *, DBusError *); DBusConnection * (*connection_open_private)(const char *, DBusError *); void (*connection_set_exit_on_disconnect)(DBusConnection *, dbus_bool_t); - dbus_bool_t (*connection_get_is_connected)(DBusConnection *); - dbus_bool_t (*connection_add_filter)(DBusConnection *, DBusHandleMessageFunction, - void *, DBusFreeFunction); + dbus_bool_t (*connection_get_is_connected)(DBusConnection *); + dbus_bool_t (*connection_add_filter)(DBusConnection *, DBusHandleMessageFunction, void *, DBusFreeFunction); dbus_bool_t (*connection_try_register_object_path)(DBusConnection *, const char *, const DBusObjectPathVTable *, void *, DBusError *); dbus_bool_t (*connection_send)(DBusConnection *, DBusMessage *, dbus_uint32_t *); @@ -51,7 +50,7 @@ typedef struct SDL_DBusContext { void (*connection_flush)(DBusConnection *); dbus_bool_t (*connection_read_write)(DBusConnection *, int); DBusDispatchStatus (*connection_dispatch)(DBusConnection *); - dbus_bool_t (*message_is_signal)(DBusMessage *, const char *, const char *); + dbus_bool_t (*message_is_signal)(DBusMessage *, const char *, const char *); DBusMessage *(*message_new_method_call)(const char *, const char *, const char *, const char *); dbus_bool_t (*message_append_args)(DBusMessage *, int, ...); dbus_bool_t (*message_append_args_valist)(DBusMessage *, int, va_list); @@ -61,7 +60,7 @@ typedef struct SDL_DBusContext { dbus_bool_t (*message_iter_next)(DBusMessageIter *); void (*message_iter_get_basic)(DBusMessageIter *, void *); int (*message_iter_get_arg_type)(DBusMessageIter *); - void (*message_iter_recurse)(DBusMessageIter *, DBusMessageIter *); + void (*message_iter_recurse)(DBusMessageIter *, DBusMessageIter *); void (*message_unref)(DBusMessage *); void (*error_init)(DBusError *); dbus_bool_t (*error_is_set)(const DBusError *); diff --git a/Engine/lib/sdl/src/core/linux/SDL_evdev.c b/Engine/lib/sdl/src/core/linux/SDL_evdev.c index a50692617..5443c2198 100644 --- a/Engine/lib/sdl/src/core/linux/SDL_evdev.c +++ b/Engine/lib/sdl/src/core/linux/SDL_evdev.c @@ -101,6 +101,7 @@ typedef struct SDL_EVDEV_PrivateData SDL_EVDEV_keyboard_state *kbd; } SDL_EVDEV_PrivateData; +#undef _THIS #define _THIS SDL_EVDEV_PrivateData *_this static _THIS = NULL; diff --git a/Engine/lib/sdl/src/core/linux/SDL_evdev_kbd.c b/Engine/lib/sdl/src/core/linux/SDL_evdev_kbd.c index 250e6440b..00a3a54d6 100644 --- a/Engine/lib/sdl/src/core/linux/SDL_evdev_kbd.c +++ b/Engine/lib/sdl/src/core/linux/SDL_evdev_kbd.c @@ -21,6 +21,7 @@ #include "../../SDL_internal.h" #include "SDL_evdev_kbd.h" +#include "SDL_hints.h" #ifdef SDL_INPUT_LINUXKD @@ -34,6 +35,8 @@ #include #include /* for TIOCL_GETSHIFTSTATE */ +#include + #include "../../events/SDL_events_c.h" #include "SDL_evdev_kbd_default_accents.h" #include "SDL_evdev_kbd_default_keymap.h" @@ -191,6 +194,151 @@ static int SDL_EVDEV_kbd_load_keymaps(SDL_EVDEV_keyboard_state *kbd) return 0; } +static SDL_EVDEV_keyboard_state * kbd_cleanup_state = NULL; +static int kbd_cleanup_sigactions_installed = 0; +static int kbd_cleanup_atexit_installed = 0; + +static struct sigaction old_sigaction[NSIG]; + +static int fatal_signals[] = +{ + /* Handlers for SIGTERM and SIGINT are installed in SDL_QuitInit. */ + SIGHUP, SIGQUIT, SIGILL, SIGABRT, + SIGFPE, SIGSEGV, SIGPIPE, SIGBUS, + SIGSYS +}; + +static void kbd_cleanup(void) +{ + SDL_EVDEV_keyboard_state* kbd = kbd_cleanup_state; + if (kbd == NULL) { + return; + } + kbd_cleanup_state = NULL; + + fprintf(stderr, "(SDL restoring keyboard) "); + ioctl(kbd->console_fd, KDSKBMODE, kbd->old_kbd_mode); +} + +void +SDL_EVDEV_kbd_reraise_signal(int sig) +{ + raise(sig); +} + +siginfo_t* SDL_EVDEV_kdb_cleanup_siginfo = NULL; +void* SDL_EVDEV_kdb_cleanup_ucontext = NULL; + +static void kbd_cleanup_signal_action(int signum, siginfo_t* info, void* ucontext) +{ + struct sigaction* old_action_p = &(old_sigaction[signum]); + sigset_t sigset; + + /* Restore original signal handler before going any further. */ + sigaction(signum, old_action_p, NULL); + + /* Unmask current signal. */ + sigemptyset(&sigset); + sigaddset(&sigset, signum); + sigprocmask(SIG_UNBLOCK, &sigset, NULL); + + /* Save original signal info and context for archeologists. */ + SDL_EVDEV_kdb_cleanup_siginfo = info; + SDL_EVDEV_kdb_cleanup_ucontext = ucontext; + + /* Restore keyboard. */ + kbd_cleanup(); + + /* Reraise signal. */ + SDL_EVDEV_kbd_reraise_signal(signum); +} + +static void kbd_unregister_emerg_cleanup() +{ + int tabidx, signum; + + kbd_cleanup_state = NULL; + + if (!kbd_cleanup_sigactions_installed) { + return; + } + kbd_cleanup_sigactions_installed = 0; + + for (tabidx = 0; tabidx < sizeof(fatal_signals) / sizeof(fatal_signals[0]); ++tabidx) { + struct sigaction* old_action_p; + struct sigaction cur_action; + signum = fatal_signals[tabidx]; + old_action_p = &(old_sigaction[signum]); + + /* Examine current signal action */ + if (sigaction(signum, NULL, &cur_action)) + continue; + + /* Check if action installed and not modifed */ + if (!(cur_action.sa_flags & SA_SIGINFO) + || cur_action.sa_sigaction != &kbd_cleanup_signal_action) + continue; + + /* Restore original action */ + sigaction(signum, old_action_p, NULL); + } +} + +static void kbd_cleanup_atexit(void) +{ + /* Restore keyboard. */ + kbd_cleanup(); + + /* Try to restore signal handlers in case shared library is being unloaded */ + kbd_unregister_emerg_cleanup(); +} + +static void kbd_register_emerg_cleanup(SDL_EVDEV_keyboard_state * kbd) +{ + int tabidx, signum; + + if (kbd_cleanup_state != NULL) { + return; + } + kbd_cleanup_state = kbd; + + if (!kbd_cleanup_atexit_installed) { + /* Since glibc 2.2.3, atexit() (and on_exit(3)) can be used within a shared library to establish + * functions that are called when the shared library is unloaded. + * -- man atexit(3) + */ + atexit(kbd_cleanup_atexit); + kbd_cleanup_atexit_installed = 1; + } + + if (kbd_cleanup_sigactions_installed) { + return; + } + kbd_cleanup_sigactions_installed = 1; + + for (tabidx = 0; tabidx < sizeof(fatal_signals) / sizeof(fatal_signals[0]); ++tabidx) { + struct sigaction* old_action_p; + struct sigaction new_action; + signum = fatal_signals[tabidx]; + old_action_p = &(old_sigaction[signum]); + if (sigaction(signum, NULL, old_action_p)) + continue; + + /* Skip SIGHUP and SIGPIPE if handler is already installed + * - assume the handler will do the cleanup + */ + if ((signum == SIGHUP || signum == SIGPIPE) + && (old_action_p->sa_handler != SIG_DFL + || (void (*)(int))old_action_p->sa_sigaction != SIG_DFL)) + continue; + + new_action = *old_action_p; + new_action.sa_flags |= SA_SIGINFO; + new_action.sa_sigaction = &kbd_cleanup_signal_action; + sigaction(signum, &new_action, NULL); + } +} + SDL_EVDEV_keyboard_state * SDL_EVDEV_kbd_init(void) { @@ -238,10 +386,20 @@ SDL_EVDEV_kbd_init(void) kbd->key_maps = default_key_maps; } - /* Mute the keyboard so keystrokes only generate evdev events - * and do not leak through to the console - */ - ioctl(kbd->console_fd, KDSKBMODE, K_OFF); + /* Allow inhibiting keyboard mute with env. variable for debugging etc. */ + if (getenv("SDL_INPUT_LINUX_KEEP_KBD") == NULL) { + /* Mute the keyboard so keystrokes only generate evdev events + * and do not leak through to the console + */ + ioctl(kbd->console_fd, KDSKBMODE, K_OFF); + + /* Make sure to restore keyboard if application fails to call + * SDL_Quit before exit or fatal signal is raised. + */ + if (!SDL_GetHintBoolean(SDL_HINT_NO_SIGNAL_HANDLERS, SDL_FALSE)) { + kbd_register_emerg_cleanup(kbd); + } + } } #ifdef DUMP_ACCENTS @@ -260,6 +418,8 @@ SDL_EVDEV_kbd_quit(SDL_EVDEV_keyboard_state *kbd) return; } + kbd_unregister_emerg_cleanup(); + if (kbd->console_fd >= 0) { /* Restore the original keyboard mode */ ioctl(kbd->console_fd, KDSKBMODE, kbd->old_kbd_mode); @@ -609,7 +769,10 @@ SDL_EVDEV_kbd_keycode(SDL_EVDEV_keyboard_state *kbd, unsigned int keycode, int d shift_final = (kbd->shift_state | kbd->slockstate) ^ kbd->lockstate; key_map = kbd->key_maps[shift_final]; if (!key_map) { + /* Unsupported shift state (e.g. ctrl = 4, alt = 8), just reset to the default state */ + kbd->shift_state = 0; kbd->slockstate = 0; + kbd->lockstate = 0; return; } diff --git a/Engine/lib/sdl/src/core/linux/SDL_evdev_kbd.h b/Engine/lib/sdl/src/core/linux/SDL_evdev_kbd.h index 831ba3ade..5e51cdd17 100644 --- a/Engine/lib/sdl/src/core/linux/SDL_evdev_kbd.h +++ b/Engine/lib/sdl/src/core/linux/SDL_evdev_kbd.h @@ -19,6 +19,9 @@ 3. This notice may not be removed or altered from any source distribution. */ +#ifndef SDL_evdev_kbd_h_ +#define SDL_evdev_kbd_h_ + struct SDL_EVDEV_keyboard_state; typedef struct SDL_EVDEV_keyboard_state SDL_EVDEV_keyboard_state; @@ -26,4 +29,6 @@ extern SDL_EVDEV_keyboard_state *SDL_EVDEV_kbd_init(void); extern void SDL_EVDEV_kbd_keycode(SDL_EVDEV_keyboard_state *state, unsigned int keycode, int down); extern void SDL_EVDEV_kbd_quit(SDL_EVDEV_keyboard_state *state); +#endif /* SDL_evdev_kbd_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/core/linux/SDL_udev.c b/Engine/lib/sdl/src/core/linux/SDL_udev.c index dfbeb790a..751e2cabb 100644 --- a/Engine/lib/sdl/src/core/linux/SDL_udev.c +++ b/Engine/lib/sdl/src/core/linux/SDL_udev.c @@ -63,7 +63,7 @@ SDL_UDEV_load_syms(void) { /* cast funcs to char* first, to please GCC's strict aliasing rules. */ #define SDL_UDEV_SYM(x) \ - if (!SDL_UDEV_load_sym(#x, (void **) (char *) & _this->x)) return -1 + if (!SDL_UDEV_load_sym(#x, (void **) (char *) & _this->syms.x)) return -1 SDL_UDEV_SYM(udev_device_get_action); SDL_UDEV_SYM(udev_device_get_devnode); @@ -100,7 +100,7 @@ static SDL_bool SDL_UDEV_hotplug_update_available(void) { if (_this->udev_mon != NULL) { - const int fd = _this->udev_monitor_get_fd(_this->udev_mon); + const int fd = _this->syms.udev_monitor_get_fd(_this->udev_mon); if (SDL_IOReady(fd, SDL_FALSE, 0)) { return SDL_TRUE; } @@ -130,21 +130,21 @@ SDL_UDEV_Init(void) * Listen for input devices (mouse, keyboard, joystick, etc) and sound devices */ - _this->udev = _this->udev_new(); + _this->udev = _this->syms.udev_new(); if (_this->udev == NULL) { SDL_UDEV_Quit(); return SDL_SetError("udev_new() failed"); } - _this->udev_mon = _this->udev_monitor_new_from_netlink(_this->udev, "udev"); + _this->udev_mon = _this->syms.udev_monitor_new_from_netlink(_this->udev, "udev"); if (_this->udev_mon == NULL) { SDL_UDEV_Quit(); return SDL_SetError("udev_monitor_new_from_netlink() failed"); } - _this->udev_monitor_filter_add_match_subsystem_devtype(_this->udev_mon, "input", NULL); - _this->udev_monitor_filter_add_match_subsystem_devtype(_this->udev_mon, "sound", NULL); - _this->udev_monitor_enable_receiving(_this->udev_mon); + _this->syms.udev_monitor_filter_add_match_subsystem_devtype(_this->udev_mon, "input", NULL); + _this->syms.udev_monitor_filter_add_match_subsystem_devtype(_this->udev_mon, "sound", NULL); + _this->syms.udev_monitor_enable_receiving(_this->udev_mon); /* Do an initial scan of existing devices */ SDL_UDEV_Scan(); @@ -170,11 +170,11 @@ SDL_UDEV_Quit(void) if (_this->ref_count < 1) { if (_this->udev_mon != NULL) { - _this->udev_monitor_unref(_this->udev_mon); + _this->syms.udev_monitor_unref(_this->udev_mon); _this->udev_mon = NULL; } if (_this->udev != NULL) { - _this->udev_unref(_this->udev); + _this->syms.udev_unref(_this->udev); _this->udev = NULL; } @@ -202,28 +202,28 @@ SDL_UDEV_Scan(void) return; } - enumerate = _this->udev_enumerate_new(_this->udev); + enumerate = _this->syms.udev_enumerate_new(_this->udev); if (enumerate == NULL) { SDL_UDEV_Quit(); SDL_SetError("udev_enumerate_new() failed"); return; } - _this->udev_enumerate_add_match_subsystem(enumerate, "input"); - _this->udev_enumerate_add_match_subsystem(enumerate, "sound"); + _this->syms.udev_enumerate_add_match_subsystem(enumerate, "input"); + _this->syms.udev_enumerate_add_match_subsystem(enumerate, "sound"); - _this->udev_enumerate_scan_devices(enumerate); - devs = _this->udev_enumerate_get_list_entry(enumerate); - for (item = devs; item; item = _this->udev_list_entry_get_next(item)) { - const char *path = _this->udev_list_entry_get_name(item); - struct udev_device *dev = _this->udev_device_new_from_syspath(_this->udev, path); + _this->syms.udev_enumerate_scan_devices(enumerate); + devs = _this->syms.udev_enumerate_get_list_entry(enumerate); + for (item = devs; item; item = _this->syms.udev_list_entry_get_next(item)) { + const char *path = _this->syms.udev_list_entry_get_name(item); + struct udev_device *dev = _this->syms.udev_device_new_from_syspath(_this->udev, path); if (dev != NULL) { device_event(SDL_UDEV_DEVICEADDED, dev); - _this->udev_device_unref(dev); + _this->syms.udev_device_unref(dev); } } - _this->udev_enumerate_unref(enumerate); + _this->syms.udev_enumerate_unref(enumerate); } @@ -305,7 +305,7 @@ static void get_caps(struct udev_device *dev, struct udev_device *pdev, const ch unsigned long v; SDL_memset(bitmask, 0, bitmask_len*sizeof(*bitmask)); - value = _this->udev_device_get_sysattr_value(pdev, attr); + value = _this->syms.udev_device_get_sysattr_value(pdev, attr); if (!value) { return; } @@ -340,8 +340,8 @@ guess_device_class(struct udev_device *dev) /* walk up the parental chain until we find the real input device; the * argument is very likely a subdevice of this, like eventN */ pdev = dev; - while (pdev && !_this->udev_device_get_sysattr_value(pdev, "capabilities/ev")) { - pdev = _this->udev_device_get_parent_with_subsystem_devtype(pdev, "input", NULL); + while (pdev && !_this->syms.udev_device_get_sysattr_value(pdev, "capabilities/ev")) { + pdev = _this->syms.udev_device_get_parent_with_subsystem_devtype(pdev, "input", NULL); } if (!pdev) { return 0; @@ -405,28 +405,28 @@ device_event(SDL_UDEV_deviceevent type, struct udev_device *dev) const char *path; SDL_UDEV_CallbackList *item; - path = _this->udev_device_get_devnode(dev); + path = _this->syms.udev_device_get_devnode(dev); if (path == NULL) { return; } - subsystem = _this->udev_device_get_subsystem(dev); + subsystem = _this->syms.udev_device_get_subsystem(dev); if (SDL_strcmp(subsystem, "sound") == 0) { devclass = SDL_UDEV_DEVICE_SOUND; } else if (SDL_strcmp(subsystem, "input") == 0) { /* udev rules reference: http://cgit.freedesktop.org/systemd/systemd/tree/src/udev/udev-builtin-input_id.c */ - val = _this->udev_device_get_property_value(dev, "ID_INPUT_JOYSTICK"); + val = _this->syms.udev_device_get_property_value(dev, "ID_INPUT_JOYSTICK"); if (val != NULL && SDL_strcmp(val, "1") == 0 ) { devclass |= SDL_UDEV_DEVICE_JOYSTICK; } - val = _this->udev_device_get_property_value(dev, "ID_INPUT_MOUSE"); + val = _this->syms.udev_device_get_property_value(dev, "ID_INPUT_MOUSE"); if (val != NULL && SDL_strcmp(val, "1") == 0 ) { devclass |= SDL_UDEV_DEVICE_MOUSE; } - val = _this->udev_device_get_property_value(dev, "ID_INPUT_TOUCHSCREEN"); + val = _this->syms.udev_device_get_property_value(dev, "ID_INPUT_TOUCHSCREEN"); if (val != NULL && SDL_strcmp(val, "1") == 0 ) { devclass |= SDL_UDEV_DEVICE_TOUCHSCREEN; } @@ -437,14 +437,14 @@ device_event(SDL_UDEV_deviceevent type, struct udev_device *dev) Ref: http://cgit.freedesktop.org/systemd/systemd/tree/src/udev/udev-builtin-input_id.c#n183 */ - val = _this->udev_device_get_property_value(dev, "ID_INPUT_KEY"); + val = _this->syms.udev_device_get_property_value(dev, "ID_INPUT_KEY"); if (val != NULL && SDL_strcmp(val, "1") == 0 ) { devclass |= SDL_UDEV_DEVICE_KEYBOARD; } if (devclass == 0) { /* Fall back to old style input classes */ - val = _this->udev_device_get_property_value(dev, "ID_CLASS"); + val = _this->syms.udev_device_get_property_value(dev, "ID_CLASS"); if (val != NULL) { if (SDL_strcmp(val, "joystick") == 0) { devclass = SDL_UDEV_DEVICE_JOYSTICK; @@ -481,11 +481,11 @@ SDL_UDEV_Poll(void) } while (SDL_UDEV_hotplug_update_available()) { - dev = _this->udev_monitor_receive_device(_this->udev_mon); + dev = _this->syms.udev_monitor_receive_device(_this->udev_mon); if (dev == NULL) { break; } - action = _this->udev_device_get_action(dev); + action = _this->syms.udev_device_get_action(dev); if (SDL_strcmp(action, "add") == 0) { /* Wait for the device to finish initialization */ @@ -496,7 +496,7 @@ SDL_UDEV_Poll(void) device_event(SDL_UDEV_DEVICEREMOVED, dev); } - _this->udev_device_unref(dev); + _this->syms.udev_device_unref(dev); } } @@ -547,6 +547,22 @@ SDL_UDEV_DelCallback(SDL_UDEV_Callback cb) } +const SDL_UDEV_Symbols * +SDL_UDEV_GetUdevSyms(void) +{ + if (SDL_UDEV_Init() < 0) { + SDL_SetError("Could not initialize UDEV"); + return NULL; + } + + return &_this->syms; +} + +void +SDL_UDEV_ReleaseUdevSyms(void) +{ + SDL_UDEV_Quit(); +} #endif /* SDL_USE_LIBUDEV */ diff --git a/Engine/lib/sdl/src/core/linux/SDL_udev.h b/Engine/lib/sdl/src/core/linux/SDL_udev.h index edf51870b..8be74340a 100644 --- a/Engine/lib/sdl/src/core/linux/SDL_udev.h +++ b/Engine/lib/sdl/src/core/linux/SDL_udev.h @@ -64,22 +64,13 @@ typedef struct SDL_UDEV_CallbackList { struct SDL_UDEV_CallbackList *next; } SDL_UDEV_CallbackList; -typedef struct SDL_UDEV_PrivateData -{ - const char *udev_library; - void *udev_handle; - struct udev *udev; - struct udev_monitor *udev_mon; - int ref_count; - SDL_UDEV_CallbackList *first, *last; - - /* Function pointers */ +typedef struct SDL_UDEV_Symbols { const char *(*udev_device_get_action)(struct udev_device *); const char *(*udev_device_get_devnode)(struct udev_device *); const char *(*udev_device_get_subsystem)(struct udev_device *); - struct udev_device *(*udev_device_get_parent_with_subsystem_devtype)(struct udev_device *udev_device, const char *subsystem, const char *devtype); + struct udev_device *(*udev_device_get_parent_with_subsystem_devtype)(struct udev_device *udev_device, const char *subsystem, const char *devtype); const char *(*udev_device_get_property_value)(struct udev_device *, const char *); - const char *(*udev_device_get_sysattr_value)(struct udev_device *udev_device, const char *sysattr); + const char *(*udev_device_get_sysattr_value)(struct udev_device *udev_device, const char *sysattr); struct udev_device *(*udev_device_new_from_syspath)(struct udev *, const char *); void (*udev_device_unref)(struct udev_device *); int (*udev_enumerate_add_match_property)(struct udev_enumerate *, const char *, const char *); @@ -100,6 +91,19 @@ typedef struct SDL_UDEV_PrivateData void (*udev_unref)(struct udev *); struct udev_device * (*udev_device_new_from_devnum)(struct udev *udev, char type, dev_t devnum); dev_t (*udev_device_get_devnum) (struct udev_device *udev_device); +} SDL_UDEV_Symbols; + +typedef struct SDL_UDEV_PrivateData +{ + const char *udev_library; + void *udev_handle; + struct udev *udev; + struct udev_monitor *udev_mon; + int ref_count; + SDL_UDEV_CallbackList *first, *last; + + /* Function pointers */ + SDL_UDEV_Symbols syms; } SDL_UDEV_PrivateData; extern int SDL_UDEV_Init(void); @@ -110,8 +114,8 @@ extern void SDL_UDEV_Poll(void); extern void SDL_UDEV_Scan(void); extern int SDL_UDEV_AddCallback(SDL_UDEV_Callback cb); extern void SDL_UDEV_DelCallback(SDL_UDEV_Callback cb); - - +extern const SDL_UDEV_Symbols *SDL_UDEV_GetUdevSyms(void); +extern void SDL_UDEV_ReleaseUdevSyms(void); #endif /* HAVE_LIBUDEV_H */ diff --git a/Engine/lib/sdl/src/core/windows/SDL_windows.c b/Engine/lib/sdl/src/core/windows/SDL_windows.c index 66240435f..4da7d0775 100644 --- a/Engine/lib/sdl/src/core/windows/SDL_windows.c +++ b/Engine/lib/sdl/src/core/windows/SDL_windows.c @@ -31,6 +31,9 @@ #ifndef _WIN32_WINNT_VISTA #define _WIN32_WINNT_VISTA 0x0600 #endif +#ifndef _WIN32_WINNT_WIN7 +#define _WIN32_WINNT_WIN7 0x0601 +#endif /* Sets an error message based on an HRESULT */ diff --git a/Engine/lib/sdl/src/core/winrt/SDL_winrtapp_xaml.cpp b/Engine/lib/sdl/src/core/winrt/SDL_winrtapp_xaml.cpp index 9789d036f..7e2aac8a7 100644 --- a/Engine/lib/sdl/src/core/winrt/SDL_winrtapp_xaml.cpp +++ b/Engine/lib/sdl/src/core/winrt/SDL_winrtapp_xaml.cpp @@ -44,7 +44,7 @@ SDL_bool WINRT_XAMLWasEnabled = SDL_FALSE; #if WINAPI_FAMILY == WINAPI_FAMILY_APP extern "C" ISwapChainBackgroundPanelNative * WINRT_GlobalSwapChainBackgroundPanelNative = NULL; -static Windows::Foundation::EventRegistrationToken WINRT_XAMLAppEventToken; +static Windows::Foundation::EventRegistrationToken WINRT_XAMLAppEventToken; #endif diff --git a/Engine/lib/sdl/src/cpuinfo/SDL_cpuinfo.c b/Engine/lib/sdl/src/cpuinfo/SDL_cpuinfo.c index 4e2c0f101..441035883 100644 --- a/Engine/lib/sdl/src/cpuinfo/SDL_cpuinfo.c +++ b/Engine/lib/sdl/src/cpuinfo/SDL_cpuinfo.c @@ -22,6 +22,7 @@ #include "SDL_config.h" #else #include "../SDL_internal.h" +#include "SDL_simd.h" #endif #if defined(__WIN32__) @@ -38,6 +39,7 @@ /* CPU feature detection for SDL */ #include "SDL_cpuinfo.h" +#include "SDL_assert.h" #ifdef HAVE_SYSCONF #include @@ -76,18 +78,19 @@ #endif #endif -#define CPU_HAS_RDTSC 0x00000001 -#define CPU_HAS_ALTIVEC 0x00000002 -#define CPU_HAS_MMX 0x00000004 -#define CPU_HAS_3DNOW 0x00000008 -#define CPU_HAS_SSE 0x00000010 -#define CPU_HAS_SSE2 0x00000020 -#define CPU_HAS_SSE3 0x00000040 -#define CPU_HAS_SSE41 0x00000100 -#define CPU_HAS_SSE42 0x00000200 -#define CPU_HAS_AVX 0x00000400 -#define CPU_HAS_AVX2 0x00000800 -#define CPU_HAS_NEON 0x00001000 +#define CPU_HAS_RDTSC (1 << 0) +#define CPU_HAS_ALTIVEC (1 << 1) +#define CPU_HAS_MMX (1 << 2) +#define CPU_HAS_3DNOW (1 << 3) +#define CPU_HAS_SSE (1 << 4) +#define CPU_HAS_SSE2 (1 << 5) +#define CPU_HAS_SSE3 (1 << 6) +#define CPU_HAS_SSE41 (1 << 7) +#define CPU_HAS_SSE42 (1 << 8) +#define CPU_HAS_AVX (1 << 9) +#define CPU_HAS_AVX2 (1 << 10) +#define CPU_HAS_NEON (1 << 11) +#define CPU_HAS_AVX512F (1 << 12) #if SDL_ALTIVEC_BLITTERS && HAVE_SETJMP && !__MACOSX__ && !__OpenBSD__ /* This is the brute force way of detecting instruction sets... @@ -246,6 +249,7 @@ done: static int CPU_CPUIDFeatures[4]; static int CPU_CPUIDMaxFunction = 0; static SDL_bool CPU_OSSavesYMM = SDL_FALSE; +static SDL_bool CPU_OSSavesZMM = SDL_FALSE; static void CPU_calcCPUIDFeatures(void) @@ -266,7 +270,7 @@ CPU_calcCPUIDFeatures(void) /* Check to make sure we can call xgetbv */ if (c & 0x08000000) { - /* Call xgetbv to see if YMM register state is saved */ + /* Call xgetbv to see if YMM (etc) register state is saved */ #if defined(__GNUC__) && (defined(i386) || defined(__x86_64__)) __asm__(".byte 0x0f, 0x01, 0xd0" : "=a" (a) : "c" (0) : "%edx"); #elif defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64)) && (_MSC_FULL_VER >= 160040219) /* VS2010 SP1 */ @@ -280,6 +284,7 @@ CPU_calcCPUIDFeatures(void) } #endif CPU_OSSavesYMM = ((a & 6) == 6) ? SDL_TRUE : SDL_FALSE; + CPU_OSSavesZMM = (CPU_OSSavesYMM && ((a & 0xe0) == 0xe0)) ? SDL_TRUE : SDL_FALSE; } } } @@ -400,6 +405,18 @@ CPU_haveAVX2(void) return 0; } +static int +CPU_haveAVX512F(void) +{ + if (CPU_OSSavesZMM && (CPU_CPUIDMaxFunction >= 7)) { + int a, b, c, d; + (void) a; (void) b; (void) c; (void) d; /* compiler warnings... */ + cpuid(7, a, b, c, d); + return (b & 0x00010000); + } + return 0; +} + static int SDL_CPUCount = 0; int @@ -571,6 +588,7 @@ SDL_GetCPUCacheLineSize(void) } static Uint32 SDL_CPUFeatures = 0xFFFFFFFF; +static Uint32 SDL_SIMDAlignment = 0xFFFFFFFF; static Uint32 SDL_GetCPUFeatures(void) @@ -578,41 +596,57 @@ SDL_GetCPUFeatures(void) if (SDL_CPUFeatures == 0xFFFFFFFF) { CPU_calcCPUIDFeatures(); SDL_CPUFeatures = 0; + SDL_SIMDAlignment = 4; /* a good safe base value */ if (CPU_haveRDTSC()) { SDL_CPUFeatures |= CPU_HAS_RDTSC; } if (CPU_haveAltiVec()) { SDL_CPUFeatures |= CPU_HAS_ALTIVEC; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 16); } if (CPU_haveMMX()) { SDL_CPUFeatures |= CPU_HAS_MMX; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 8); } if (CPU_have3DNow()) { SDL_CPUFeatures |= CPU_HAS_3DNOW; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 8); } if (CPU_haveSSE()) { SDL_CPUFeatures |= CPU_HAS_SSE; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 16); } if (CPU_haveSSE2()) { SDL_CPUFeatures |= CPU_HAS_SSE2; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 16); } if (CPU_haveSSE3()) { SDL_CPUFeatures |= CPU_HAS_SSE3; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 16); } if (CPU_haveSSE41()) { SDL_CPUFeatures |= CPU_HAS_SSE41; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 16); } if (CPU_haveSSE42()) { SDL_CPUFeatures |= CPU_HAS_SSE42; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 16); } if (CPU_haveAVX()) { SDL_CPUFeatures |= CPU_HAS_AVX; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 32); } if (CPU_haveAVX2()) { SDL_CPUFeatures |= CPU_HAS_AVX2; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 32); + } + if (CPU_haveAVX512F()) { + SDL_CPUFeatures |= CPU_HAS_AVX512F; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 64); } if (CPU_haveNEON()) { SDL_CPUFeatures |= CPU_HAS_NEON; + SDL_SIMDAlignment = SDL_max(SDL_SIMDAlignment, 16); } } return SDL_CPUFeatures; @@ -685,6 +719,12 @@ SDL_HasAVX2(void) return CPU_FEATURE_AVAILABLE(CPU_HAS_AVX2); } +SDL_bool +SDL_HasAVX512F(void) +{ + return CPU_FEATURE_AVAILABLE(CPU_HAS_AVX512F); +} + SDL_bool SDL_HasNEON(void) { @@ -745,6 +785,44 @@ SDL_GetSystemRAM(void) } +size_t +SDL_SIMDGetAlignment(void) +{ + if (SDL_SIMDAlignment == 0xFFFFFFFF) { + SDL_GetCPUFeatures(); /* make sure this has been calculated */ + } + SDL_assert(SDL_SIMDAlignment != 0); + return SDL_SIMDAlignment; +} + +void * +SDL_SIMDAlloc(const size_t len) +{ + const size_t alignment = SDL_SIMDGetAlignment(); + const size_t padding = alignment - (len % alignment); + const size_t padded = (padding != alignment) ? (len + padding) : len; + Uint8 *retval = NULL; + Uint8 *ptr = (Uint8 *) SDL_malloc(padded + alignment + sizeof (void *)); + if (ptr) { + /* store the actual malloc pointer right before our aligned pointer. */ + retval = ptr + sizeof (void *); + retval += alignment - (((size_t) retval) % alignment); + *(((void **) retval) - 1) = ptr; + } + return retval; +} + +void +SDL_SIMDFree(void *ptr) +{ + if (ptr) { + void **realptr = (void **) ptr; + realptr--; + SDL_free(*(((void **) ptr) - 1)); + } +} + + #ifdef TEST_MAIN #include @@ -767,6 +845,7 @@ main() printf("SSE4.2: %d\n", SDL_HasSSE42()); printf("AVX: %d\n", SDL_HasAVX()); printf("AVX2: %d\n", SDL_HasAVX2()); + printf("AVX-512F: %d\n", SDL_HasAVX512F()); printf("NEON: %d\n", SDL_HasNEON()); printf("RAM: %d MB\n", SDL_GetSystemRAM()); return 0; diff --git a/Engine/lib/sdl/src/cpuinfo/SDL_simd.h b/Engine/lib/sdl/src/cpuinfo/SDL_simd.h new file mode 100644 index 000000000..e2b28bc8c --- /dev/null +++ b/Engine/lib/sdl/src/cpuinfo/SDL_simd.h @@ -0,0 +1,88 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "SDL.h" +#include "../SDL_internal.h" + +/** + * \brief Report the alignment this system needs for SIMD allocations. + * + * This will return the minimum number of bytes to which a pointer must be + * aligned to be compatible with SIMD instructions on the current machine. + * For example, if the machine supports SSE only, it will return 16, but if + * it supports AVX-512F, it'll return 64 (etc). This only reports values for + * instruction sets SDL knows about, so if your SDL build doesn't have + * SDL_HasAVX512F(), then it might return 16 for the SSE support it sees and + * not 64 for the AVX-512 instructions that exist but SDL doesn't know about. + * Plan accordingly. + */ +extern size_t SDL_SIMDGetAlignment(void); + +/** + * \brief Allocate memory in a SIMD-friendly way. + * + * This will allocate a block of memory that is suitable for use with SIMD + * instructions. Specifically, it will be properly aligned and padded for + * the system's supported vector instructions. + * + * The memory returned will be padded such that it is safe to read or write + * an incomplete vector at the end of the memory block. This can be useful + * so you don't have to drop back to a scalar fallback at the end of your + * SIMD processing loop to deal with the final elements without overflowing + * the allocated buffer. + * + * You must free this memory with SDL_FreeSIMD(), not free() or SDL_free() + * or delete[], etc. + * + * Note that SDL will only deal with SIMD instruction sets it is aware of; + * for example, SDL 2.0.8 knows that SSE wants 16-byte vectors + * (SDL_HasSSE()), and AVX2 wants 32 bytes (SDL_HasAVX2()), but doesn't + * know that AVX-512 wants 64. To be clear: if you can't decide to use an + * instruction set with an SDL_Has*() function, don't use that instruction + * set with memory allocated through here. + * + * SDL_AllocSIMD(0) will return a non-NULL pointer, assuming the system isn't + * out of memory. + * + * \param len The length, in bytes, of the block to allocated. The actual + * allocated block might be larger due to padding, etc. + * \return Pointer to newly-allocated block, NULL if out of memory. + * + * \sa SDL_SIMDAlignment + * \sa SDL_SIMDFree + */ +extern void * SDL_SIMDAlloc(const size_t len); + +/** + * \brief Deallocate memory obtained from SDL_SIMDAlloc + * + * It is not valid to use this function on a pointer from anything but + * SDL_SIMDAlloc(). It can't be used on pointers from malloc, realloc, + * SDL_malloc, memalign, new[], etc. + * + * However, SDL_SIMDFree(NULL) is a legal no-op. + * + * \sa SDL_SIMDAlloc + */ +extern void SDL_SIMDFree(void *ptr); + +/* vi: set ts=4 sw=4 expandtab: */ + diff --git a/Engine/lib/sdl/src/dynapi/SDL_dynapi.c b/Engine/lib/sdl/src/dynapi/SDL_dynapi.c index b898826b2..97bc21846 100644 --- a/Engine/lib/sdl/src/dynapi/SDL_dynapi.c +++ b/Engine/lib/sdl/src/dynapi/SDL_dynapi.c @@ -27,6 +27,7 @@ #if defined(__OS2__) #define INCL_DOS #define INCL_DOSERRORS +#include #include #endif @@ -167,15 +168,10 @@ SDL_DYNAPI_VARARGS(,,) #error Write me. #endif - - -/* Here's the exported entry point that fills in the jump table. */ -/* Use specific types when an "int" might suffice to keep this sane. */ -typedef Sint32 (SDLCALL *SDL_DYNAPI_ENTRYFN)(Uint32 apiver, void *table, Uint32 tablesize); -extern DECLSPEC Sint32 SDLCALL SDL_DYNAPI_entry(Uint32, void *, Uint32); - -Sint32 -SDL_DYNAPI_entry(Uint32 apiver, void *table, Uint32 tablesize) +/* we make this a static function so we can call the correct one without the + system's dynamic linker resolving to the wrong version of this. */ +static Sint32 +initialize_jumptable(Uint32 apiver, void *table, Uint32 tablesize) { SDL_DYNAPI_jump_table *output_jump_table = (SDL_DYNAPI_jump_table *) table; @@ -202,6 +198,18 @@ SDL_DYNAPI_entry(Uint32 apiver, void *table, Uint32 tablesize) } +/* Here's the exported entry point that fills in the jump table. */ +/* Use specific types when an "int" might suffice to keep this sane. */ +typedef Sint32 (SDLCALL *SDL_DYNAPI_ENTRYFN)(Uint32 apiver, void *table, Uint32 tablesize); +extern DECLSPEC Sint32 SDLCALL SDL_DYNAPI_entry(Uint32, void *, Uint32); + +Sint32 +SDL_DYNAPI_entry(Uint32 apiver, void *table, Uint32 tablesize) +{ + return initialize_jumptable(apiver, table, tablesize); +} + + /* Obviously we can't use SDL_LoadObject() to load SDL. :) */ /* Also obviously, we never close the loaded library. */ #if defined(WIN32) || defined(_WIN32) || defined(__CYGWIN__) @@ -260,7 +268,7 @@ static void SDL_InitDynamicAPILocked(void) { const char *libname = SDL_getenv_REAL("SDL_DYNAMIC_API"); - SDL_DYNAPI_ENTRYFN entry = SDL_DYNAPI_entry; /* funcs from here by default. */ + SDL_DYNAPI_ENTRYFN entry = NULL; /* funcs from here by default. */ if (libname) { entry = (SDL_DYNAPI_ENTRYFN) get_sdlapi_entry(libname, "SDL_DYNAPI_entry"); @@ -268,16 +276,15 @@ SDL_InitDynamicAPILocked(void) /* !!! FIXME: fail to startup here instead? */ /* !!! FIXME: definitely warn user. */ /* Just fill in the function pointers from this library. */ - entry = SDL_DYNAPI_entry; } } - if (entry(SDL_DYNAPI_VERSION, &jump_table, sizeof (jump_table)) < 0) { + if (!entry || (entry(SDL_DYNAPI_VERSION, &jump_table, sizeof (jump_table)) < 0)) { /* !!! FIXME: fail to startup here instead? */ /* !!! FIXME: definitely warn user. */ /* Just fill in the function pointers from this library. */ - if (entry != SDL_DYNAPI_entry) { - if (!SDL_DYNAPI_entry(SDL_DYNAPI_VERSION, &jump_table, sizeof (jump_table))) { + if (!entry) { + if (!initialize_jumptable(SDL_DYNAPI_VERSION, &jump_table, sizeof (jump_table))) { /* !!! FIXME: now we're screwed. Should definitely abort now. */ } } diff --git a/Engine/lib/sdl/src/dynapi/SDL_dynapi_overrides.h b/Engine/lib/sdl/src/dynapi/SDL_dynapi_overrides.h index 1ec2eaffd..745421382 100644 --- a/Engine/lib/sdl/src/dynapi/SDL_dynapi_overrides.h +++ b/Engine/lib/sdl/src/dynapi/SDL_dynapi_overrides.h @@ -669,3 +669,35 @@ #define SDL_WinRTGetDeviceFamily SDL_WinRTGetDeviceFamily_REAL #define SDL_log10 SDL_log10_REAL #define SDL_log10f SDL_log10f_REAL +#define SDL_GameControllerMappingForDeviceIndex SDL_GameControllerMappingForDeviceIndex_REAL +#define SDL_LinuxSetThreadPriority SDL_LinuxSetThreadPriority_REAL +#define SDL_HasAVX512F SDL_HasAVX512F_REAL +#define SDL_IsChromebook SDL_IsChromebook_REAL +#define SDL_IsDeXMode SDL_IsDeXMode_REAL +#define SDL_AndroidBackButton SDL_AndroidBackButton_REAL +#define SDL_exp SDL_exp_REAL +#define SDL_expf SDL_expf_REAL +#define SDL_wcsdup SDL_wcsdup_REAL +#define SDL_GameControllerRumble SDL_GameControllerRumble_REAL +#define SDL_JoystickRumble SDL_JoystickRumble_REAL +#define SDL_NumSensors SDL_NumSensors_REAL +#define SDL_SensorGetDeviceName SDL_SensorGetDeviceName_REAL +#define SDL_SensorGetDeviceType SDL_SensorGetDeviceType_REAL +#define SDL_SensorGetDeviceNonPortableType SDL_SensorGetDeviceNonPortableType_REAL +#define SDL_SensorGetDeviceInstanceID SDL_SensorGetDeviceInstanceID_REAL +#define SDL_SensorOpen SDL_SensorOpen_REAL +#define SDL_SensorFromInstanceID SDL_SensorFromInstanceID_REAL +#define SDL_SensorGetName SDL_SensorGetName_REAL +#define SDL_SensorGetType SDL_SensorGetType_REAL +#define SDL_SensorGetNonPortableType SDL_SensorGetNonPortableType_REAL +#define SDL_SensorGetInstanceID SDL_SensorGetInstanceID_REAL +#define SDL_SensorGetData SDL_SensorGetData_REAL +#define SDL_SensorClose SDL_SensorClose_REAL +#define SDL_SensorUpdate SDL_SensorUpdate_REAL +#define SDL_IsTablet SDL_IsTablet_REAL +#define SDL_GetDisplayOrientation SDL_GetDisplayOrientation_REAL +#define SDL_HasColorKey SDL_HasColorKey_REAL +#define SDL_CreateThreadWithStackSize SDL_CreateThreadWithStackSize_REAL +#define SDL_JoystickGetDevicePlayerIndex SDL_JoystickGetDevicePlayerIndex_REAL +#define SDL_JoystickGetPlayerIndex SDL_JoystickGetPlayerIndex_REAL +#define SDL_GameControllerGetPlayerIndex SDL_GameControllerGetPlayerIndex_REAL diff --git a/Engine/lib/sdl/src/dynapi/SDL_dynapi_procs.h b/Engine/lib/sdl/src/dynapi/SDL_dynapi_procs.h index b715d33ce..0a1f3aefa 100644 --- a/Engine/lib/sdl/src/dynapi/SDL_dynapi_procs.h +++ b/Engine/lib/sdl/src/dynapi/SDL_dynapi_procs.h @@ -707,3 +707,51 @@ SDL_DYNAPI_PROC(SDL_bool,SDL_IsAndroidTV,(void),(),return) #endif SDL_DYNAPI_PROC(double,SDL_log10,(double a),(a),return) SDL_DYNAPI_PROC(float,SDL_log10f,(float a),(a),return) +SDL_DYNAPI_PROC(char*,SDL_GameControllerMappingForDeviceIndex,(int a),(a),return) +#ifdef __LINUX__ +SDL_DYNAPI_PROC(int,SDL_LinuxSetThreadPriority,(Sint64 a, int b),(a,b),return) +#endif +SDL_DYNAPI_PROC(SDL_bool,SDL_HasAVX512F,(void),(),return) +#ifdef __ANDROID__ +SDL_DYNAPI_PROC(SDL_bool,SDL_IsChromebook,(void),(),return) +SDL_DYNAPI_PROC(SDL_bool,SDL_IsDeXMode,(void),(),return) +SDL_DYNAPI_PROC(void,SDL_AndroidBackButton,(void),(),return) +#endif +SDL_DYNAPI_PROC(double,SDL_exp,(double a),(a),return) +SDL_DYNAPI_PROC(float,SDL_expf,(float a),(a),return) +SDL_DYNAPI_PROC(wchar_t*,SDL_wcsdup,(const wchar_t *a),(a),return) +SDL_DYNAPI_PROC(int,SDL_GameControllerRumble,(SDL_GameController *a, Uint16 b, Uint16 c, Uint32 d),(a,b,c,d),return) +SDL_DYNAPI_PROC(int,SDL_JoystickRumble,(SDL_Joystick *a, Uint16 b, Uint16 c, Uint32 d),(a,b,c,d),return) +SDL_DYNAPI_PROC(int,SDL_NumSensors,(void),(),return) +SDL_DYNAPI_PROC(const char*,SDL_SensorGetDeviceName,(int a),(a),return) +SDL_DYNAPI_PROC(SDL_SensorType,SDL_SensorGetDeviceType,(int a),(a),return) +SDL_DYNAPI_PROC(int,SDL_SensorGetDeviceNonPortableType,(int a),(a),return) +SDL_DYNAPI_PROC(SDL_SensorID,SDL_SensorGetDeviceInstanceID,(int a),(a),return) +SDL_DYNAPI_PROC(SDL_Sensor*,SDL_SensorOpen,(int a),(a),return) +SDL_DYNAPI_PROC(SDL_Sensor*,SDL_SensorFromInstanceID,(SDL_SensorID a),(a),return) +SDL_DYNAPI_PROC(const char*,SDL_SensorGetName,(SDL_Sensor *a),(a),return) +SDL_DYNAPI_PROC(SDL_SensorType,SDL_SensorGetType,(SDL_Sensor *a),(a),return) +SDL_DYNAPI_PROC(int,SDL_SensorGetNonPortableType,(SDL_Sensor *a),(a),return) +SDL_DYNAPI_PROC(SDL_SensorID,SDL_SensorGetInstanceID,(SDL_Sensor *a),(a),return) +SDL_DYNAPI_PROC(int,SDL_SensorGetData,(SDL_Sensor *a, float *b, int c),(a,b,c),return) +SDL_DYNAPI_PROC(void,SDL_SensorClose,(SDL_Sensor *a),(a),) +SDL_DYNAPI_PROC(void,SDL_SensorUpdate,(void),(),) +SDL_DYNAPI_PROC(SDL_bool,SDL_IsTablet,(void),(),return) +SDL_DYNAPI_PROC(SDL_DisplayOrientation,SDL_GetDisplayOrientation,(int a),(a),return) +SDL_DYNAPI_PROC(SDL_bool,SDL_HasColorKey,(SDL_Surface *a),(a),return) + +#ifdef SDL_CreateThreadWithStackSize +#undef SDL_CreateThreadWithStackSize +#endif + +#if defined(__WIN32__) && !defined(HAVE_LIBC) +SDL_DYNAPI_PROC(SDL_Thread*,SDL_CreateThreadWithStackSize,(SDL_ThreadFunction a, const char *b, const size_t c, void *d, pfnSDL_CurrentBeginThread e, pfnSDL_CurrentEndThread f),(a,b,c,d,e,f),return) +#elif defined(__OS2__) +SDL_DYNAPI_PROC(SDL_Thread*,SDL_CreateThreadWithStackSize,(SDL_ThreadFunction a, const char *b, const size_t c, void *d, pfnSDL_CurrentBeginThread e, pfnSDL_CurrentEndThread f),(a,b,c,d,e,f),return) +#else +SDL_DYNAPI_PROC(SDL_Thread*,SDL_CreateThreadWithStackSize,(SDL_ThreadFunction a, const char *b, const size_t c, void *d),(a,b,c,d),return) +#endif + +SDL_DYNAPI_PROC(int,SDL_JoystickGetDevicePlayerIndex,(int a),(a),return) +SDL_DYNAPI_PROC(int,SDL_JoystickGetPlayerIndex,(SDL_Joystick *a),(a),return) +SDL_DYNAPI_PROC(int,SDL_GameControllerGetPlayerIndex,(SDL_GameController *a),(a),return) diff --git a/Engine/lib/sdl/src/events/SDL_displayevents.c b/Engine/lib/sdl/src/events/SDL_displayevents.c new file mode 100644 index 000000000..6c696af17 --- /dev/null +++ b/Engine/lib/sdl/src/events/SDL_displayevents.c @@ -0,0 +1,60 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "../SDL_internal.h" + +/* Display event handling code for SDL */ + +#include "SDL_events.h" +#include "SDL_events_c.h" + + +int +SDL_SendDisplayEvent(SDL_VideoDisplay *display, Uint8 displayevent, int data1) +{ + int posted; + + if (!display) { + return 0; + } + switch (displayevent) { + case SDL_DISPLAYEVENT_ORIENTATION: + if (data1 == SDL_ORIENTATION_UNKNOWN || data1 == display->orientation) { + return 0; + } + display->orientation = (SDL_DisplayOrientation)data1; + break; + } + + /* Post the event, if desired */ + posted = 0; + if (SDL_GetEventState(SDL_DISPLAYEVENT) == SDL_ENABLE) { + SDL_Event event; + event.type = SDL_DISPLAYEVENT; + event.display.event = displayevent; + event.display.display = SDL_GetIndexOfDisplay(display); + event.display.data1 = data1; + posted = (SDL_PushEvent(&event) > 0); + } + + return (posted); +} + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/events/SDL_displayevents_c.h b/Engine/lib/sdl/src/events/SDL_displayevents_c.h new file mode 100644 index 000000000..41def7b92 --- /dev/null +++ b/Engine/lib/sdl/src/events/SDL_displayevents_c.h @@ -0,0 +1,30 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "../SDL_internal.h" + +#ifndef SDL_displayevents_c_h_ +#define SDL_displayevents_c_h_ + +extern int SDL_SendDisplayEvent(SDL_VideoDisplay *display, Uint8 displayevent, int data1); + +#endif /* SDL_displayevents_c_h_ */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/events/SDL_events.c b/Engine/lib/sdl/src/events/SDL_events.c index f2e5b6278..25e8ac49c 100644 --- a/Engine/lib/sdl/src/events/SDL_events.c +++ b/Engine/lib/sdl/src/events/SDL_events.c @@ -417,6 +417,10 @@ SDL_StartEventLoop(void) SDL_EventState(SDL_TEXTINPUT, SDL_DISABLE); SDL_EventState(SDL_TEXTEDITING, SDL_DISABLE); SDL_EventState(SDL_SYSWMEVENT, SDL_DISABLE); +#if 0 /* Leave these events enabled so apps can respond to items being dragged onto them at startup */ + SDL_EventState(SDL_DROPFILE, SDL_DISABLE); + SDL_EventState(SDL_DROPTEXT, SDL_DISABLE); +#endif SDL_AtomicSet(&SDL_EventQ.active, 1); @@ -604,6 +608,10 @@ SDL_FlushEvent(Uint32 type) void SDL_FlushEvents(Uint32 minType, Uint32 maxType) { + /* !!! FIXME: we need to manually SDL_free() the strings in TEXTINPUT and + drag'n'drop events if we're flushing them without passing them to the + app, but I don't know if this is the right place to do that. */ + /* Don't look after we've quit */ if (!SDL_AtomicGet(&SDL_EventQ.active)) { return; @@ -651,6 +659,13 @@ SDL_PumpEvents(void) } #endif +#if !SDL_SENSOR_DISABLED + /* Check for sensor state change */ + if (!SDL_disabled_events[SDL_SENSORUPDATE >> 8]) { + SDL_SensorUpdate(); + } +#endif + SDL_SendPendingQuit(); /* in case we had a signal handler fire, etc. */ } @@ -863,6 +878,8 @@ SDL_FilterEvents(SDL_EventFilter filter, void *userdata) Uint8 SDL_EventState(Uint32 type, int state) { + const SDL_bool isdnd = ((state == SDL_DISABLE) || (state == SDL_ENABLE)) && + ((type == SDL_DROPFILE) || (type == SDL_DROPTEXT)); Uint8 current_state; Uint8 hi = ((type >> 8) & 0xff); Uint8 lo = (type & 0xff); @@ -898,6 +915,12 @@ SDL_EventState(Uint32 type, int state) } } + /* turn off drag'n'drop support if we've disabled the events. + This might change some UI details at the OS level. */ + if (isdnd) { + SDL_ToggleDragAndDropSupport(); + } + return current_state; } diff --git a/Engine/lib/sdl/src/events/SDL_events_c.h b/Engine/lib/sdl/src/events/SDL_events_c.h index b1bd277e7..d9684b5fa 100644 --- a/Engine/lib/sdl/src/events/SDL_events_c.h +++ b/Engine/lib/sdl/src/events/SDL_events_c.h @@ -18,12 +18,19 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_events_c_h_ +#define SDL_events_c_h_ + #include "../SDL_internal.h" /* Useful functions and variables from SDL_events.c */ #include "SDL_events.h" #include "SDL_thread.h" +#include "../video/SDL_sysvideo.h" + #include "SDL_clipboardevents_c.h" +#include "SDL_displayevents_c.h" #include "SDL_dropevents_c.h" #include "SDL_gesture_c.h" #include "SDL_keyboard_c.h" @@ -46,4 +53,6 @@ extern void SDL_QuitQuit(void); extern void SDL_SendPendingQuit(void); +#endif /* SDL_events_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/events/SDL_mouse.c b/Engine/lib/sdl/src/events/SDL_mouse.c index 4f4e62f87..ff23c5ea4 100644 --- a/Engine/lib/sdl/src/events/SDL_mouse.c +++ b/Engine/lib/sdl/src/events/SDL_mouse.c @@ -33,12 +33,38 @@ /* The mouse state */ static SDL_Mouse SDL_mouse; -static Uint32 SDL_double_click_time = 500; -static int SDL_double_click_radius = 1; static int SDL_PrivateSendMouseMotion(SDL_Window * window, SDL_MouseID mouseID, int relative, int x, int y); +static void SDLCALL +SDL_MouseDoubleClickTimeChanged(void *userdata, const char *name, const char *oldValue, const char *hint) +{ + SDL_Mouse *mouse = (SDL_Mouse *)userdata; + + if (hint && *hint) { + mouse->double_click_time = SDL_atoi(hint); + } else { +#ifdef __WIN32__ + mouse->double_click_time = GetDoubleClickTime(); +#else + mouse->double_click_time = 500; +#endif + } +} + +static void SDLCALL +SDL_MouseDoubleClickRadiusChanged(void *userdata, const char *name, const char *oldValue, const char *hint) +{ + SDL_Mouse *mouse = (SDL_Mouse *)userdata; + + if (hint && *hint) { + mouse->double_click_radius = SDL_atoi(hint); + } else { + mouse->double_click_radius = 32; /* 32 pixels seems about right for touch interfaces */ + } +} + static void SDLCALL SDL_MouseNormalSpeedScaleChanged(void *userdata, const char *name, const char *oldValue, const char *hint) { @@ -83,6 +109,12 @@ SDL_MouseInit(void) SDL_zerop(mouse); + SDL_AddHintCallback(SDL_HINT_MOUSE_DOUBLE_CLICK_TIME, + SDL_MouseDoubleClickTimeChanged, mouse); + + SDL_AddHintCallback(SDL_HINT_MOUSE_DOUBLE_CLICK_RADIUS, + SDL_MouseDoubleClickRadiusChanged, mouse); + SDL_AddHintCallback(SDL_HINT_MOUSE_NORMAL_SPEED_SCALE, SDL_MouseNormalSpeedScaleChanged, mouse); @@ -114,12 +146,6 @@ SDL_GetMouse(void) return &SDL_mouse; } -void -SDL_SetDoubleClickTime(Uint32 interval) -{ - SDL_double_click_time = interval; -} - SDL_Window * SDL_GetMouseFocus(void) { @@ -454,9 +480,9 @@ SDL_PrivateSendMouseButton(SDL_Window * window, SDL_MouseID mouseID, Uint8 state if (state == SDL_PRESSED) { Uint32 now = SDL_GetTicks(); - if (SDL_TICKS_PASSED(now, clickstate->last_timestamp + SDL_double_click_time) || - SDL_abs(mouse->x - clickstate->last_x) > SDL_double_click_radius || - SDL_abs(mouse->y - clickstate->last_y) > SDL_double_click_radius) { + if (SDL_TICKS_PASSED(now, clickstate->last_timestamp + mouse->double_click_time) || + SDL_abs(mouse->x - clickstate->last_x) > mouse->double_click_radius || + SDL_abs(mouse->y - clickstate->last_y) > mouse->double_click_radius) { clickstate->click_count = 0; } clickstate->last_timestamp = now; @@ -688,6 +714,7 @@ static SDL_bool ShouldUseRelativeModeWarp(SDL_Mouse *mouse) { if (!mouse->SetRelativeMouseMode) { + SDL_assert(mouse->WarpMouse); /* Need this functionality for relative mode warp implementation */ return SDL_TRUE; } @@ -720,6 +747,9 @@ SDL_SetRelativeMouseMode(SDL_bool enabled) } else if (mouse->SetRelativeMouseMode(enabled) < 0) { if (enabled) { /* Fall back to warp mode if native relative mode failed */ + if (!mouse->WarpMouse) { + return SDL_SetError("No relative mode implementation available"); + } mouse->relative_mode_warp = SDL_TRUE; } } diff --git a/Engine/lib/sdl/src/events/SDL_mouse_c.h b/Engine/lib/sdl/src/events/SDL_mouse_c.h index 28089e0c4..ad444922a 100644 --- a/Engine/lib/sdl/src/events/SDL_mouse_c.h +++ b/Engine/lib/sdl/src/events/SDL_mouse_c.h @@ -90,6 +90,8 @@ typedef struct float relative_speed_scale; float scale_accum_x; float scale_accum_y; + Uint32 double_click_time; + int double_click_radius; SDL_bool touch_mouse_events; /* Data for double-click tracking */ @@ -112,9 +114,6 @@ extern int SDL_MouseInit(void); /* Get the mouse state structure */ SDL_Mouse *SDL_GetMouse(void); -/* Set the default double-click interval */ -extern void SDL_SetDoubleClickTime(Uint32 interval); - /* Set the default mouse cursor */ extern void SDL_SetDefaultCursor(SDL_Cursor * cursor); diff --git a/Engine/lib/sdl/src/events/SDL_windowevents.c b/Engine/lib/sdl/src/events/SDL_windowevents.c index 610fad5ce..167084109 100644 --- a/Engine/lib/sdl/src/events/SDL_windowevents.c +++ b/Engine/lib/sdl/src/events/SDL_windowevents.c @@ -25,30 +25,16 @@ #include "SDL_events.h" #include "SDL_events_c.h" #include "SDL_mouse_c.h" -#include "../video/SDL_sysvideo.h" static int SDLCALL -RemovePendingResizedEvents(void * userdata, SDL_Event *event) +RemovePendingSizeChangedAndResizedEvents(void * userdata, SDL_Event *event) { SDL_Event *new_event = (SDL_Event *)userdata; if (event->type == SDL_WINDOWEVENT && - event->window.event == SDL_WINDOWEVENT_RESIZED && - event->window.windowID == new_event->window.windowID) { - /* We're about to post a new size event, drop the old one */ - return 0; - } - return 1; -} - -static int SDLCALL -RemovePendingSizeChangedEvents(void * userdata, SDL_Event *event) -{ - SDL_Event *new_event = (SDL_Event *)userdata; - - if (event->type == SDL_WINDOWEVENT && - event->window.event == SDL_WINDOWEVENT_SIZE_CHANGED && + (event->window.event == SDL_WINDOWEVENT_SIZE_CHANGED || + event->window.event == SDL_WINDOWEVENT_RESIZED) && event->window.windowID == new_event->window.windowID) { /* We're about to post a new size event, drop the old one */ return 0; @@ -200,11 +186,8 @@ SDL_SendWindowEvent(SDL_Window * window, Uint8 windowevent, int data1, event.window.windowID = window->id; /* Fixes queue overflow with resize events that aren't processed */ - if (windowevent == SDL_WINDOWEVENT_RESIZED) { - SDL_FilterEvents(RemovePendingResizedEvents, &event); - } if (windowevent == SDL_WINDOWEVENT_SIZE_CHANGED) { - SDL_FilterEvents(RemovePendingSizeChangedEvents, &event); + SDL_FilterEvents(RemovePendingSizeChangedAndResizedEvents, &event); } if (windowevent == SDL_WINDOWEVENT_MOVED) { SDL_FilterEvents(RemovePendingMoveEvents, &event); diff --git a/Engine/lib/sdl/src/events/scancodes_xfree86.h b/Engine/lib/sdl/src/events/scancodes_xfree86.h index 7d1f844b0..6e65507f6 100644 --- a/Engine/lib/sdl/src/events/scancodes_xfree86.h +++ b/Engine/lib/sdl/src/events/scancodes_xfree86.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef scancodes_xfree86_h_ +#define scancodes_xfree86_h_ + #include "../../include/SDL_scancode.h" /* XFree86 key code to SDL scancode mapping table @@ -503,4 +507,6 @@ static const SDL_Scancode xvnc_scancode_table[] = { /* 80 */ SDL_SCANCODE_F12, }; +#endif /* scancodes_xfree86_h_ */ + /* *INDENT-ON* */ diff --git a/Engine/lib/sdl/src/haptic/SDL_haptic.c b/Engine/lib/sdl/src/haptic/SDL_haptic.c index 4988c30ba..f23ba18df 100644 --- a/Engine/lib/sdl/src/haptic/SDL_haptic.c +++ b/Engine/lib/sdl/src/haptic/SDL_haptic.c @@ -389,9 +389,11 @@ SDL_HapticClose(SDL_Haptic * haptic) void SDL_HapticQuit(void) { + while (SDL_haptics) { + SDL_HapticClose(SDL_haptics); + } + SDL_SYS_HapticQuit(); - SDL_assert(SDL_haptics == NULL); - SDL_haptics = NULL; } /* @@ -765,6 +767,7 @@ SDL_HapticRumbleInit(SDL_Haptic * haptic) SDL_zerop(efx); if (haptic->supported & SDL_HAPTIC_SINE) { efx->type = SDL_HAPTIC_SINE; + efx->periodic.direction.type = SDL_HAPTIC_CARTESIAN; efx->periodic.period = 1000; efx->periodic.magnitude = 0x4000; efx->periodic.length = 5000; diff --git a/Engine/lib/sdl/src/haptic/SDL_haptic_c.h b/Engine/lib/sdl/src/haptic/SDL_haptic_c.h index 26d900d44..390dc7854 100644 --- a/Engine/lib/sdl/src/haptic/SDL_haptic_c.h +++ b/Engine/lib/sdl/src/haptic/SDL_haptic_c.h @@ -19,7 +19,12 @@ 3. This notice may not be removed or altered from any source distribution. */ +#ifndef SDL_haptic_c_h_ +#define SDL_haptic_c_h_ + extern int SDL_HapticInit(void); extern void SDL_HapticQuit(void); +#endif /* SDL_haptic_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/haptic/android/SDL_syshaptic.c b/Engine/lib/sdl/src/haptic/android/SDL_syshaptic.c index 1fb235249..7cb289ba5 100644 --- a/Engine/lib/sdl/src/haptic/android/SDL_syshaptic.c +++ b/Engine/lib/sdl/src/haptic/android/SDL_syshaptic.c @@ -195,6 +195,10 @@ SDL_SYS_HapticClose(SDL_Haptic * haptic) void SDL_SYS_HapticQuit(void) { +/* We don't have any way to scan for joysticks (and their vibrators) at init, so don't wipe the list + * of joysticks here in case this is a reinit. + */ +#if 0 SDL_hapticlist_item *item = NULL; SDL_hapticlist_item *next = NULL; @@ -206,6 +210,7 @@ SDL_SYS_HapticQuit(void) SDL_hapticlist = SDL_hapticlist_tail = NULL; numhaptics = 0; return; +#endif } @@ -230,7 +235,12 @@ int SDL_SYS_HapticRunEffect(SDL_Haptic * haptic, struct haptic_effect *effect, Uint32 iterations) { - Android_JNI_HapticRun (((SDL_hapticlist_item *)haptic->hwdata)->device_id, effect->effect.leftright.length); + float large = effect->effect.leftright.large_magnitude / 32767.0f; + float small = effect->effect.leftright.small_magnitude / 32767.0f; + + float total = (large * 0.6f) + (small * 0.4f); + + Android_JNI_HapticRun (((SDL_hapticlist_item *)haptic->hwdata)->device_id, total, effect->effect.leftright.length); return 0; } @@ -238,6 +248,7 @@ SDL_SYS_HapticRunEffect(SDL_Haptic * haptic, struct haptic_effect *effect, int SDL_SYS_HapticStopEffect(SDL_Haptic * haptic, struct haptic_effect *effect) { + Android_JNI_HapticStop (((SDL_hapticlist_item *)haptic->hwdata)->device_id); return 0; } diff --git a/Engine/lib/sdl/src/haptic/linux/SDL_syshaptic.c b/Engine/lib/sdl/src/haptic/linux/SDL_syshaptic.c index 9c11a2f0e..4e4f8a5f2 100644 --- a/Engine/lib/sdl/src/haptic/linux/SDL_syshaptic.c +++ b/Engine/lib/sdl/src/haptic/linux/SDL_syshaptic.c @@ -181,6 +181,9 @@ SDL_SYS_HapticInit(void) SDL_UDEV_Quit(); return SDL_SetError("Could not setup haptic <-> udev callback"); } + + /* Force a scan to build the initial device list */ + SDL_UDEV_Scan(); #endif /* SDL_USE_LIBUDEV */ return numhaptics; @@ -798,7 +801,8 @@ SDL_SYS_ToFFEffect(struct ff_effect *dest, SDL_HapticEffect * src) else if (periodic->type == SDL_HAPTIC_SAWTOOTHDOWN) dest->u.periodic.waveform = FF_SAW_DOWN; dest->u.periodic.period = CLAMP(periodic->period); - dest->u.periodic.magnitude = periodic->magnitude; + /* Linux expects 0-65535, so multiply by 2 */ + dest->u.periodic.magnitude = CLAMP(periodic->magnitude) * 2; dest->u.periodic.offset = periodic->offset; /* Linux phase is defined in interval "[0x0000, 0x10000[", corresponds with "[0deg, 360deg[" phase shift. */ dest->u.periodic.phase = ((Uint32)periodic->phase * 0x10000U) / 36000; @@ -905,9 +909,9 @@ SDL_SYS_ToFFEffect(struct ff_effect *dest, SDL_HapticEffect * src) dest->trigger.button = 0; dest->trigger.interval = 0; - /* Rumble */ - dest->u.rumble.strong_magnitude = leftright->large_magnitude; - dest->u.rumble.weak_magnitude = leftright->small_magnitude; + /* Rumble (Linux expects 0-65535, so multiply by 2) */ + dest->u.rumble.strong_magnitude = CLAMP(leftright->large_magnitude) * 2; + dest->u.rumble.weak_magnitude = CLAMP(leftright->small_magnitude) * 2; break; diff --git a/Engine/lib/sdl/src/haptic/windows/SDL_windowshaptic.c b/Engine/lib/sdl/src/haptic/windows/SDL_windowshaptic.c index 3d7361de2..2e806c971 100644 --- a/Engine/lib/sdl/src/haptic/windows/SDL_windowshaptic.c +++ b/Engine/lib/sdl/src/haptic/windows/SDL_windowshaptic.c @@ -157,7 +157,7 @@ SDL_SYS_HapticMouse(void) /* Grab the first mouse haptic device we find. */ for (item = SDL_hapticlist; item != NULL; item = item->next) { - if (item->capabilities.dwDevType == DI8DEVCLASS_POINTER ) { + if (item->capabilities.dwDevType == DI8DEVCLASS_POINTER) { return index; } ++index; @@ -173,14 +173,16 @@ SDL_SYS_HapticMouse(void) int SDL_SYS_JoystickIsHaptic(SDL_Joystick * joystick) { - const struct joystick_hwdata *hwdata = joystick->hwdata; + if (joystick->driver != &SDL_WINDOWS_JoystickDriver) { + return 0; + } #if SDL_HAPTIC_XINPUT - if (hwdata->bXInputHaptic) { + if (joystick->hwdata->bXInputHaptic) { return 1; } #endif #if SDL_HAPTIC_DINPUT - if (hwdata->Capabilities.dwFlags & DIDC_FORCEFEEDBACK) { + if (joystick->hwdata->Capabilities.dwFlags & DIDC_FORCEFEEDBACK) { return 1; } #endif @@ -193,6 +195,9 @@ SDL_SYS_JoystickIsHaptic(SDL_Joystick * joystick) int SDL_SYS_JoystickSameHaptic(SDL_Haptic * haptic, SDL_Joystick * joystick) { + if (joystick->driver != &SDL_WINDOWS_JoystickDriver) { + return 0; + } if (joystick->hwdata->bXInputHaptic != haptic->hwdata->bXInputHaptic) { return 0; /* one is XInput, one is not; not the same device. */ } else if (joystick->hwdata->bXInputHaptic) { @@ -208,6 +213,8 @@ SDL_SYS_JoystickSameHaptic(SDL_Haptic * haptic, SDL_Joystick * joystick) int SDL_SYS_HapticOpenFromJoystick(SDL_Haptic * haptic, SDL_Joystick * joystick) { + SDL_assert(joystick->driver == &SDL_WINDOWS_JoystickDriver); + if (joystick->hwdata->bXInputDevice) { return SDL_XINPUT_HapticOpenFromJoystick(haptic, joystick); } else { diff --git a/Engine/lib/sdl/src/hidapi/AUTHORS.txt b/Engine/lib/sdl/src/hidapi/AUTHORS.txt new file mode 100644 index 000000000..7acafd78c --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/AUTHORS.txt @@ -0,0 +1,16 @@ + +HIDAPI Authors: + +Alan Ott : + Original Author and Maintainer + Linux, Windows, and Mac implementations + +Ludovic Rousseau : + Formatting for Doxygen documentation + Bug fixes + Correctness fixes + + +For a comprehensive list of contributions, see the commit list at github: + http://github.com/signal11/hidapi/commits/master + diff --git a/Engine/lib/sdl/src/hidapi/HACKING.txt b/Engine/lib/sdl/src/hidapi/HACKING.txt new file mode 100644 index 000000000..761d4b655 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/HACKING.txt @@ -0,0 +1,15 @@ +This file is mostly for the maintainer. + +1. Build hidapi.dll +2. Build hidtest.exe in DEBUG and RELEASE +3. Commit all + +4. Run the Following + export VERSION=0.1.0 + export TAG_NAME=hidapi-$VERSION + git tag $TAG_NAME + git archive --format zip --prefix $TAG_NAME/ $TAG_NAME >../$TAG_NAME.zip +5. Test the zip file. +6. Run the following: + git push origin $TAG_NAME + diff --git a/Engine/lib/sdl/src/hidapi/LICENSE-bsd.txt b/Engine/lib/sdl/src/hidapi/LICENSE-bsd.txt new file mode 100644 index 000000000..538cdf95c --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/LICENSE-bsd.txt @@ -0,0 +1,26 @@ +Copyright (c) 2010, Alan Ott, Signal 11 Software +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of Signal 11 Software nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/Engine/lib/sdl/src/hidapi/LICENSE-gpl3.txt b/Engine/lib/sdl/src/hidapi/LICENSE-gpl3.txt new file mode 100644 index 000000000..94a9ed024 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/LICENSE-gpl3.txt @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/Engine/lib/sdl/src/hidapi/LICENSE-orig.txt b/Engine/lib/sdl/src/hidapi/LICENSE-orig.txt new file mode 100644 index 000000000..e3f338082 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/LICENSE-orig.txt @@ -0,0 +1,9 @@ + HIDAPI - Multi-Platform library for + communication with HID devices. + + Copyright 2009, Alan Ott, Signal 11 Software. + All Rights Reserved. + + This software may be used by anyone for any reason so + long as the copyright notice in the source files + remains intact. diff --git a/Engine/lib/sdl/src/hidapi/LICENSE.txt b/Engine/lib/sdl/src/hidapi/LICENSE.txt new file mode 100644 index 000000000..e1676d4c4 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/LICENSE.txt @@ -0,0 +1,13 @@ +HIDAPI can be used under one of three licenses. + +1. The GNU General Public License, version 3.0, in LICENSE-gpl3.txt +2. A BSD-Style License, in LICENSE-bsd.txt. +3. The more liberal original HIDAPI license. LICENSE-orig.txt + +The license chosen is at the discretion of the user of HIDAPI. For example: +1. An author of GPL software would likely use HIDAPI under the terms of the +GPL. + +2. An author of commercial closed-source software would likely use HIDAPI +under the terms of the BSD-style license or the original HIDAPI license. + diff --git a/Engine/lib/sdl/src/hidapi/Makefile.am b/Engine/lib/sdl/src/hidapi/Makefile.am new file mode 100644 index 000000000..3382a1f04 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/Makefile.am @@ -0,0 +1,85 @@ + +ACLOCAL_AMFLAGS = -I m4 + +if OS_FREEBSD +pkgconfigdir=$(prefix)/libdata/pkgconfig +else +pkgconfigdir=$(libdir)/pkgconfig +endif + +if OS_LINUX +pkgconfig_DATA=pc/hidapi-hidraw.pc pc/hidapi-libusb.pc +else +pkgconfig_DATA=pc/hidapi.pc +endif + +SUBDIRS= + +if OS_LINUX +SUBDIRS += linux libusb +endif + +if OS_DARWIN +SUBDIRS += mac +endif + +if OS_IOS +SUBDIRS += ios +endif + +if OS_FREEBSD +SUBDIRS += libusb +endif + +if OS_KFREEBSD +SUBDIRS += libusb +endif + +if OS_WINDOWS +SUBDIRS += windows +endif + +SUBDIRS += hidtest + +if BUILD_TESTGUI +SUBDIRS += testgui +endif + +EXTRA_DIST = udev doxygen + +dist_doc_DATA = \ + README.txt \ + AUTHORS.txt \ + LICENSE-bsd.txt \ + LICENSE-gpl3.txt \ + LICENSE-orig.txt \ + LICENSE.txt + +SCMCLEAN_TARGETS= \ + aclocal.m4 \ + config.guess \ + config.sub \ + configure \ + config.h.in \ + depcomp \ + install-sh \ + ltmain.sh \ + missing \ + mac/Makefile.in \ + testgui/Makefile.in \ + libusb/Makefile.in \ + Makefile.in \ + linux/Makefile.in \ + windows/Makefile.in \ + m4/libtool.m4 \ + m4/lt~obsolete.m4 \ + m4/ltoptions.m4 \ + m4/ltsugar.m4 \ + m4/ltversion.m4 + +SCMCLEAN_DIR_TARGETS = \ + autom4te.cache + +scm-clean: distclean + rm -f $(SCMCLEAN_TARGETS) + rm -Rf $(SCMCLEAN_DIR_TARGETS) diff --git a/Engine/lib/sdl/src/hidapi/README.txt b/Engine/lib/sdl/src/hidapi/README.txt new file mode 100644 index 000000000..f19dae4ab --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/README.txt @@ -0,0 +1,339 @@ + HIDAPI library for Windows, Linux, FreeBSD and Mac OS X + ========================================================= + +About +====== + +HIDAPI is a multi-platform library which allows an application to interface +with USB and Bluetooth HID-Class devices on Windows, Linux, FreeBSD, and Mac +OS X. HIDAPI can be either built as a shared library (.so or .dll) or +can be embedded directly into a target application by adding a single source +file (per platform) and a single header. + +HIDAPI has four back-ends: + * Windows (using hid.dll) + * Linux/hidraw (using the Kernel's hidraw driver) + * Linux/libusb (using libusb-1.0) + * FreeBSD (using libusb-1.0) + * Mac (using IOHidManager) + +On Linux, either the hidraw or the libusb back-end can be used. There are +tradeoffs, and the functionality supported is slightly different. + +Linux/hidraw (linux/hid.c): +This back-end uses the hidraw interface in the Linux kernel. While this +back-end will support both USB and Bluetooth, it has some limitations on +kernels prior to 2.6.39, including the inability to send or receive feature +reports. In addition, it will only communicate with devices which have +hidraw nodes associated with them. Keyboards, mice, and some other devices +which are blacklisted from having hidraw nodes will not work. Fortunately, +for nearly all the uses of hidraw, this is not a problem. + +Linux/FreeBSD/libusb (libusb/hid.c): +This back-end uses libusb-1.0 to communicate directly to a USB device. This +back-end will of course not work with Bluetooth devices. + +HIDAPI also comes with a Test GUI. The Test GUI is cross-platform and uses +Fox Toolkit (http://www.fox-toolkit.org). It will build on every platform +which HIDAPI supports. Since it relies on a 3rd party library, building it +is optional but recommended because it is so useful when debugging hardware. + +What Does the API Look Like? +============================= +The API provides the the most commonly used HID functions including sending +and receiving of input, output, and feature reports. The sample program, +which communicates with a heavily hacked up version of the Microchip USB +Generic HID sample looks like this (with error checking removed for +simplicity): + +#ifdef WIN32 +#include +#endif +#include +#include +#include "hidapi.h" + +#define MAX_STR 255 + +int main(int argc, char* argv[]) +{ + int res; + unsigned char buf[65]; + wchar_t wstr[MAX_STR]; + hid_device *handle; + int i; + + // Initialize the hidapi library + res = hid_init(); + + // Open the device using the VID, PID, + // and optionally the Serial number. + handle = hid_open(0x4d8, 0x3f, NULL); + + // Read the Manufacturer String + res = hid_get_manufacturer_string(handle, wstr, MAX_STR); + wprintf(L"Manufacturer String: %s\n", wstr); + + // Read the Product String + res = hid_get_product_string(handle, wstr, MAX_STR); + wprintf(L"Product String: %s\n", wstr); + + // Read the Serial Number String + res = hid_get_serial_number_string(handle, wstr, MAX_STR); + wprintf(L"Serial Number String: (%d) %s\n", wstr[0], wstr); + + // Read Indexed String 1 + res = hid_get_indexed_string(handle, 1, wstr, MAX_STR); + wprintf(L"Indexed String 1: %s\n", wstr); + + // Toggle LED (cmd 0x80). The first byte is the report number (0x0). + buf[0] = 0x0; + buf[1] = 0x80; + res = hid_write(handle, buf, 65); + + // Request state (cmd 0x81). The first byte is the report number (0x0). + buf[0] = 0x0; + buf[1] = 0x81; + res = hid_write(handle, buf, 65); + + // Read requested state + res = hid_read(handle, buf, 65); + + // Print out the returned buffer. + for (i = 0; i < 4; i++) + printf("buf[%d]: %d\n", i, buf[i]); + + // Finalize the hidapi library + res = hid_exit(); + + return 0; +} + +If you have your own simple test programs which communicate with standard +hardware development boards (such as those from Microchip, TI, Atmel, +FreeScale and others), please consider sending me something like the above +for inclusion into the HIDAPI source. This will help others who have the +same hardware as you do. + +License +======== +HIDAPI may be used by one of three licenses as outlined in LICENSE.txt. + +Download +========= +HIDAPI can be downloaded from github + git clone git://github.com/signal11/hidapi.git + +Build Instructions +=================== + +This section is long. Don't be put off by this. It's not long because it's +complicated to build HIDAPI; it's quite the opposite. This section is long +because of the flexibility of HIDAPI and the large number of ways in which +it can be built and used. You will likely pick a single build method. + +HIDAPI can be built in several different ways. If you elect to build a +shared library, you will need to build it from the HIDAPI source +distribution. If you choose instead to embed HIDAPI directly into your +application, you can skip the building and look at the provided platform +Makefiles for guidance. These platform Makefiles are located in linux/ +libusb/ mac/ and windows/ and are called Makefile-manual. In addition, +Visual Studio projects are provided. Even if you're going to embed HIDAPI +into your project, it is still beneficial to build the example programs. + + +Prerequisites: +--------------- + + Linux: + ------- + On Linux, you will need to install development packages for libudev, + libusb and optionally Fox-toolkit (for the test GUI). On + Debian/Ubuntu systems these can be installed by running: + sudo apt-get install libudev-dev libusb-1.0-0-dev libfox-1.6-dev + + If you downloaded the source directly from the git repository (using + git clone), you'll need Autotools: + sudo apt-get install autotools-dev autoconf automake libtool + + FreeBSD: + --------- + On FreeBSD you will need to install GNU make, libiconv, and + optionally Fox-Toolkit (for the test GUI). This is done by running + the following: + pkg_add -r gmake libiconv fox16 + + If you downloaded the source directly from the git repository (using + git clone), you'll need Autotools: + pkg_add -r autotools + + Mac: + ----- + On Mac, you will need to install Fox-Toolkit if you wish to build + the Test GUI. There are two ways to do this, and each has a slight + complication. Which method you use depends on your use case. + + If you wish to build the Test GUI just for your own testing on your + own computer, then the easiest method is to install Fox-Toolkit + using ports: + sudo port install fox + + If you wish to build the TestGUI app bundle to redistribute to + others, you will need to install Fox-toolkit from source. This is + because the version of fox that gets installed using ports uses the + ports X11 libraries which are not compatible with the Apple X11 + libraries. If you install Fox with ports and then try to distribute + your built app bundle, it will simply fail to run on other systems. + To install Fox-Toolkit manually, download the source package from + http://www.fox-toolkit.org, extract it, and run the following from + within the extracted source: + ./configure && make && make install + + Windows: + --------- + On Windows, if you want to build the test GUI, you will need to get + the hidapi-externals.zip package from the download site. This + contains pre-built binaries for Fox-toolkit. Extract + hidapi-externals.zip just outside of hidapi, so that + hidapi-externals and hidapi are on the same level, as shown: + + Parent_Folder + | + +hidapi + +hidapi-externals + + Again, this step is not required if you do not wish to build the + test GUI. + + +Building HIDAPI into a shared library on Unix Platforms: +--------------------------------------------------------- + +On Unix-like systems such as Linux, FreeBSD, Mac, and even Windows, using +Mingw or Cygwin, the easiest way to build a standard system-installed shared +library is to use the GNU Autotools build system. If you checked out the +source from the git repository, run the following: + + ./bootstrap + ./configure + make + make install <----- as root, or using sudo + +If you downloaded a source package (ie: if you did not run git clone), you +can skip the ./bootstrap step. + +./configure can take several arguments which control the build. The two most +likely to be used are: + --enable-testgui + Enable build of the Test GUI. This requires Fox toolkit to + be installed. Instructions for installing Fox-Toolkit on + each platform are in the Prerequisites section above. + + --prefix=/usr + Specify where you want the output headers and libraries to + be installed. The example above will put the headers in + /usr/include and the binaries in /usr/lib. The default is to + install into /usr/local which is fine on most systems. + +Building the manual way on Unix platforms: +------------------------------------------- + +Manual Makefiles are provided mostly to give the user and idea what it takes +to build a program which embeds HIDAPI directly inside of it. These should +really be used as examples only. If you want to build a system-wide shared +library, use the Autotools method described above. + + To build HIDAPI using the manual makefiles, change to the directory + of your platform and run make. For example, on Linux run: + cd linux/ + make -f Makefile-manual + + To build the Test GUI using the manual makefiles: + cd testgui/ + make -f Makefile-manual + +Building on Windows: +--------------------- + +To build the HIDAPI DLL on Windows using Visual Studio, build the .sln file +in the windows/ directory. + +To build the Test GUI on windows using Visual Studio, build the .sln file in +the testgui/ directory. + +To build HIDAPI using MinGW or Cygwin using Autotools, use the instructions +in the section titled "Building HIDAPI into a shared library on Unix +Platforms" above. Note that building the Test GUI with MinGW or Cygwin will +require the Windows procedure in the Prerequisites section above (ie: +hidapi-externals.zip). + +To build HIDAPI using MinGW using the Manual Makefiles, see the section +"Building the manual way on Unix platforms" above. + +HIDAPI can also be built using the Windows DDK (now also called the Windows +Driver Kit or WDK). This method was originally required for the HIDAPI build +but not anymore. However, some users still prefer this method. It is not as +well supported anymore but should still work. Patches are welcome if it does +not. To build using the DDK: + + 1. Install the Windows Driver Kit (WDK) from Microsoft. + 2. From the Start menu, in the Windows Driver Kits folder, select Build + Environments, then your operating system, then the x86 Free Build + Environment (or one that is appropriate for your system). + 3. From the console, change directory to the windows/ddk_build/ directory, + which is part of the HIDAPI distribution. + 4. Type build. + 5. You can find the output files (DLL and LIB) in a subdirectory created + by the build system which is appropriate for your environment. On + Windows XP, this directory is objfre_wxp_x86/i386. + +Cross Compiling +================ + +This section talks about cross compiling HIDAPI for Linux using autotools. +This is useful for using HIDAPI on embedded Linux targets. These +instructions assume the most raw kind of embedded Linux build, where all +prerequisites will need to be built first. This process will of course vary +based on your embedded Linux build system if you are using one, such as +OpenEmbedded or Buildroot. + +For the purpose of this section, it will be assumed that the following +environment variables are exported. + + $ export STAGING=$HOME/out + $ export HOST=arm-linux + +STAGING and HOST can be modified to suit your setup. + +Prerequisites +-------------- + +Note that the build of libudev is the very basic configuration. + +Build Libusb. From the libusb source directory, run: + ./configure --host=$HOST --prefix=$STAGING + make + make install + +Build libudev. From the libudev source directory, run: + ./configure --disable-gudev --disable-introspection --disable-hwdb \ + --host=$HOST --prefix=$STAGING + make + make install + +Building HIDAPI +---------------- + +Build HIDAPI: + + PKG_CONFIG_DIR= \ + PKG_CONFIG_LIBDIR=$STAGING/lib/pkgconfig:$STAGING/share/pkgconfig \ + PKG_CONFIG_SYSROOT_DIR=$STAGING \ + ./configure --host=$HOST --prefix=$STAGING + + +Signal 11 Software - 2010-04-11 + 2010-07-28 + 2011-09-10 + 2012-05-01 + 2012-07-03 diff --git a/Engine/lib/sdl/src/hidapi/android/hid.cpp b/Engine/lib/sdl/src/hidapi/android/hid.cpp new file mode 100644 index 000000000..7b8d41c71 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/android/hid.cpp @@ -0,0 +1,1159 @@ +//=================== Copyright Valve Corporation, All rights reserved. ======= +// +// Purpose: A wrapper implementing "HID" API for Android +// +// This layer glues the hidapi API to Android's USB and BLE stack. +// +//============================================================================= + +#include +#include +#include +#include // For ETIMEDOUT and ECONNRESET +#include // For malloc() and free() +#include // For memcpy() + +#define TAG "hidapi" + +// Have error log always available +#define LOGE(...) __android_log_print(ANDROID_LOG_ERROR, TAG, __VA_ARGS__) + +#ifdef DEBUG +#define LOGV(...) __android_log_print(ANDROID_LOG_VERBOSE, TAG, __VA_ARGS__) +#define LOGD(...) __android_log_print(ANDROID_LOG_DEBUG, TAG, __VA_ARGS__) +#else +#define LOGV(...) +#define LOGD(...) +#endif + +#define SDL_JAVA_PREFIX org_libsdl_app +#define CONCAT1(prefix, class, function) CONCAT2(prefix, class, function) +#define CONCAT2(prefix, class, function) Java_ ## prefix ## _ ## class ## _ ## function +#define HID_DEVICE_MANAGER_JAVA_INTERFACE(function) CONCAT1(SDL_JAVA_PREFIX, HIDDeviceManager, function) + +#include "../hidapi/hidapi.h" + +typedef uint32_t uint32; +typedef uint64_t uint64; + + +struct hid_device_ +{ + int m_nId; + int m_nDeviceRefCount; +}; + +static JavaVM *g_JVM; +static pthread_key_t g_ThreadKey; + +template +class hid_device_ref +{ +public: + hid_device_ref( T *pObject = nullptr ) : m_pObject( nullptr ) + { + SetObject( pObject ); + } + + hid_device_ref( const hid_device_ref &rhs ) : m_pObject( nullptr ) + { + SetObject( rhs.GetObject() ); + } + + ~hid_device_ref() + { + SetObject( nullptr ); + } + + void SetObject( T *pObject ) + { + if ( m_pObject && m_pObject->DecrementRefCount() == 0 ) + { + delete m_pObject; + } + + m_pObject = pObject; + + if ( m_pObject ) + { + m_pObject->IncrementRefCount(); + } + } + + hid_device_ref &operator =( T *pObject ) + { + SetObject( pObject ); + return *this; + } + + hid_device_ref &operator =( const hid_device_ref &rhs ) + { + SetObject( rhs.GetObject() ); + return *this; + } + + T *GetObject() const + { + return m_pObject; + } + + T* operator->() const + { + return m_pObject; + } + + operator bool() const + { + return ( m_pObject != nullptr ); + } + +private: + T *m_pObject; +}; + +class hid_mutex_guard +{ +public: + hid_mutex_guard( pthread_mutex_t *pMutex ) : m_pMutex( pMutex ) + { + pthread_mutex_lock( m_pMutex ); + } + ~hid_mutex_guard() + { + pthread_mutex_unlock( m_pMutex ); + } + +private: + pthread_mutex_t *m_pMutex; +}; + +class hid_buffer +{ +public: + hid_buffer() : m_pData( nullptr ), m_nSize( 0 ), m_nAllocated( 0 ) + { + } + + hid_buffer( const uint8_t *pData, size_t nSize ) : m_pData( nullptr ), m_nSize( 0 ), m_nAllocated( 0 ) + { + assign( pData, nSize ); + } + + ~hid_buffer() + { + delete[] m_pData; + } + + void assign( const uint8_t *pData, size_t nSize ) + { + if ( nSize > m_nAllocated ) + { + delete[] m_pData; + m_pData = new uint8_t[ nSize ]; + m_nAllocated = nSize; + } + + m_nSize = nSize; + memcpy( m_pData, pData, nSize ); + } + + void clear() + { + m_nSize = 0; + } + + size_t size() const + { + return m_nSize; + } + + const uint8_t *data() const + { + return m_pData; + } + +private: + uint8_t *m_pData; + size_t m_nSize; + size_t m_nAllocated; +}; + +class hid_buffer_pool +{ +public: + hid_buffer_pool() : m_nSize( 0 ), m_pHead( nullptr ), m_pTail( nullptr ), m_pFree( nullptr ) + { + } + + ~hid_buffer_pool() + { + clear(); + + while ( m_pFree ) + { + hid_buffer_entry *pEntry = m_pFree; + m_pFree = m_pFree->m_pNext; + delete pEntry; + } + } + + size_t size() const { return m_nSize; } + + const hid_buffer &front() const { return m_pHead->m_buffer; } + + void pop_front() + { + hid_buffer_entry *pEntry = m_pHead; + if ( pEntry ) + { + m_pHead = pEntry->m_pNext; + if ( !m_pHead ) + { + m_pTail = nullptr; + } + pEntry->m_pNext = m_pFree; + m_pFree = pEntry; + --m_nSize; + } + } + + void emplace_back( const uint8_t *pData, size_t nSize ) + { + hid_buffer_entry *pEntry; + + if ( m_pFree ) + { + pEntry = m_pFree; + m_pFree = m_pFree->m_pNext; + } + else + { + pEntry = new hid_buffer_entry; + } + pEntry->m_pNext = nullptr; + + if ( m_pTail ) + { + m_pTail->m_pNext = pEntry; + } + else + { + m_pHead = pEntry; + } + m_pTail = pEntry; + + pEntry->m_buffer.assign( pData, nSize ); + ++m_nSize; + } + + void clear() + { + while ( size() > 0 ) + { + pop_front(); + } + } + +private: + struct hid_buffer_entry + { + hid_buffer m_buffer; + hid_buffer_entry *m_pNext; + }; + + size_t m_nSize; + hid_buffer_entry *m_pHead; + hid_buffer_entry *m_pTail; + hid_buffer_entry *m_pFree; +}; + +static jbyteArray NewByteArray( JNIEnv* env, const uint8_t *pData, size_t nDataLen ) +{ + jbyteArray array = env->NewByteArray( nDataLen ); + jbyte *pBuf = env->GetByteArrayElements( array, NULL ); + memcpy( pBuf, pData, nDataLen ); + env->ReleaseByteArrayElements( array, pBuf, 0 ); + + return array; +} + +static char *CreateStringFromJString( JNIEnv *env, const jstring &sString ) +{ + size_t nLength = env->GetStringUTFLength( sString ); + const char *pjChars = env->GetStringUTFChars( sString, NULL ); + char *psString = (char*)malloc( nLength + 1 ); + memcpy( psString, pjChars, nLength ); + psString[ nLength ] = '\0'; + env->ReleaseStringUTFChars( sString, pjChars ); + return psString; +} + +static wchar_t *CreateWStringFromJString( JNIEnv *env, const jstring &sString ) +{ + size_t nLength = env->GetStringLength( sString ); + const jchar *pjChars = env->GetStringChars( sString, NULL ); + wchar_t *pwString = (wchar_t*)malloc( ( nLength + 1 ) * sizeof( wchar_t ) ); + wchar_t *pwChars = pwString; + for ( size_t iIndex = 0; iIndex < nLength; ++iIndex ) + { + pwChars[ iIndex ] = pjChars[ iIndex ]; + } + pwString[ nLength ] = '\0'; + env->ReleaseStringChars( sString, pjChars ); + return pwString; +} + +static wchar_t *CreateWStringFromWString( const wchar_t *pwSrc ) +{ + size_t nLength = wcslen( pwSrc ); + wchar_t *pwString = (wchar_t*)malloc( ( nLength + 1 ) * sizeof( wchar_t ) ); + memcpy( pwString, pwSrc, nLength * sizeof( wchar_t ) ); + pwString[ nLength ] = '\0'; + return pwString; +} + +static hid_device_info *CopyHIDDeviceInfo( const hid_device_info *pInfo ) +{ + hid_device_info *pCopy = new hid_device_info; + *pCopy = *pInfo; + pCopy->path = strdup( pInfo->path ); + pCopy->product_string = CreateWStringFromWString( pInfo->product_string ); + pCopy->manufacturer_string = CreateWStringFromWString( pInfo->manufacturer_string ); + pCopy->serial_number = CreateWStringFromWString( pInfo->serial_number ); + return pCopy; +} + +static void FreeHIDDeviceInfo( hid_device_info *pInfo ) +{ + free( pInfo->path ); + free( pInfo->serial_number ); + free( pInfo->manufacturer_string ); + free( pInfo->product_string ); + delete pInfo; +} + +static jclass g_HIDDeviceManagerCallbackClass; +static jobject g_HIDDeviceManagerCallbackHandler; +static jmethodID g_midHIDDeviceManagerOpen; +static jmethodID g_midHIDDeviceManagerSendOutputReport; +static jmethodID g_midHIDDeviceManagerSendFeatureReport; +static jmethodID g_midHIDDeviceManagerGetFeatureReport; +static jmethodID g_midHIDDeviceManagerClose; + +static uint64_t get_timespec_ms( const struct timespec &ts ) +{ + return (uint64_t)ts.tv_sec * 1000 + ts.tv_nsec / 1000000; +} + +class CHIDDevice +{ +public: + CHIDDevice( int nDeviceID, hid_device_info *pInfo ) + { + m_nId = nDeviceID; + m_pInfo = pInfo; + + // The Bluetooth Steam Controller needs special handling + const int VALVE_USB_VID = 0x28DE; + const int D0G_BLE2_PID = 0x1106; + if ( pInfo->vendor_id == VALVE_USB_VID && pInfo->product_id == D0G_BLE2_PID ) + { + m_bIsBLESteamController = true; + } + } + + ~CHIDDevice() + { + FreeHIDDeviceInfo( m_pInfo ); + + // Note that we don't delete m_pDevice, as the app may still have a reference to it + } + + int IncrementRefCount() + { + int nValue; + pthread_mutex_lock( &m_refCountLock ); + nValue = ++m_nRefCount; + pthread_mutex_unlock( &m_refCountLock ); + return nValue; + } + + int DecrementRefCount() + { + int nValue; + pthread_mutex_lock( &m_refCountLock ); + nValue = --m_nRefCount; + pthread_mutex_unlock( &m_refCountLock ); + return nValue; + } + + int GetId() + { + return m_nId; + } + + const hid_device_info *GetDeviceInfo() + { + return m_pInfo; + } + + hid_device *GetDevice() + { + return m_pDevice; + } + + void ExceptionCheck( JNIEnv *env, const char *pszMethodName ) + { + if ( env->ExceptionCheck() ) + { + // Get our exception + jthrowable jExcept = env->ExceptionOccurred(); + + // Clear the exception so we can call JNI again + env->ExceptionClear(); + + // Get our exception message + jclass jExceptClass = env->GetObjectClass( jExcept ); + jmethodID jMessageMethod = env->GetMethodID( jExceptClass, "getMessage", "()Ljava/lang/String;" ); + jstring jMessage = (jstring)( env->CallObjectMethod( jExcept, jMessageMethod ) ); + const char *pszMessage = env->GetStringUTFChars( jMessage, NULL ); + + // ...and log it. + LOGE( "CHIDDevice::%s threw an exception: %s", pszMethodName, pszMessage ); + + // Cleanup + env->ReleaseStringUTFChars( jMessage, pszMessage ); + env->DeleteLocalRef( jMessage ); + env->DeleteLocalRef( jExceptClass ); + env->DeleteLocalRef( jExcept ); + } + } + + bool BOpen() + { + // Make sure thread is attached to JVM/env + JNIEnv *env; + g_JVM->AttachCurrentThread( &env, NULL ); + pthread_setspecific( g_ThreadKey, (void*)env ); + + m_bIsWaitingForOpen = false; + m_bOpenResult = env->CallBooleanMethod( g_HIDDeviceManagerCallbackHandler, g_midHIDDeviceManagerOpen, m_nId ); + ExceptionCheck( env, "BOpen" ); + + if ( m_bIsWaitingForOpen ) + { + hid_mutex_guard cvl( &m_cvLock ); + + const int OPEN_TIMEOUT_SECONDS = 60; + struct timespec ts, endtime; + clock_gettime( CLOCK_REALTIME, &ts ); + endtime = ts; + endtime.tv_sec += OPEN_TIMEOUT_SECONDS; + do + { + if ( pthread_cond_timedwait( &m_cv, &m_cvLock, &endtime ) != 0 ) + { + break; + } + } + while ( m_bIsWaitingForOpen && get_timespec_ms( ts ) < get_timespec_ms( endtime ) ); + } + + if ( !m_bOpenResult ) + { + if ( m_bIsWaitingForOpen ) + { + LOGV( "Device open failed - timed out waiting for device permission" ); + } + else + { + LOGV( "Device open failed" ); + } + return false; + } + + m_pDevice = new hid_device; + m_pDevice->m_nId = m_nId; + m_pDevice->m_nDeviceRefCount = 1; + LOGD("Creating device %d (%p), refCount = 1\n", m_pDevice->m_nId, m_pDevice); + return true; + } + + void SetOpenPending() + { + m_bIsWaitingForOpen = true; + } + + void SetOpenResult( bool bResult ) + { + if ( m_bIsWaitingForOpen ) + { + m_bOpenResult = bResult; + m_bIsWaitingForOpen = false; + pthread_cond_signal( &m_cv ); + } + } + + void ProcessInput( const uint8_t *pBuf, size_t nBufSize ) + { + hid_mutex_guard l( &m_dataLock ); + + size_t MAX_REPORT_QUEUE_SIZE = 16; + if ( m_vecData.size() >= MAX_REPORT_QUEUE_SIZE ) + { + m_vecData.pop_front(); + } + m_vecData.emplace_back( pBuf, nBufSize ); + } + + int GetInput( unsigned char *data, size_t length ) + { + hid_mutex_guard l( &m_dataLock ); + + if ( m_vecData.size() == 0 ) + { +// LOGV( "hid_read_timeout no data available" ); + return 0; + } + + const hid_buffer &buffer = m_vecData.front(); + size_t nDataLen = buffer.size() > length ? length : buffer.size(); + if ( m_bIsBLESteamController ) + { + data[0] = 0x03; + memcpy( data + 1, buffer.data(), nDataLen ); + ++nDataLen; + } + else + { + memcpy( data, buffer.data(), nDataLen ); + } + m_vecData.pop_front(); + +// LOGV("Read %u bytes", nDataLen); +// LOGV("%02x %02x %02x %02x %02x %02x %02x %02x ....", +// data[0], data[1], data[2], data[3], +// data[4], data[5], data[6], data[7]); + + return nDataLen; + } + + int SendOutputReport( const unsigned char *pData, size_t nDataLen ) + { + // Make sure thread is attached to JVM/env + JNIEnv *env; + g_JVM->AttachCurrentThread( &env, NULL ); + pthread_setspecific( g_ThreadKey, (void*)env ); + + jbyteArray pBuf = NewByteArray( env, pData, nDataLen ); + int nRet = env->CallIntMethod( g_HIDDeviceManagerCallbackHandler, g_midHIDDeviceManagerSendOutputReport, m_nId, pBuf ); + ExceptionCheck( env, "SendOutputReport" ); + + env->DeleteLocalRef( pBuf ); + return nRet; + } + + int SendFeatureReport( const unsigned char *pData, size_t nDataLen ) + { + // Make sure thread is attached to JVM/env + JNIEnv *env; + g_JVM->AttachCurrentThread( &env, NULL ); + pthread_setspecific( g_ThreadKey, (void*)env ); + + jbyteArray pBuf = NewByteArray( env, pData, nDataLen ); + int nRet = env->CallIntMethod( g_HIDDeviceManagerCallbackHandler, g_midHIDDeviceManagerSendFeatureReport, m_nId, pBuf ); + ExceptionCheck( env, "SendFeatureReport" ); + env->DeleteLocalRef( pBuf ); + return nRet; + } + + void ProcessFeatureReport( const uint8_t *pBuf, size_t nBufSize ) + { + hid_mutex_guard cvl( &m_cvLock ); + if ( m_bIsWaitingForFeatureReport ) + { + m_featureReport.assign( pBuf, nBufSize ); + + m_bIsWaitingForFeatureReport = false; + m_nFeatureReportError = 0; + pthread_cond_signal( &m_cv ); + } + } + + int GetFeatureReport( unsigned char *pData, size_t nDataLen ) + { + // Make sure thread is attached to JVM/env + JNIEnv *env; + g_JVM->AttachCurrentThread( &env, NULL ); + pthread_setspecific( g_ThreadKey, (void*)env ); + + { + hid_mutex_guard cvl( &m_cvLock ); + if ( m_bIsWaitingForFeatureReport ) + { + LOGV( "Get feature report already ongoing... bail" ); + return -1; // Read already ongoing, we currently do not serialize, TODO + } + m_bIsWaitingForFeatureReport = true; + } + + jbyteArray pBuf = NewByteArray( env, pData, nDataLen ); + int nRet = env->CallBooleanMethod( g_HIDDeviceManagerCallbackHandler, g_midHIDDeviceManagerGetFeatureReport, m_nId, pBuf ) ? 0 : -1; + ExceptionCheck( env, "GetFeatureReport" ); + env->DeleteLocalRef( pBuf ); + if ( nRet < 0 ) + { + LOGV( "GetFeatureReport failed" ); + m_bIsWaitingForFeatureReport = false; + return -1; + } + + { + hid_mutex_guard cvl( &m_cvLock ); + if ( m_bIsWaitingForFeatureReport ) + { + LOGV("=== Going to sleep" ); + // Wait in CV until we are no longer waiting for a feature report. + const int FEATURE_REPORT_TIMEOUT_SECONDS = 2; + struct timespec ts, endtime; + clock_gettime( CLOCK_REALTIME, &ts ); + endtime = ts; + endtime.tv_sec += FEATURE_REPORT_TIMEOUT_SECONDS; + do + { + if ( pthread_cond_timedwait( &m_cv, &m_cvLock, &endtime ) != 0 ) + { + break; + } + } + while ( m_bIsWaitingForFeatureReport && get_timespec_ms( ts ) < get_timespec_ms( endtime ) ); + + // We are back + if ( m_bIsWaitingForFeatureReport ) + { + m_nFeatureReportError = -ETIMEDOUT; + m_bIsWaitingForFeatureReport = false; + } + LOGV( "=== Got feature report err=%d", m_nFeatureReportError ); + if ( m_nFeatureReportError != 0 ) + { + return m_nFeatureReportError; + } + } + + size_t uBytesToCopy = m_featureReport.size() > nDataLen ? nDataLen : m_featureReport.size(); + memcpy( pData, m_featureReport.data(), uBytesToCopy ); + m_featureReport.clear(); + LOGV( "=== Got %u bytes", uBytesToCopy ); + + return uBytesToCopy; + } + } + + void Close( bool bDeleteDevice ) + { + // Make sure thread is attached to JVM/env + JNIEnv *env; + g_JVM->AttachCurrentThread( &env, NULL ); + pthread_setspecific( g_ThreadKey, (void*)env ); + + env->CallVoidMethod( g_HIDDeviceManagerCallbackHandler, g_midHIDDeviceManagerClose, m_nId ); + ExceptionCheck( env, "Close" ); + + hid_mutex_guard dataLock( &m_dataLock ); + m_vecData.clear(); + + // Clean and release pending feature report reads + hid_mutex_guard cvLock( &m_cvLock ); + m_featureReport.clear(); + m_bIsWaitingForFeatureReport = false; + m_nFeatureReportError = -ECONNRESET; + pthread_cond_broadcast( &m_cv ); + + if ( bDeleteDevice ) + { + delete m_pDevice; + m_pDevice = nullptr; + } + } + +private: + pthread_mutex_t m_refCountLock = PTHREAD_MUTEX_INITIALIZER; + int m_nRefCount = 0; + int m_nId = 0; + hid_device_info *m_pInfo = nullptr; + hid_device *m_pDevice = nullptr; + bool m_bIsBLESteamController = false; + + pthread_mutex_t m_dataLock = PTHREAD_MUTEX_INITIALIZER; // This lock has to be held to access m_vecData + hid_buffer_pool m_vecData; + + // For handling get_feature_report + pthread_mutex_t m_cvLock = PTHREAD_MUTEX_INITIALIZER; // This lock has to be held to access any variables below + pthread_cond_t m_cv = PTHREAD_COND_INITIALIZER; + bool m_bIsWaitingForOpen = false; + bool m_bOpenResult = false; + bool m_bIsWaitingForFeatureReport = false; + int m_nFeatureReportError = 0; + hid_buffer m_featureReport; + +public: + hid_device_ref next; +}; + +class CHIDDevice; +static pthread_mutex_t g_DevicesMutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t g_DevicesRefCountMutex = PTHREAD_MUTEX_INITIALIZER; +static hid_device_ref g_Devices; + +static hid_device_ref FindDevice( int nDeviceId ) +{ + hid_device_ref pDevice; + + hid_mutex_guard l( &g_DevicesMutex ); + for ( pDevice = g_Devices; pDevice; pDevice = pDevice->next ) + { + if ( pDevice->GetId() == nDeviceId ) + { + break; + } + } + return pDevice; +} + +static void ThreadDestroyed(void* value) +{ + /* The thread is being destroyed, detach it from the Java VM and set the g_ThreadKey value to NULL as required */ + JNIEnv *env = (JNIEnv*) value; + if (env != NULL) { + g_JVM->DetachCurrentThread(); + pthread_setspecific(g_ThreadKey, NULL); + } +} + + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceRegisterCallback)(JNIEnv *env, jobject thiz); + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceReleaseCallback)(JNIEnv *env, jobject thiz); + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceConnected)(JNIEnv *env, jobject thiz, int nDeviceID, jstring sIdentifier, int nVendorId, int nProductId, jstring sSerialNumber, int nReleaseNumber, jstring sManufacturer, jstring sProduct, int nInterface ); + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceOpenPending)(JNIEnv *env, jobject thiz, int nDeviceID); + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceOpenResult)(JNIEnv *env, jobject thiz, int nDeviceID, bool bOpened); + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceDisconnected)(JNIEnv *env, jobject thiz, int nDeviceID); + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceInputReport)(JNIEnv *env, jobject thiz, int nDeviceID, jbyteArray value); + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceFeatureReport)(JNIEnv *env, jobject thiz, int nDeviceID, jbyteArray value); + + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceRegisterCallback)(JNIEnv *env, jobject thiz ) +{ + LOGV( "HIDDeviceRegisterCallback()"); + + env->GetJavaVM( &g_JVM ); + + /* + * Create mThreadKey so we can keep track of the JNIEnv assigned to each thread + * Refer to http://developer.android.com/guide/practices/design/jni.html for the rationale behind this + */ + if (pthread_key_create(&g_ThreadKey, ThreadDestroyed) != 0) { + __android_log_print(ANDROID_LOG_ERROR, TAG, "Error initializing pthread key"); + } + + if ( g_HIDDeviceManagerCallbackHandler != NULL ) + { + env->DeleteGlobalRef( g_HIDDeviceManagerCallbackClass ); + g_HIDDeviceManagerCallbackClass = NULL; + env->DeleteGlobalRef( g_HIDDeviceManagerCallbackHandler ); + g_HIDDeviceManagerCallbackHandler = NULL; + } + + g_HIDDeviceManagerCallbackHandler = env->NewGlobalRef( thiz ); + jclass objClass = env->GetObjectClass( thiz ); + if ( objClass ) + { + g_HIDDeviceManagerCallbackClass = reinterpret_cast< jclass >( env->NewGlobalRef( objClass ) ); + g_midHIDDeviceManagerOpen = env->GetMethodID( g_HIDDeviceManagerCallbackClass, "openDevice", "(I)Z" ); + if ( !g_midHIDDeviceManagerOpen ) + { + __android_log_print(ANDROID_LOG_ERROR, TAG, "HIDDeviceRegisterCallback: callback class missing openDevice" ); + } + g_midHIDDeviceManagerSendOutputReport = env->GetMethodID( g_HIDDeviceManagerCallbackClass, "sendOutputReport", "(I[B)I" ); + if ( !g_midHIDDeviceManagerSendOutputReport ) + { + __android_log_print(ANDROID_LOG_ERROR, TAG, "HIDDeviceRegisterCallback: callback class missing sendOutputReport" ); + } + g_midHIDDeviceManagerSendFeatureReport = env->GetMethodID( g_HIDDeviceManagerCallbackClass, "sendFeatureReport", "(I[B)I" ); + if ( !g_midHIDDeviceManagerSendFeatureReport ) + { + __android_log_print(ANDROID_LOG_ERROR, TAG, "HIDDeviceRegisterCallback: callback class missing sendFeatureReport" ); + } + g_midHIDDeviceManagerGetFeatureReport = env->GetMethodID( g_HIDDeviceManagerCallbackClass, "getFeatureReport", "(I[B)Z" ); + if ( !g_midHIDDeviceManagerGetFeatureReport ) + { + __android_log_print(ANDROID_LOG_ERROR, TAG, "HIDDeviceRegisterCallback: callback class missing getFeatureReport" ); + } + g_midHIDDeviceManagerClose = env->GetMethodID( g_HIDDeviceManagerCallbackClass, "closeDevice", "(I)V" ); + if ( !g_midHIDDeviceManagerClose ) + { + __android_log_print(ANDROID_LOG_ERROR, TAG, "HIDDeviceRegisterCallback: callback class missing closeDevice" ); + } + env->DeleteLocalRef( objClass ); + } +} + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceReleaseCallback)(JNIEnv *env, jobject thiz) +{ + LOGV("HIDDeviceReleaseCallback"); + if ( env->IsSameObject( thiz, g_HIDDeviceManagerCallbackHandler ) ) + { + env->DeleteGlobalRef( g_HIDDeviceManagerCallbackClass ); + g_HIDDeviceManagerCallbackClass = NULL; + env->DeleteGlobalRef( g_HIDDeviceManagerCallbackHandler ); + g_HIDDeviceManagerCallbackHandler = NULL; + } +} + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceConnected)(JNIEnv *env, jobject thiz, int nDeviceID, jstring sIdentifier, int nVendorId, int nProductId, jstring sSerialNumber, int nReleaseNumber, jstring sManufacturer, jstring sProduct, int nInterface ) +{ + LOGV( "HIDDeviceConnected() id=%d VID/PID = %.4x/%.4x, interface %d\n", nDeviceID, nVendorId, nProductId, nInterface ); + + hid_device_info *pInfo = new hid_device_info; + memset( pInfo, 0, sizeof( *pInfo ) ); + pInfo->path = CreateStringFromJString( env, sIdentifier ); + pInfo->vendor_id = nVendorId; + pInfo->product_id = nProductId; + pInfo->serial_number = CreateWStringFromJString( env, sSerialNumber ); + pInfo->release_number = nReleaseNumber; + pInfo->manufacturer_string = CreateWStringFromJString( env, sManufacturer ); + pInfo->product_string = CreateWStringFromJString( env, sProduct ); + pInfo->interface_number = nInterface; + + hid_device_ref pDevice( new CHIDDevice( nDeviceID, pInfo ) ); + + hid_mutex_guard l( &g_DevicesMutex ); + hid_device_ref pLast, pCurr; + for ( pCurr = g_Devices; pCurr; pLast = pCurr, pCurr = pCurr->next ) + { + continue; + } + if ( pLast ) + { + pLast->next = pDevice; + } + else + { + g_Devices = pDevice; + } +} + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceOpenPending)(JNIEnv *env, jobject thiz, int nDeviceID) +{ + LOGV( "HIDDeviceOpenPending() id=%d\n", nDeviceID ); + hid_device_ref pDevice = FindDevice( nDeviceID ); + if ( pDevice ) + { + pDevice->SetOpenPending(); + } +} + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceOpenResult)(JNIEnv *env, jobject thiz, int nDeviceID, bool bOpened) +{ + LOGV( "HIDDeviceOpenResult() id=%d, result=%s\n", nDeviceID, bOpened ? "true" : "false" ); + hid_device_ref pDevice = FindDevice( nDeviceID ); + if ( pDevice ) + { + pDevice->SetOpenResult( bOpened ); + } +} + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceDisconnected)(JNIEnv *env, jobject thiz, int nDeviceID) +{ + LOGV( "HIDDeviceDisconnected() id=%d\n", nDeviceID ); + hid_device_ref pDevice; + { + hid_mutex_guard l( &g_DevicesMutex ); + hid_device_ref pLast, pCurr; + for ( pCurr = g_Devices; pCurr; pLast = pCurr, pCurr = pCurr->next ) + { + if ( pCurr->GetId() == nDeviceID ) + { + pDevice = pCurr; + + if ( pLast ) + { + pLast->next = pCurr->next; + } + else + { + g_Devices = pCurr->next; + } + } + } + } + if ( pDevice ) + { + pDevice->Close( false ); + } +} + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceInputReport)(JNIEnv *env, jobject thiz, int nDeviceID, jbyteArray value) +{ + jbyte *pBuf = env->GetByteArrayElements(value, NULL); + jsize nBufSize = env->GetArrayLength(value); + +// LOGV( "HIDDeviceInput() id=%d len=%u\n", nDeviceID, nBufSize ); + hid_device_ref pDevice = FindDevice( nDeviceID ); + if ( pDevice ) + { + pDevice->ProcessInput( reinterpret_cast< const uint8_t* >( pBuf ), nBufSize ); + } + + env->ReleaseByteArrayElements(value, pBuf, 0); +} + +extern "C" +JNIEXPORT void JNICALL HID_DEVICE_MANAGER_JAVA_INTERFACE(HIDDeviceFeatureReport)(JNIEnv *env, jobject thiz, int nDeviceID, jbyteArray value) +{ + jbyte *pBuf = env->GetByteArrayElements(value, NULL); + jsize nBufSize = env->GetArrayLength(value); + + LOGV( "HIDDeviceFeatureReport() id=%d len=%u\n", nDeviceID, nBufSize ); + hid_device_ref pDevice = FindDevice( nDeviceID ); + if ( pDevice ) + { + pDevice->ProcessFeatureReport( reinterpret_cast< const uint8_t* >( pBuf ), nBufSize ); + } + + env->ReleaseByteArrayElements(value, pBuf, 0); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +extern "C" +{ + +int hid_init(void) +{ + return 0; +} + +struct hid_device_info HID_API_EXPORT * HID_API_CALL hid_enumerate(unsigned short vendor_id, unsigned short product_id) +{ + struct hid_device_info *root = NULL; + hid_mutex_guard l( &g_DevicesMutex ); + for ( hid_device_ref pDevice = g_Devices; pDevice; pDevice = pDevice->next ) + { + const hid_device_info *info = pDevice->GetDeviceInfo(); + if ( ( vendor_id == 0 && product_id == 0 ) || + ( vendor_id == info->vendor_id && product_id == info->product_id ) ) + { + hid_device_info *dev = CopyHIDDeviceInfo( info ); + dev->next = root; + root = dev; + } + } + return root; +} + +void HID_API_EXPORT HID_API_CALL hid_free_enumeration(struct hid_device_info *devs) +{ + while ( devs ) + { + struct hid_device_info *next = devs->next; + FreeHIDDeviceInfo( devs ); + devs = next; + } +} + +HID_API_EXPORT hid_device * HID_API_CALL hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number) +{ + // TODO: Implement + return NULL; +} + +HID_API_EXPORT hid_device * HID_API_CALL hid_open_path(const char *path, int bExclusive) +{ + LOGV( "hid_open_path( %s )", path ); + + hid_device_ref< CHIDDevice > pDevice; + { + hid_mutex_guard r( &g_DevicesRefCountMutex ); + hid_mutex_guard l( &g_DevicesMutex ); + for ( hid_device_ref pCurr = g_Devices; pCurr; pCurr = pCurr->next ) + { + if ( strcmp( pCurr->GetDeviceInfo()->path, path ) == 0 ) + { + hid_device *pValue = pCurr->GetDevice(); + if ( pValue ) + { + ++pValue->m_nDeviceRefCount; + LOGD("Incrementing device %d (%p), refCount = %d\n", pValue->m_nId, pValue, pValue->m_nDeviceRefCount); + return pValue; + } + + // Hold a shared pointer to the controller for the duration + pDevice = pCurr; + break; + } + } + } + if ( pDevice && pDevice->BOpen() ) + { + return pDevice->GetDevice(); + } + return NULL; +} + +int HID_API_EXPORT HID_API_CALL hid_write(hid_device *device, const unsigned char *data, size_t length) +{ + LOGV( "hid_write id=%d length=%u", device->m_nId, length ); + hid_device_ref pDevice = FindDevice( device->m_nId ); + if ( pDevice ) + { + return pDevice->SendOutputReport( data, length ); + } + return -1; // Controller was disconnected +} + +// TODO: Implement timeout? +int HID_API_EXPORT HID_API_CALL hid_read_timeout(hid_device *device, unsigned char *data, size_t length, int milliseconds) +{ +// LOGV( "hid_read_timeout id=%d length=%u timeout=%d", device->m_nId, length, milliseconds ); + hid_device_ref pDevice = FindDevice( device->m_nId ); + if ( pDevice ) + { + return pDevice->GetInput( data, length ); + } + LOGV( "controller was disconnected" ); + return -1; // Controller was disconnected +} + +// TODO: Implement blocking +int HID_API_EXPORT HID_API_CALL hid_read(hid_device *device, unsigned char *data, size_t length) +{ + LOGV( "hid_read id=%d length=%u", device->m_nId, length ); + return hid_read_timeout( device, data, length, 0 ); +} + +// TODO: Implement? +int HID_API_EXPORT HID_API_CALL hid_set_nonblocking(hid_device *device, int nonblock) +{ + return -1; +} + +int HID_API_EXPORT HID_API_CALL hid_send_feature_report(hid_device *device, const unsigned char *data, size_t length) +{ + LOGV( "hid_send_feature_report id=%d length=%u", device->m_nId, length ); + hid_device_ref pDevice = FindDevice( device->m_nId ); + if ( pDevice ) + { + return pDevice->SendFeatureReport( data, length ); + } + return -1; // Controller was disconnected +} + + +// Synchronous operation. Will block until completed. +int HID_API_EXPORT HID_API_CALL hid_get_feature_report(hid_device *device, unsigned char *data, size_t length) +{ + LOGV( "hid_get_feature_report id=%d length=%u", device->m_nId, length ); + hid_device_ref pDevice = FindDevice( device->m_nId ); + if ( pDevice ) + { + return pDevice->GetFeatureReport( data, length ); + } + return -1; // Controller was disconnected +} + + +void HID_API_EXPORT HID_API_CALL hid_close(hid_device *device) +{ + LOGV( "hid_close id=%d", device->m_nId ); + hid_mutex_guard r( &g_DevicesRefCountMutex ); + LOGD("Decrementing device %d (%p), refCount = %d\n", device->m_nId, device, device->m_nDeviceRefCount - 1); + if ( --device->m_nDeviceRefCount == 0 ) + { + hid_device_ref pDevice = FindDevice( device->m_nId ); + if ( pDevice ) + { + pDevice->Close( true ); + } + else + { + delete device; + } + LOGD("Deleted device %p\n", device); + } + +} + +int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *device, wchar_t *string, size_t maxlen) +{ + hid_device_ref pDevice = FindDevice( device->m_nId ); + if ( pDevice ) + { + wcsncpy( string, pDevice->GetDeviceInfo()->manufacturer_string, maxlen ); + return 0; + } + return -1; +} + +int HID_API_EXPORT_CALL hid_get_product_string(hid_device *device, wchar_t *string, size_t maxlen) +{ + hid_device_ref pDevice = FindDevice( device->m_nId ); + if ( pDevice ) + { + wcsncpy( string, pDevice->GetDeviceInfo()->product_string, maxlen ); + return 0; + } + return -1; +} + +int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *device, wchar_t *string, size_t maxlen) +{ + hid_device_ref pDevice = FindDevice( device->m_nId ); + if ( pDevice ) + { + wcsncpy( string, pDevice->GetDeviceInfo()->serial_number, maxlen ); + return 0; + } + return -1; +} + +int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *device, int string_index, wchar_t *string, size_t maxlen) +{ + return -1; +} + +HID_API_EXPORT const wchar_t* HID_API_CALL hid_error(hid_device *device) +{ + return NULL; +} + +int hid_exit(void) +{ + return 0; +} + +} diff --git a/Engine/lib/sdl/src/hidapi/android/jni/Android.mk b/Engine/lib/sdl/src/hidapi/android/jni/Android.mk new file mode 100644 index 000000000..4462e88bf --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/android/jni/Android.mk @@ -0,0 +1,16 @@ +LOCAL_PATH:= $(call my-dir) + +HIDAPI_ROOT_REL:= ../.. +HIDAPI_ROOT_ABS:= $(LOCAL_PATH)/../.. + +include $(CLEAR_VARS) + +LOCAL_CPPFLAGS += -std=c++11 + +LOCAL_SRC_FILES := \ + $(HIDAPI_ROOT_REL)/android/hid.cpp + +LOCAL_MODULE := libhidapi +LOCAL_LDLIBS := -llog + +include $(BUILD_SHARED_LIBRARY) diff --git a/Engine/lib/sdl/src/hidapi/android/jni/Application.mk b/Engine/lib/sdl/src/hidapi/android/jni/Application.mk new file mode 100644 index 000000000..4fc6ba506 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/android/jni/Application.mk @@ -0,0 +1,2 @@ +APP_STL := gnustl_static +APP_ABI := armeabi-v7a diff --git a/Engine/lib/sdl/src/hidapi/android/project.properties b/Engine/lib/sdl/src/hidapi/android/project.properties new file mode 100644 index 000000000..6e18427a4 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/android/project.properties @@ -0,0 +1,14 @@ +# This file is automatically generated by Android Tools. +# Do not modify this file -- YOUR CHANGES WILL BE ERASED! +# +# This file must be checked in Version Control Systems. +# +# To customize properties used by the Ant build system edit +# "ant.properties", and override values to adapt the script to your +# project structure. +# +# To enable ProGuard to shrink and obfuscate your code, uncomment this (available properties: sdk.dir, user.home): +#proguard.config=${sdk.dir}/tools/proguard/proguard-android.txt:proguard-project.txt + +# Project target. +target=android-21 diff --git a/Engine/lib/sdl/src/hidapi/bootstrap b/Engine/lib/sdl/src/hidapi/bootstrap new file mode 100644 index 000000000..81e9b74b6 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/bootstrap @@ -0,0 +1,2 @@ +#!/bin/sh -x +autoreconf --install --verbose --force diff --git a/Engine/lib/sdl/src/hidapi/configure.ac b/Engine/lib/sdl/src/hidapi/configure.ac new file mode 100644 index 000000000..c6747f906 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/configure.ac @@ -0,0 +1,236 @@ +AC_PREREQ(2.63) + +# Version number. This is currently the only place. +m4_define([HIDAPI_MAJOR], 0) +m4_define([HIDAPI_MINOR], 8) +m4_define([HIDAPI_RELEASE], 0) +m4_define([HIDAPI_RC], -rc1) +m4_define([VERSION_STRING], HIDAPI_MAJOR[.]HIDAPI_MINOR[.]HIDAPI_RELEASE[]HIDAPI_RC) + +AC_INIT([hidapi],[VERSION_STRING],[alan@signal11.us]) + +# Library soname version +# Follow the following rules (particularly the ones in the second link): +# http://www.gnu.org/software/libtool/manual/html_node/Updating-version-info.html +# http://sourceware.org/autobook/autobook/autobook_91.html +lt_current="0" +lt_revision="0" +lt_age="0" +LTLDFLAGS="-version-info ${lt_current}:${lt_revision}:${lt_age}" + +AC_CONFIG_MACRO_DIR([m4]) +AM_INIT_AUTOMAKE([foreign -Wall -Werror]) +AC_CONFIG_MACRO_DIR([m4]) + +m4_ifdef([AM_PROG_AR], [AM_PROG_AR]) +LT_INIT + +AC_PROG_CC +AC_PROG_CXX +AC_PROG_OBJC +PKG_PROG_PKG_CONFIG + + +m4_ifdef([AM_SILENT_RULES], [AM_SILENT_RULES([yes])]) + +hidapi_lib_error() { + echo "" + echo " Library $1 was not found on this system." + echo " Please install it and re-run ./configure" + echo "" + exit 1 +} + +hidapi_prog_error() { + echo "" + echo " Program $1 was not found on this system." + echo " This program is part of $2." + echo " Please install it and re-run ./configure" + echo "" + exit 1 +} + +AC_MSG_CHECKING([operating system]) +AC_MSG_RESULT($host) +case $host in +*-linux*) + AC_MSG_RESULT([ (Linux back-end)]) + AC_DEFINE(OS_LINUX, 1, [Linux implementations]) + AC_SUBST(OS_LINUX) + backend="linux" + os="linux" + threads="pthreads" + + # HIDAPI/hidraw libs + PKG_CHECK_MODULES([libudev], [libudev], true, [hidapi_lib_error libudev]) + LIBS_HIDRAW_PR+=" $libudev_LIBS" + CFLAGS_HIDRAW+=" $libudev_CFLAGS" + + # HIDAPI/libusb libs + AC_CHECK_LIB([rt], [clock_gettime], [LIBS_LIBUSB_PRIVATE+=" -lrt"], [hidapi_lib_error librt]) + PKG_CHECK_MODULES([libusb], [libusb-1.0 >= 1.0.9], true, [hidapi_lib_error libusb-1.0]) + LIBS_LIBUSB_PRIVATE+=" $libusb_LIBS" + CFLAGS_LIBUSB+=" $libusb_CFLAGS" + ;; +*-darwin*) + AC_MSG_RESULT([ (Mac OS X back-end)]) + AC_DEFINE(OS_DARWIN, 1, [Mac implementation]) + AC_SUBST(OS_DARWIN) + backend="mac" + os="darwin" + threads="pthreads" + LIBS="${LIBS} -framework IOKit -framework CoreFoundation" + ;; +*-freebsd*) + AC_MSG_RESULT([ (FreeBSD back-end)]) + AC_DEFINE(OS_FREEBSD, 1, [FreeBSD implementation]) + AC_SUBST(OS_FREEBSD) + backend="libusb" + os="freebsd" + threads="pthreads" + + CFLAGS="$CFLAGS -I/usr/local/include" + LDFLAGS="$LDFLAGS -L/usr/local/lib" + LIBS="${LIBS}" + AC_CHECK_LIB([usb], [libusb_init], [LIBS_LIBUSB_PRIVATE="${LIBS_LIBUSB_PRIVATE} -lusb"], [hidapi_lib_error libusb]) + AC_CHECK_LIB([iconv], [iconv_open], [LIBS_LIBUSB_PRIVATE="${LIBS_LIBUSB_PRIVATE} -liconv"], [hidapi_lib_error libiconv]) + echo libs_priv: $LIBS_LIBUSB_PRIVATE + ;; +*-kfreebsd*) + AC_MSG_RESULT([ (kFreeBSD back-end)]) + AC_DEFINE(OS_KFREEBSD, 1, [kFreeBSD implementation]) + AC_SUBST(OS_KFREEBSD) + backend="libusb" + os="kfreebsd" + threads="pthreads" + + AC_CHECK_LIB([usb], [libusb_init], [LIBS_LIBUSB_PRIVATE="${LIBS_LIBUSB_PRIVATE} -lusb"], [hidapi_lib_error libusb]) + echo libs_priv: $LIBS_LIBUSB_PRIVATE + ;; +*-mingw*) + AC_MSG_RESULT([ (Windows back-end, using MinGW)]) + backend="windows" + os="windows" + threads="windows" + win_implementation="mingw" + ;; +*-cygwin*) + AC_MSG_RESULT([ (Windows back-end, using Cygwin)]) + backend="windows" + os="windows" + threads="windows" + win_implementation="cygwin" + ;; +*) + AC_MSG_ERROR([HIDAPI is not supported on your operating system yet]) +esac + +LIBS_HIDRAW="${LIBS} ${LIBS_HIDRAW_PR}" +LIBS_LIBUSB="${LIBS} ${LIBS_LIBUSB_PRIVATE}" +AC_SUBST([LIBS_HIDRAW]) +AC_SUBST([LIBS_LIBUSB]) +AC_SUBST([CFLAGS_LIBUSB]) +AC_SUBST([CFLAGS_HIDRAW]) + +if test "x$os" = xwindows; then + AC_DEFINE(OS_WINDOWS, 1, [Windows implementations]) + AC_SUBST(OS_WINDOWS) + LDFLAGS="${LDFLAGS} -no-undefined" + LIBS="${LIBS} -lsetupapi" +fi + +if test "x$threads" = xpthreads; then + AX_PTHREAD([found_pthreads=yes], [found_pthreads=no]) + + if test "x$found_pthreads" = xyes; then + if test "x$os" = xlinux; then + # Only use pthreads for libusb implementation on Linux. + LIBS_LIBUSB="$PTHREAD_LIBS $LIBS_LIBUSB" + CFLAGS_LIBUSB="$CFLAGS_LIBUSB $PTHREAD_CFLAGS" + # There's no separate CC on Linux for threading, + # so it's ok that both implementations use $PTHREAD_CC + CC="$PTHREAD_CC" + else + LIBS="$PTHREAD_LIBS $LIBS" + CFLAGS="$CFLAGS $PTHREAD_CFLAGS" + CC="$PTHREAD_CC" + fi + fi +fi + +# Test GUI +AC_ARG_ENABLE([testgui], + [AS_HELP_STRING([--enable-testgui], + [enable building of test GUI (default n)])], + [testgui_enabled=$enableval], + [testgui_enabled='no']) +AM_CONDITIONAL([BUILD_TESTGUI], [test "x$testgui_enabled" != "xno"]) + +# Configure the MacOS TestGUI app bundle +rm -Rf testgui/TestGUI.app +mkdir -p testgui/TestGUI.app +cp -R ${srcdir}/testgui/TestGUI.app.in/* testgui/TestGUI.app +chmod -R u+w testgui/TestGUI.app +mkdir testgui/TestGUI.app/Contents/MacOS/ + +if test "x$testgui_enabled" != "xno"; then + if test "x$os" = xdarwin; then + # On Mac OS, don't use pkg-config. + AC_CHECK_PROG([foxconfig], [fox-config], [fox-config], false) + if test "x$foxconfig" = "xfalse"; then + hidapi_prog_error fox-config "FOX Toolkit" + fi + LIBS_TESTGUI+=`$foxconfig --libs` + LIBS_TESTGUI+=" -framework Cocoa -L/usr/X11R6/lib" + CFLAGS_TESTGUI+=`$foxconfig --cflags` + OBJCFLAGS+=" -x objective-c++" + elif test "x$os" = xwindows; then + # On Windows, just set the paths for Fox toolkit + if test "x$win_implementation" = xmingw; then + CFLAGS_TESTGUI="-I\$(srcdir)/../../hidapi-externals/fox/include -g -c" + LIBS_TESTGUI=" -mwindows \$(srcdir)/../../hidapi-externals/fox/lib/libFOX-1.6.a -lgdi32 -Wl,--enable-auto-import -static-libgcc -static-libstdc++ -lkernel32" + else + # Cygwin + CFLAGS_TESTGUI="-DWIN32 -I\$(srcdir)/../../hidapi-externals/fox/include -g -c" + LIBS_TESTGUI="\$(srcdir)/../../hidapi-externals/fox/lib/libFOX-cygwin-1.6.a -lgdi32 -Wl,--enable-auto-import -static-libgcc -static-libstdc++ -lkernel32" + fi + else + # On Linux and FreeBSD platforms, use pkg-config to find fox. + PKG_CHECK_MODULES([fox], [fox17], [], [PKG_CHECK_MODULES([fox], [fox])]) + LIBS_TESTGUI="${LIBS_TESTGUI} $fox_LIBS" + if test "x$os" = xfreebsd; then + LIBS_TESTGUI="${LIBS_TESTGUI} -L/usr/local/lib" + fi + CFLAGS_TESTGUI="${CFLAGS_TESTGUI} $fox_CFLAGS" + fi +fi +AC_SUBST([LIBS_TESTGUI]) +AC_SUBST([CFLAGS_TESTGUI]) +AC_SUBST([backend]) + +# OS info for Automake +AM_CONDITIONAL(OS_LINUX, test "x$os" = xlinux) +AM_CONDITIONAL(OS_DARWIN, test "x$os" = xdarwin) +AM_CONDITIONAL(OS_FREEBSD, test "x$os" = xfreebsd) +AM_CONDITIONAL(OS_KFREEBSD, test "x$os" = xkfreebsd) +AM_CONDITIONAL(OS_WINDOWS, test "x$os" = xwindows) + +AC_CONFIG_HEADERS([config.h]) + +if test "x$os" = "xlinux"; then + AC_CONFIG_FILES([pc/hidapi-hidraw.pc]) + AC_CONFIG_FILES([pc/hidapi-libusb.pc]) +else + AC_CONFIG_FILES([pc/hidapi.pc]) +fi + +AC_SUBST(LTLDFLAGS) + +AC_CONFIG_FILES([Makefile \ + hidtest/Makefile \ + libusb/Makefile \ + linux/Makefile \ + mac/Makefile \ + testgui/Makefile \ + windows/Makefile]) +AC_OUTPUT diff --git a/Engine/lib/sdl/src/hidapi/doxygen/Doxyfile b/Engine/lib/sdl/src/hidapi/doxygen/Doxyfile new file mode 100644 index 000000000..9d983e9f2 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/doxygen/Doxyfile @@ -0,0 +1,1630 @@ +# Doxyfile 1.7.1 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project +# +# All text after a hash (#) is considered a comment and will be ignored +# The format is: +# TAG = value [value, ...] +# For lists items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (" ") + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# http://www.gnu.org/software/libiconv for the list of possible encodings. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded +# by quotes) that should identify the project. + +PROJECT_NAME = hidapi + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. +# This could be handy for archiving the generated documentation or +# if some version control system is used. + +PROJECT_NUMBER = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) +# base path where the generated documentation will be put. +# If a relative path is entered, it will be relative to the location +# where doxygen was started. If left blank the current directory will be used. + +OUTPUT_DIRECTORY = + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create +# 4096 sub-directories (in 2 levels) under the output directory of each output +# format and will distribute the generated files over these directories. +# Enabling this option can be useful when feeding doxygen a huge amount of +# source files, where putting all generated files in the same directory would +# otherwise cause performance problems for the file system. + +CREATE_SUBDIRS = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# The default language is English, other supported languages are: +# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, +# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, +# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English +# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, +# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrilic, Slovak, +# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will +# include brief member descriptions after the members that are listed in +# the file and class documentation (similar to JavaDoc). +# Set to NO to disable this. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend +# the brief description of a member or function before the detailed description. +# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator +# that is used to form the text in various listings. Each string +# in this list, if found as the leading text of the brief description, will be +# stripped from the text and the result after processing the whole list, is +# used as the annotated text. Otherwise, the brief description is used as-is. +# If left blank, the following values are used ("$name" is automatically +# replaced with the name of the entity): "The $name class" "The $name widget" +# "The $name file" "is" "provides" "specifies" "contains" +# "represents" "a" "an" "the" + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# Doxygen will generate a detailed section even if there is only a brief +# description. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full +# path before files name in the file list and in the header files. If set +# to NO the shortest path that makes the file name unique will be used. + +FULL_PATH_NAMES = YES + +# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag +# can be used to strip a user-defined part of the path. Stripping is +# only done if one of the specified strings matches the left-hand part of +# the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the +# path to strip. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of +# the path mentioned in the documentation of a class, which tells +# the reader which header file to include in order to use a class. +# If left blank only the name of the header file containing the class +# definition is used. Otherwise one should specify the include paths that +# are normally passed to the compiler using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter +# (but less readable) file names. This can be useful is your file systems +# doesn't support long names like on DOS, Mac, or CD-ROM. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen +# will interpret the first line (until the first dot) of a JavaDoc-style +# comment as the brief description. If set to NO, the JavaDoc +# comments will behave just like regular Qt-style comments +# (thus requiring an explicit @brief command for a brief description.) + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then Doxygen will +# interpret the first line (until the first dot) of a Qt-style +# comment as the brief description. If set to NO, the comments +# will behave just like regular Qt-style comments (thus requiring +# an explicit \brief command for a brief description.) + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen +# treat a multi-line C++ special comment block (i.e. a block of //! or /// +# comments) as a brief description. This used to be the default behaviour. +# The new default is to treat a multi-line C++ comment block as a detailed +# description. Set this tag to YES if you prefer the old behaviour instead. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented +# member inherits the documentation from any documented member that it +# re-implements. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce +# a new page for each member. If set to NO, the documentation of a member will +# be part of the file/class/namespace that contains it. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. +# Doxygen uses this value to replace tabs by spaces in code fragments. + +TAB_SIZE = 8 + +# This tag can be used to specify a number of aliases that acts +# as commands in the documentation. An alias has the form "name=value". +# For example adding "sideeffect=\par Side Effects:\n" will allow you to +# put the command \sideeffect (or @sideeffect) in the documentation, which +# will result in a user-defined paragraph with heading "Side Effects:". +# You can put \n's in the value part of an alias to insert newlines. + +ALIASES = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C +# sources only. Doxygen will then generate output that is more tailored for C. +# For instance, some of the names that are used will be different. The list +# of all members will be omitted, etc. + +OPTIMIZE_OUTPUT_FOR_C = YES + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java +# sources only. Doxygen will then generate output that is more tailored for +# Java. For instance, namespaces will be presented as packages, qualified +# scopes will look different, etc. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources only. Doxygen will then generate output that is more tailored for +# Fortran. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for +# VHDL. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given extension. +# Doxygen has a built-in mapping, but you can override or extend it using this +# tag. The format is ext=language, where ext is a file extension, and language +# is one of the parsers supported by doxygen: IDL, Java, Javascript, CSharp, C, +# C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, C++. For instance to make +# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C +# (default is Fortran), use: inc=Fortran f=C. Note that for custom extensions +# you also need to set FILE_PATTERNS otherwise the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should +# set this tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. +# func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. +# Doxygen will parse them like normal C++ but will assume all classes use public +# instead of private inheritance when no explicit protection keyword is present. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate getter +# and setter methods for a property. Setting this option to YES (the default) +# will make doxygen to replace the get and set methods by a property in the +# documentation. This will only work if the methods are indeed getting or +# setting a simple type. If this is not the case, or you want to show the +# methods anyway, you should set this option to NO. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES (the default) to allow class member groups of +# the same type (for instance a group of public functions) to be put as a +# subgroup of that type (e.g. under the Public Functions section). Set it to +# NO to prevent subgrouping. Alternatively, this can be done per class using +# the \nosubgrouping command. + +SUBGROUPING = YES + +# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum +# is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically +# be useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. + +TYPEDEF_HIDES_STRUCT = NO + +# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to +# determine which symbols to keep in memory and which to flush to disk. +# When the cache is full, less often used symbols will be written to disk. +# For small to medium size projects (<1000 input files) the default value is +# probably good enough. For larger projects a too small cache size can cause +# doxygen to be busy swapping symbols to and from disk most of the time +# causing a significant performance penality. +# If the system has enough physical memory increasing the cache will improve the +# performance by keeping more symbols in memory. Note that the value works on +# a logarithmic scale so increasing the size by one will rougly double the +# memory usage. The cache size is given by this formula: +# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols + +SYMBOL_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. +# Private class members and static file members will be hidden unless +# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES + +EXTRACT_ALL = NO + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class +# will be included in the documentation. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_STATIC tag is set to YES all static members of a file +# will be included in the documentation. + +EXTRACT_STATIC = NO + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) +# defined locally in source files will be included in the documentation. +# If set to NO only classes defined in header files are included. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local +# methods, which are defined in the implementation section but not in +# the interface are included in the documentation. +# If set to NO (the default) only methods in the interface are included. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base +# name of the file that contains the anonymous namespace. By default +# anonymous namespace are hidden. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all +# undocumented members of documented classes, files or namespaces. +# If set to NO (the default) these members will be included in the +# various overviews, but no documentation section is generated. +# This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. +# If set to NO (the default) these classes will be included in the various +# overviews. This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all +# friend (class|struct|union) declarations. +# If set to NO (the default) these declarations will be included in the +# documentation. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any +# documentation blocks found inside the body of a function. +# If set to NO (the default) these blocks will be appended to the +# function's detailed documentation block. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation +# that is typed after a \internal command is included. If the tag is set +# to NO (the default) then the documentation will be excluded. +# Set it to YES to include the internal documentation. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate +# file names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen +# will show members with their full class and namespace scopes in the +# documentation. If set to YES the scope will be hidden. + +HIDE_SCOPE_NAMES = NO + +# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen +# will put a list of the files that are included by a file in the documentation +# of that file. + +SHOW_INCLUDE_FILES = YES + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen +# will list include files with double quotes in the documentation +# rather than with sharp brackets. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] +# is inserted in the documentation for inline members. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen +# will sort the (detailed) documentation of file and class members +# alphabetically by member name. If set to NO the members will appear in +# declaration order. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the +# brief documentation of file, namespace and class members alphabetically +# by member name. If set to NO (the default) the members will appear in +# declaration order. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen +# will sort the (brief and detailed) documentation of class members so that +# constructors and destructors are listed first. If set to NO (the default) +# the constructors will appear in the respective orders defined by +# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. +# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO +# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the +# hierarchy of group names into alphabetical order. If set to NO (the default) +# the group names will appear in their defined order. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be +# sorted by fully-qualified names, including namespaces. If set to +# NO (the default), the class list will be sorted only by class name, +# not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the +# alphabetical list. + +SORT_BY_SCOPE_NAME = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or +# disable (NO) the todo list. This list is created by putting \todo +# commands in the documentation. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or +# disable (NO) the test list. This list is created by putting \test +# commands in the documentation. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or +# disable (NO) the bug list. This list is created by putting \bug +# commands in the documentation. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or +# disable (NO) the deprecated list. This list is created by putting +# \deprecated commands in the documentation. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional +# documentation sections, marked by \if sectionname ... \endif. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines +# the initial value of a variable or define consists of for it to appear in +# the documentation. If the initializer consists of more lines than specified +# here it will be hidden. Use a value of 0 to hide initializers completely. +# The appearance of the initializer of individual variables and defines in the +# documentation can be controlled using \showinitializer or \hideinitializer +# command in the documentation regardless of this setting. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated +# at the bottom of the documentation of classes and structs. If set to YES the +# list will mention the files that were used to generate the documentation. + +SHOW_USED_FILES = YES + +# If the sources in your project are distributed over multiple directories +# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy +# in the documentation. The default is NO. + +SHOW_DIRECTORIES = NO + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. +# This will remove the Files entry from the Quick Index and from the +# Folder Tree View (if specified). The default is YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the +# Namespaces page. +# This will remove the Namespaces entry from the Quick Index +# and from the Folder Tree View (if specified). The default is YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command , where is the value of +# the FILE_VERSION_FILTER tag, and is the name of an input file +# provided by doxygen. Whatever the program writes to standard output +# is used as the file version. See the manual for examples. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. The create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. +# You can optionally specify a file name after the option, if omitted +# DoxygenLayout.xml will be used as the name of the layout file. + +LAYOUT_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated +# by doxygen. Possible values are YES and NO. If left blank NO is used. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated by doxygen. Possible values are YES and NO. If left blank +# NO is used. + +WARNINGS = YES + +# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings +# for undocumented members. If EXTRACT_ALL is set to YES then this flag will +# automatically be disabled. + +WARN_IF_UNDOCUMENTED = YES + +# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some +# parameters in a documented function, or documenting parameters that +# don't exist or using markup commands wrongly. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be abled to get warnings for +# functions that are documented, but have no documentation for their parameters +# or return value. If set to NO (the default) doxygen will only warn about +# wrong or incomplete parameter documentation, but not about the absence of +# documentation. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that +# doxygen can produce. The string should contain the $file, $line, and $text +# tags, which will be replaced by the file and line number from which the +# warning originated and the warning text. Optionally the format may contain +# $version, which will be replaced by the version of the file (if it could +# be obtained via FILE_VERSION_FILTER) + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning +# and error messages should be written. If left blank the output is written +# to stderr. + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag can be used to specify the files and/or directories that contain +# documented source files. You may enter file names like "myfile.cpp" or +# directories like "/usr/src/myproject". Separate the files or directories +# with spaces. + +INPUT = ../hidapi + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is +# also the default input encoding. Doxygen uses libiconv (or the iconv built +# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for +# the list of possible encodings. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank the following patterns are tested: +# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx +# *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.py *.f90 + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to turn specify whether or not subdirectories +# should be searched for input files as well. Possible values are YES and NO. +# If left blank NO is used. + +RECURSIVE = NO + +# The EXCLUDE tag can be used to specify files and/or directories that should +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used select whether or not files or +# directories that are symbolic links (a Unix filesystem feature) are excluded +# from the input. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. Note that the wildcards are matched +# against the file with absolute path, so to exclude all test directories +# for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or +# directories that contain example code fragments that are included (see +# the \include command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude +# commands irrespective of the value of the RECURSIVE tag. +# Possible values are YES and NO. If left blank NO is used. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or +# directories that contain image that are included in the documentation (see +# the \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command , where +# is the value of the INPUT_FILTER tag, and is the name of an +# input file. Doxygen will then use the output that the filter program writes +# to standard output. +# If FILTER_PATTERNS is specified, this tag will be +# ignored. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. +# Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. +# The filters are a list of the form: +# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further +# info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER +# is applied to all files. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will be used to filter the input files when producing source +# files to browse (i.e. when SOURCE_BROWSER is set to YES). + +FILTER_SOURCE_FILES = NO + +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will +# be generated. Documented entities will be cross-referenced with these sources. +# Note: To get rid of all source code in the generated output, make sure also +# VERBATIM_HEADERS is set to NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body +# of functions and classes directly in the documentation. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct +# doxygen to hide any special comment blocks from generated source code +# fragments. Normal C and C++ comments will always remain visible. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES +# then for each documented function all documented +# functions referencing it will be listed. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES +# then for each documented function all documented entities +# called/used by that function will be listed. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) +# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from +# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will +# link to the source code. +# Otherwise they will link to the documentation. + +REFERENCES_LINK_SOURCE = YES + +# If the USE_HTAGS tag is set to YES then the references to source code +# will point to the HTML generated by the htags(1) tool instead of doxygen +# built-in source browser. The htags tool is part of GNU's global source +# tagging system (see http://www.gnu.org/software/global/global.html). You +# will need version 4.8.6 or higher. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen +# will generate a verbatim copy of the header file for each class for +# which an include is specified. Set to NO to disable this. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index +# of all compounds will be generated. Enable this if the project +# contains a lot of classes, structs, unions or interfaces. + +ALPHABETICAL_INDEX = YES + +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all +# classes will be put under the same header in the alphabetical index. +# The IGNORE_PREFIX tag can be used to specify one or more prefixes that +# should be ignored while generating the index headers. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES (the default) Doxygen will +# generate HTML output. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `html' will be used as the default path. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for +# each generated HTML page (for example: .htm,.php,.asp). If it is left blank +# doxygen will generate files with .html extension. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a personal HTML header for +# each generated HTML page. If it is left blank doxygen will generate a +# standard header. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a personal HTML footer for +# each generated HTML page. If it is left blank doxygen will generate a +# standard footer. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading +# style sheet that is used by each HTML page. It can be used to +# fine-tune the look of the HTML output. If the tag is left blank doxygen +# will generate a default style sheet. Note that doxygen will try to copy +# the style sheet file to the HTML output directory, so don't put your own +# stylesheet in the HTML output directory as well, or it will be erased! + +HTML_STYLESHEET = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. +# Doxygen will adjust the colors in the stylesheet and background images +# according to this color. Hue is specified as an angle on a colorwheel, +# see http://en.wikipedia.org/wiki/Hue for more information. +# For instance the value 0 represents red, 60 is yellow, 120 is green, +# 180 is cyan, 240 is blue, 300 purple, and 360 is red again. +# The allowed range is 0 to 359. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of +# the colors in the HTML output. For a value of 0 the output will use +# grayscales only. A value of 255 will produce the most vivid colors. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to +# the luminance component of the colors in the HTML output. Values below +# 100 gradually make the output lighter, whereas values above 100 make +# the output darker. The value divided by 100 is the actual gamma applied, +# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2, +# and 100 does not change the gamma. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting +# this to NO can help when comparing the output of multiple runs. + +HTML_TIMESTAMP = YES + +# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, +# files or namespaces will be aligned in HTML using tables. If set to +# NO a bullet list will be used. + +HTML_ALIGN_MEMBERS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. For this to work a browser that supports +# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox +# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). + +HTML_DYNAMIC_SECTIONS = NO + +# If the GENERATE_DOCSET tag is set to YES, additional index files +# will be generated that can be used as input for Apple's Xcode 3 +# integrated development environment, introduced with OSX 10.5 (Leopard). +# To create a documentation set, doxygen will generate a Makefile in the +# HTML output directory. Running make will produce the docset in that +# directory and running "make install" will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find +# it at startup. +# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. + +GENERATE_DOCSET = NO + +# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the +# feed. A documentation feed provides an umbrella under which multiple +# documentation sets from a single provider (such as a company or product suite) +# can be grouped. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that +# should uniquely identify the documentation set bundle. This should be a +# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen +# will append .docset to the name. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES, additional index files +# will be generated that can be used as input for tools like the +# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) +# of the generated HTML documentation. + +GENERATE_HTMLHELP = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can +# be used to specify the file name of the resulting .chm file. You +# can add a path in front of the file if the result should not be +# written to the html output directory. + +CHM_FILE = + +# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can +# be used to specify the location (absolute path including file name) of +# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run +# the HTML help compiler on the generated index.hhp. + +HHC_LOCATION = + +# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag +# controls if a separate .chi index file is generated (YES) or that +# it should be included in the master .chm file (NO). + +GENERATE_CHI = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING +# is used to encode HtmlHelp index (hhk), content (hhc) and project file +# content. + +CHM_INDEX_ENCODING = + +# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag +# controls whether a binary table of contents is generated (YES) or a +# normal table of contents (NO) in the .chm file. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members +# to the contents of the HTML help documentation and to the tree view. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated +# that can be used as input for Qt's qhelpgenerator to generate a +# Qt Compressed Help (.qch) of the generated HTML documentation. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can +# be used to specify the file name of the resulting .qch file. +# The path specified is relative to the HTML output folder. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#namespace + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#virtual-folders + +QHP_VIRTUAL_FOLDER = doc + +# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to +# add. For more information please see +# http://doc.trolltech.com/qthelpproject.html#custom-filters + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see +#     +# Qt Help Project / Custom Filters. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's +# filter section matches. +# +# Qt Help Project / Filter Attributes. + +QHP_SECT_FILTER_ATTRS = + +# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can +# be used to specify the location of Qt's qhelpgenerator. +# If non-empty doxygen will try to run qhelpgenerator on the generated +# .qhp file. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files +# will be generated, which together with the HTML files, form an Eclipse help +# plugin. To install this plugin and make it available under the help contents +# menu in Eclipse, the contents of the directory containing the HTML and XML +# files needs to be copied into the plugins directory of eclipse. The name of +# the directory within the plugins directory should be the same as +# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before +# the help appears. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have +# this name. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# The DISABLE_INDEX tag can be used to turn on/off the condensed index at +# top of each HTML page. The value NO (the default) enables the index and +# the value YES disables it. + +DISABLE_INDEX = NO + +# This tag can be used to set the number of enum values (range [1..20]) +# that doxygen will group on one line in the generated HTML documentation. + +ENUM_VALUES_PER_LINE = 4 + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. +# If the tag value is set to YES, a side panel will be generated +# containing a tree-like index structure (just like the one that +# is generated for HTML Help). For this to work a browser that supports +# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). +# Windows users are probably better off using the HTML help feature. + +GENERATE_TREEVIEW = NO + +# By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories, +# and Class Hierarchy pages using a tree view instead of an ordered list. + +USE_INLINE_TREES = NO + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be +# used to set the initial width (in pixels) of the frame in which the tree +# is shown. + +TREEVIEW_WIDTH = 250 + +# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open +# links to external symbols imported via tag files in a separate window. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of Latex formulas included +# as images in the HTML documentation. The default is 10. Note that +# when you change the font size after a successful doxygen run you need +# to manually remove any form_*.png images from the HTML output directory +# to force them to be regenerated. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are +# not supported properly for IE 6.0, but are supported on all modern browsers. +# Note that when changing this option you need to delete any form_*.png files +# in the HTML output before the changes have effect. + +FORMULA_TRANSPARENT = YES + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box +# for the HTML output. The underlying search engine uses javascript +# and DHTML and should work on any modern browser. Note that when using +# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets +# (GENERATE_DOCSET) there is already a search function so this one should +# typically be disabled. For large projects the javascript based search engine +# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution. + +SEARCHENGINE = YES + +# When the SERVER_BASED_SEARCH tag is enabled the search engine will be +# implemented using a PHP enabled web server instead of at the web client +# using Javascript. Doxygen will generate the search PHP script and index +# file to put on the web server. The advantage of the server +# based approach is that it scales better to large projects and allows +# full text search. The disadvances is that it is more difficult to setup +# and does not have live searching capabilities. + +SERVER_BASED_SEARCH = NO + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will +# generate Latex output. + +GENERATE_LATEX = NO + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `latex' will be used as the default path. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. If left blank `latex' will be used as the default command name. +# Note that when enabling USE_PDFLATEX this option is only used for +# generating bitmaps for formulas in the HTML output, but not in the +# Makefile that is written to the output directory. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to +# generate index for LaTeX. If left blank `makeindex' will be used as the +# default command name. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact +# LaTeX documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used +# by the printer. Possible values are: a4, a4wide, letter, legal and +# executive. If left blank a4wide will be used. + +PAPER_TYPE = a4wide + +# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX +# packages that should be included in the LaTeX output. + +EXTRA_PACKAGES = + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for +# the generated latex document. The header should contain everything until +# the first chapter. If it is left blank doxygen will generate a +# standard header. Notice: only use this tag if you know what you are doing! + +LATEX_HEADER = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated +# is prepared for conversion to pdf (using ps2pdf). The pdf file will +# contain links (just like the HTML output) instead of page references +# This makes the output suitable for online browsing using a pdf viewer. + +PDF_HYPERLINKS = YES + +# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of +# plain latex in the generated Makefile. Set this option to YES to get a +# higher quality PDF documentation. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. +# command to the generated LaTeX files. This will instruct LaTeX to keep +# running if errors occur, instead of asking the user for help. +# This option is also used when generating formulas in HTML. + +LATEX_BATCHMODE = NO + +# If LATEX_HIDE_INDICES is set to YES then doxygen will not +# include the index chapters (such as File Index, Compound Index, etc.) +# in the output. + +LATEX_HIDE_INDICES = NO + +# If LATEX_SOURCE_CODE is set to YES then doxygen will include +# source code with syntax highlighting in the LaTeX output. +# Note that which sources are shown also depends on other settings +# such as SOURCE_BROWSER. + +LATEX_SOURCE_CODE = NO + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output +# The RTF output is optimized for Word 97 and may not look very pretty with +# other RTF readers or editors. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `rtf' will be used as the default path. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES Doxygen generates more compact +# RTF documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated +# will contain hyperlink fields. The RTF file will +# contain links (just like the HTML output) instead of page references. +# This makes the output suitable for online browsing using WORD or other +# programs which support those fields. +# Note: wordpad (write) and others do not support links. + +RTF_HYPERLINKS = NO + +# Load stylesheet definitions from file. Syntax is similar to doxygen's +# config file, i.e. a series of assignments. You only have to provide +# replacements, missing definitions are set to their default value. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an rtf document. +# Syntax is similar to doxygen's config file. + +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES (the default) Doxygen will +# generate man pages + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `man' will be used as the default path. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to +# the generated man pages (default is the subroutine's section .3) + +MAN_EXTENSION = .3 + +# If the MAN_LINKS tag is set to YES and Doxygen generates man output, +# then it will generate one additional man file for each entity +# documented in the real man page(s). These additional files +# only source the real man page, but without them the man command +# would be unable to find the correct page. The default is NO. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES Doxygen will +# generate an XML file that captures the structure of +# the code including all documentation. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `xml' will be used as the default path. + +XML_OUTPUT = xml + +# The XML_SCHEMA tag can be used to specify an XML schema, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_SCHEMA = + +# The XML_DTD tag can be used to specify an XML DTD, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_DTD = + +# If the XML_PROGRAMLISTING tag is set to YES Doxygen will +# dump the program listings (including syntax highlighting +# and cross-referencing information) to the XML output. Note that +# enabling this will significantly increase the size of the XML output. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will +# generate an AutoGen Definitions (see autogen.sf.net) file +# that captures the structure of the code including all +# documentation. Note that this feature is still experimental +# and incomplete at the moment. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES Doxygen will +# generate a Perl module file that captures the structure of +# the code including all documentation. Note that this +# feature is still experimental and incomplete at the +# moment. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES Doxygen will generate +# the necessary Makefile rules, Perl scripts and LaTeX code to be able +# to generate PDF and DVI output from the Perl module output. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be +# nicely formatted so it can be parsed by a human reader. +# This is useful +# if you want to understand what is going on. +# On the other hand, if this +# tag is set to NO the size of the Perl module output will be much smaller +# and Perl will parse it just the same. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file +# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. +# This is useful so different doxyrules.make files included by the same +# Makefile don't overwrite each other's variables. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will +# evaluate all C-preprocessor directives found in the sources and include +# files. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro +# names in the source code. If set to NO (the default) only conditional +# compilation will be performed. Macro expansion can be done in a controlled +# way by setting EXPAND_ONLY_PREDEF to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES +# then the macro expansion is limited to the macros specified with the +# PREDEFINED and EXPAND_AS_DEFINED tags. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files +# in the INCLUDE_PATH (see below) will be search if a #include is found. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by +# the preprocessor. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will +# be used. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that +# are defined before the preprocessor is started (similar to the -D option of +# gcc). The argument of the tag is a list of macros of the form: name +# or name=definition (no spaces). If the definition and the = are +# omitted =1 is assumed. To prevent a macro definition from being +# undefined via #undef or recursively expanded use the := operator +# instead of the = operator. + +PREDEFINED = + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then +# this tag can be used to specify a list of macro names that should be expanded. +# The macro definition that is found in the sources will be used. +# Use the PREDEFINED tag if you want to use a different macro definition. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then +# doxygen's preprocessor will remove all function-like macros that are alone +# on a line, have an all uppercase name, and do not end with a semicolon. Such +# function macros are typically used for boiler-plate code, and will confuse +# the parser if not removed. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES option can be used to specify one or more tagfiles. +# Optionally an initial location of the external documentation +# can be added for each tagfile. The format of a tag file without +# this location is as follows: +# +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where "loc1" and "loc2" can be relative or absolute paths or +# URLs. If a location is present for each tag, the installdox tool +# does not have to be run to correct the links. +# Note that each tag file must have a unique name +# (where the name does NOT include the path) +# If a tag file is not located in the directory in which doxygen +# is run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create +# a tag file that is based on the input files it reads. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external classes will be listed +# in the class index. If set to NO only the inherited external classes +# will be listed. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed +# in the modules index. If set to NO, only the current project's groups will +# be listed. + +EXTERNAL_GROUPS = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of `which perl'). + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will +# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base +# or super classes. Setting the tag to NO turns the diagrams off. Note that +# this option is superseded by the HAVE_DOT option below. This is only a +# fallback. It is recommended to install and use dot, since it yields more +# powerful graphs. + +CLASS_DIAGRAMS = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see +# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# If set to YES, the inheritance and collaboration graphs will hide +# inheritance and usage relations if the target is undocumented +# or is not a class. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz, a graph visualization +# toolkit from AT&T and Lucent Bell Labs. The other options in this section +# have no effect if this option is set to NO (the default) + +HAVE_DOT = NO + +# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is +# allowed to run in parallel. When set to 0 (the default) doxygen will +# base this on the number of processors available in the system. You can set it +# explicitly to a value larger than 0 to get control over the balance +# between CPU load and processing speed. + +DOT_NUM_THREADS = 0 + +# By default doxygen will write a font called FreeSans.ttf to the output +# directory and reference it in all dot files that doxygen generates. This +# font does not include all possible unicode characters however, so when you need +# these (or just want a differently looking font) you can specify the font name +# using DOT_FONTNAME. You need need to make sure dot is able to find the font, +# which can be done by putting it in a standard location or by setting the +# DOTFONTPATH environment variable or by setting DOT_FONTPATH to the directory +# containing the font. + +DOT_FONTNAME = FreeSans.ttf + +# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. +# The default size is 10pt. + +DOT_FONTSIZE = 10 + +# By default doxygen will tell dot to use the output directory to look for the +# FreeSans.ttf font (which doxygen will put there itself). If you specify a +# different font using DOT_FONTNAME you can set the path where dot +# can find it using this tag. + +DOT_FONTPATH = + +# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect inheritance relations. Setting this tag to YES will force the +# the CLASS_DIAGRAMS tag to NO. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect implementation dependencies (inheritance, containment, and +# class references variables) of the class with other documented classes. + +COLLABORATION_GRAPH = YES + +# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for groups, showing the direct groups dependencies + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. + +UML_LOOK = NO + +# If set to YES, the inheritance and collaboration graphs will show the +# relations between templates and their instances. + +TEMPLATE_RELATIONS = NO + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT +# tags are set to YES then doxygen will generate a graph for each documented +# file showing the direct and indirect include dependencies of the file with +# other documented files. + +INCLUDE_GRAPH = YES + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and +# HAVE_DOT tags are set to YES then doxygen will generate a graph for each +# documented header file showing the documented files that directly or +# indirectly include this file. + +INCLUDED_BY_GRAPH = YES + +# If the CALL_GRAPH and HAVE_DOT options are set to YES then +# doxygen will generate a call dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable call graphs +# for selected functions only using the \callgraph command. + +CALL_GRAPH = NO + +# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then +# doxygen will generate a caller dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable caller +# graphs for selected functions only using the \callergraph command. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen +# will graphical hierarchy of all classes instead of a textual one. + +GRAPHICAL_HIERARCHY = YES + +# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES +# then doxygen will show the dependencies a directory has on other directories +# in a graphical way. The dependency relations are determined by the #include +# relations between the files in the directories. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. Possible values are png, jpg, or gif +# If left blank png will be used. + +DOT_IMAGE_FORMAT = png + +# The tag DOT_PATH can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the +# \dotfile command). + +DOTFILE_DIRS = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of +# nodes that will be shown in the graph. If the number of nodes in a graph +# becomes larger than this value, doxygen will truncate the graph, which is +# visualized by representing a node as a red box. Note that doxygen if the +# number of direct children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note +# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the +# graphs generated by dot. A depth value of 3 means that only nodes reachable +# from the root by following a path via at most 3 edges will be shown. Nodes +# that lay further from the root node will be omitted. Note that setting this +# option to 1 or 2 may greatly reduce the computation time needed for large +# code bases. Also note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is disabled by default, because dot on Windows does not +# seem to support this out of the box. Warning: Depending on the platform used, +# enabling this option may lead to badly anti-aliased labels on the edges of +# a graph (i.e. they become hard to read). + +DOT_TRANSPARENT = NO + +# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) +# support this, this feature is disabled by default. + +DOT_MULTI_TARGETS = YES + +# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will +# generate a legend page explaining the meaning of the various boxes and +# arrows in the dot generated graphs. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will +# remove the intermediate dot files that are used to generate +# the various graphs. + +DOT_CLEANUP = YES diff --git a/Engine/lib/sdl/src/hidapi/hidapi/hidapi.h b/Engine/lib/sdl/src/hidapi/hidapi/hidapi.h new file mode 100644 index 000000000..15d632365 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/hidapi/hidapi.h @@ -0,0 +1,398 @@ +/******************************************************* + HIDAPI - Multi-Platform library for + communication with HID devices. + + Alan Ott + Signal 11 Software + + 8/22/2009 + + Copyright 2009, All Rights Reserved. + + At the discretion of the user of this library, + this software may be licensed under the terms of the + GNU General Public License v3, a BSD-Style license, or the + original HIDAPI license as outlined in the LICENSE.txt, + LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt + files located at the root of the source distribution. + These files may also be found in the public source + code repository located at: + http://github.com/signal11/hidapi . +********************************************************/ + +/** @file + * @defgroup API hidapi API + */ + +#ifndef HIDAPI_H__ +#define HIDAPI_H__ + +#include + +#if defined(_WIN32) && !defined(NAMESPACE) && (0) /* SDL: don't export hidapi syms */ + #define HID_API_EXPORT __declspec(dllexport) + #define HID_API_CALL +#else + #define HID_API_EXPORT /**< API export macro */ + #define HID_API_CALL /**< API call macro */ +#endif + +#define HID_API_EXPORT_CALL HID_API_EXPORT HID_API_CALL /**< API export and call macro*/ + +#if defined(__cplusplus) && !defined(NAMESPACE) +extern "C" { +#endif +#ifdef NAMESPACE +namespace NAMESPACE { +#endif + + struct hid_device_; + typedef struct hid_device_ hid_device; /**< opaque hidapi structure */ + + /** hidapi info structure */ + struct hid_device_info { + /** Platform-specific device path */ + char *path; + /** Device Vendor ID */ + unsigned short vendor_id; + /** Device Product ID */ + unsigned short product_id; + /** Serial Number */ + wchar_t *serial_number; + /** Device Release Number in binary-coded decimal, + also known as Device Version Number */ + unsigned short release_number; + /** Manufacturer String */ + wchar_t *manufacturer_string; + /** Product string */ + wchar_t *product_string; + /** Usage Page for this Device/Interface + (Windows/Mac only). */ + unsigned short usage_page; + /** Usage for this Device/Interface + (Windows/Mac only).*/ + unsigned short usage; + /** The USB interface which this logical device + represents. Valid on both Linux implementations + in all cases, and valid on the Windows implementation + only if the device contains more than one interface. */ + int interface_number; + + /** Pointer to the next device */ + struct hid_device_info *next; + }; + + + /** @brief Initialize the HIDAPI library. + + This function initializes the HIDAPI library. Calling it is not + strictly necessary, as it will be called automatically by + hid_enumerate() and any of the hid_open_*() functions if it is + needed. This function should be called at the beginning of + execution however, if there is a chance of HIDAPI handles + being opened by different threads simultaneously. + + @ingroup API + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_init(void); + + /** @brief Finalize the HIDAPI library. + + This function frees all of the static data associated with + HIDAPI. It should be called at the end of execution to avoid + memory leaks. + + @ingroup API + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_exit(void); + + /** @brief Enumerate the HID Devices. + + This function returns a linked list of all the HID devices + attached to the system which match vendor_id and product_id. + If @p vendor_id is set to 0 then any vendor matches. + If @p product_id is set to 0 then any product matches. + If @p vendor_id and @p product_id are both set to 0, then + all HID devices will be returned. + + @ingroup API + @param vendor_id The Vendor ID (VID) of the types of device + to open. + @param product_id The Product ID (PID) of the types of + device to open. + + @returns + This function returns a pointer to a linked list of type + struct #hid_device, containing information about the HID devices + attached to the system, or NULL in the case of failure. Free + this linked list by calling hid_free_enumeration(). + */ + struct hid_device_info HID_API_EXPORT * HID_API_CALL hid_enumerate(unsigned short vendor_id, unsigned short product_id); + + /** @brief Free an enumeration Linked List + + This function frees a linked list created by hid_enumerate(). + + @ingroup API + @param devs Pointer to a list of struct_device returned from + hid_enumerate(). + */ + void HID_API_EXPORT HID_API_CALL hid_free_enumeration(struct hid_device_info *devs); + + /** @brief Open a HID device using a Vendor ID (VID), Product ID + (PID) and optionally a serial number. + + If @p serial_number is NULL, the first device with the + specified VID and PID is opened. + + @ingroup API + @param vendor_id The Vendor ID (VID) of the device to open. + @param product_id The Product ID (PID) of the device to open. + @param serial_number The Serial Number of the device to open + (Optionally NULL). + + @returns + This function returns a pointer to a #hid_device object on + success or NULL on failure. + */ + HID_API_EXPORT hid_device * HID_API_CALL hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number); + + /** @brief Open a HID device by its path name. + + The path name be determined by calling hid_enumerate(), or a + platform-specific path name can be used (eg: /dev/hidraw0 on + Linux). + + @ingroup API + @param path The path name of the device to open + + @returns + This function returns a pointer to a #hid_device object on + success or NULL on failure. + */ + HID_API_EXPORT hid_device * HID_API_CALL hid_open_path(const char *path, int bExclusive /* = false */); + + /** @brief Write an Output report to a HID device. + + The first byte of @p data[] must contain the Report ID. For + devices which only support a single report, this must be set + to 0x0. The remaining bytes contain the report data. Since + the Report ID is mandatory, calls to hid_write() will always + contain one more byte than the report contains. For example, + if a hid report is 16 bytes long, 17 bytes must be passed to + hid_write(), the Report ID (or 0x0, for devices with a + single report), followed by the report data (16 bytes). In + this example, the length passed in would be 17. + + hid_write() will send the data on the first OUT endpoint, if + one exists. If it does not, it will send the data through + the Control Endpoint (Endpoint 0). + + @ingroup API + @param device A device handle returned from hid_open(). + @param data The data to send, including the report number as + the first byte. + @param length The length in bytes of the data to send. + + @returns + This function returns the actual number of bytes written and + -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_write(hid_device *device, const unsigned char *data, size_t length); + + /** @brief Read an Input report from a HID device with timeout. + + Input reports are returned + to the host through the INTERRUPT IN endpoint. The first byte will + contain the Report number if the device uses numbered reports. + + @ingroup API + @param device A device handle returned from hid_open(). + @param data A buffer to put the read data into. + @param length The number of bytes to read. For devices with + multiple reports, make sure to read an extra byte for + the report number. + @param milliseconds timeout in milliseconds or -1 for blocking wait. + + @returns + This function returns the actual number of bytes read and + -1 on error. If no packet was available to be read within + the timeout period, this function returns 0. + */ + int HID_API_EXPORT HID_API_CALL hid_read_timeout(hid_device *device, unsigned char *data, size_t length, int milliseconds); + + /** @brief Read an Input report from a HID device. + + Input reports are returned + to the host through the INTERRUPT IN endpoint. The first byte will + contain the Report number if the device uses numbered reports. + + @ingroup API + @param device A device handle returned from hid_open(). + @param data A buffer to put the read data into. + @param length The number of bytes to read. For devices with + multiple reports, make sure to read an extra byte for + the report number. + + @returns + This function returns the actual number of bytes read and + -1 on error. If no packet was available to be read and + the handle is in non-blocking mode, this function returns 0. + */ + int HID_API_EXPORT HID_API_CALL hid_read(hid_device *device, unsigned char *data, size_t length); + + /** @brief Set the device handle to be non-blocking. + + In non-blocking mode calls to hid_read() will return + immediately with a value of 0 if there is no data to be + read. In blocking mode, hid_read() will wait (block) until + there is data to read before returning. + + Nonblocking can be turned on and off at any time. + + @ingroup API + @param device A device handle returned from hid_open(). + @param nonblock enable or not the nonblocking reads + - 1 to enable nonblocking + - 0 to disable nonblocking. + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_set_nonblocking(hid_device *device, int nonblock); + + /** @brief Send a Feature report to the device. + + Feature reports are sent over the Control endpoint as a + Set_Report transfer. The first byte of @p data[] must + contain the Report ID. For devices which only support a + single report, this must be set to 0x0. The remaining bytes + contain the report data. Since the Report ID is mandatory, + calls to hid_send_feature_report() will always contain one + more byte than the report contains. For example, if a hid + report is 16 bytes long, 17 bytes must be passed to + hid_send_feature_report(): the Report ID (or 0x0, for + devices which do not use numbered reports), followed by the + report data (16 bytes). In this example, the length passed + in would be 17. + + @ingroup API + @param device A device handle returned from hid_open(). + @param data The data to send, including the report number as + the first byte. + @param length The length in bytes of the data to send, including + the report number. + + @returns + This function returns the actual number of bytes written and + -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_send_feature_report(hid_device *device, const unsigned char *data, size_t length); + + /** @brief Get a feature report from a HID device. + + Set the first byte of @p data[] to the Report ID of the + report to be read. Make sure to allow space for this + extra byte in @p data[]. Upon return, the first byte will + still contain the Report ID, and the report data will + start in data[1]. + + @ingroup API + @param device A device handle returned from hid_open(). + @param data A buffer to put the read data into, including + the Report ID. Set the first byte of @p data[] to the + Report ID of the report to be read, or set it to zero + if your device does not use numbered reports. + @param length The number of bytes to read, including an + extra byte for the report ID. The buffer can be longer + than the actual report. + + @returns + This function returns the number of bytes read plus + one for the report ID (which is still in the first + byte), or -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_get_feature_report(hid_device *device, unsigned char *data, size_t length); + + /** @brief Close a HID device. + + @ingroup API + @param device A device handle returned from hid_open(). + */ + void HID_API_EXPORT HID_API_CALL hid_close(hid_device *device); + + /** @brief Get The Manufacturer String from a HID device. + + @ingroup API + @param device A device handle returned from hid_open(). + @param string A wide string buffer to put the data into. + @param maxlen The length of the buffer in multiples of wchar_t. + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *device, wchar_t *string, size_t maxlen); + + /** @brief Get The Product String from a HID device. + + @ingroup API + @param device A device handle returned from hid_open(). + @param string A wide string buffer to put the data into. + @param maxlen The length of the buffer in multiples of wchar_t. + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT_CALL hid_get_product_string(hid_device *device, wchar_t *string, size_t maxlen); + + /** @brief Get The Serial Number String from a HID device. + + @ingroup API + @param device A device handle returned from hid_open(). + @param string A wide string buffer to put the data into. + @param maxlen The length of the buffer in multiples of wchar_t. + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *device, wchar_t *string, size_t maxlen); + + /** @brief Get a string from a HID device, based on its string index. + + @ingroup API + @param device A device handle returned from hid_open(). + @param string_index The index of the string to get. + @param string A wide string buffer to put the data into. + @param maxlen The length of the buffer in multiples of wchar_t. + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *device, int string_index, wchar_t *string, size_t maxlen); + + /** @brief Get a string describing the last error which occurred. + + @ingroup API + @param device A device handle returned from hid_open(). + + @returns + This function returns a string containing the last error + which occurred or NULL if none has occurred. + */ + HID_API_EXPORT const wchar_t* HID_API_CALL hid_error(hid_device *device); + +#if defined(__cplusplus) && !defined(NAMESPACE) +} +#endif +#ifdef NAMESPACE +} +#endif + +#endif + diff --git a/Engine/lib/sdl/src/hidapi/hidtest/Makefile.am b/Engine/lib/sdl/src/hidapi/hidtest/Makefile.am new file mode 100644 index 000000000..d2786445a --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/hidtest/Makefile.am @@ -0,0 +1,20 @@ +AM_CPPFLAGS = -I$(top_srcdir)/hidapi/ + +## Linux +if OS_LINUX +noinst_PROGRAMS = hidtest-libusb hidtest-hidraw + +hidtest_hidraw_SOURCES = hidtest.cpp +hidtest_hidraw_LDADD = $(top_builddir)/linux/libhidapi-hidraw.la + +hidtest_libusb_SOURCES = hidtest.cpp +hidtest_libusb_LDADD = $(top_builddir)/libusb/libhidapi-libusb.la +else + +# Other OS's +noinst_PROGRAMS = hidtest + +hidtest_SOURCES = hidtest.cpp +hidtest_LDADD = $(top_builddir)/$(backend)/libhidapi.la + +endif diff --git a/Engine/lib/sdl/src/hidapi/hidtest/hidtest.cpp b/Engine/lib/sdl/src/hidapi/hidtest/hidtest.cpp new file mode 100644 index 000000000..94f0a5c20 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/hidtest/hidtest.cpp @@ -0,0 +1,194 @@ +/******************************************************* + Windows HID simplification + + Alan Ott + Signal 11 Software + + 8/22/2009 + + Copyright 2009 + + This contents of this file may be used by anyone + for any reason without any conditions and may be + used as a starting point for your own applications + which use HIDAPI. +********************************************************/ + +#include +#include +#include +#include +#include "hidapi.h" + +// Headers needed for sleeping. +#ifdef _WIN32 + #include +#else + #include +#endif + +int main(int argc, char* argv[]) +{ + int res; + unsigned char buf[256]; + #define MAX_STR 255 + wchar_t wstr[MAX_STR]; + hid_device *handle; + int i; + +#ifdef WIN32 + UNREFERENCED_PARAMETER(argc); + UNREFERENCED_PARAMETER(argv); +#endif + + struct hid_device_info *devs, *cur_dev; + + if (hid_init()) + return -1; + + devs = hid_enumerate(0x0, 0x0); + cur_dev = devs; + while (cur_dev) { + printf("Device Found\n type: %04hx %04hx\n path: %s\n serial_number: %ls", cur_dev->vendor_id, cur_dev->product_id, cur_dev->path, cur_dev->serial_number); + printf("\n"); + printf(" Manufacturer: %ls\n", cur_dev->manufacturer_string); + printf(" Product: %ls\n", cur_dev->product_string); + printf(" Release: %hx\n", cur_dev->release_number); + printf(" Interface: %d\n", cur_dev->interface_number); + printf("\n"); + cur_dev = cur_dev->next; + } + hid_free_enumeration(devs); + + // Set up the command buffer. + memset(buf,0x00,sizeof(buf)); + buf[0] = 0x01; + buf[1] = 0x81; + + + // Open the device using the VID, PID, + // and optionally the Serial number. + ////handle = hid_open(0x4d8, 0x3f, L"12345"); + handle = hid_open(0x4d8, 0x3f, NULL); + if (!handle) { + printf("unable to open device\n"); + return 1; + } + + // Read the Manufacturer String + wstr[0] = 0x0000; + res = hid_get_manufacturer_string(handle, wstr, MAX_STR); + if (res < 0) + printf("Unable to read manufacturer string\n"); + printf("Manufacturer String: %ls\n", wstr); + + // Read the Product String + wstr[0] = 0x0000; + res = hid_get_product_string(handle, wstr, MAX_STR); + if (res < 0) + printf("Unable to read product string\n"); + printf("Product String: %ls\n", wstr); + + // Read the Serial Number String + wstr[0] = 0x0000; + res = hid_get_serial_number_string(handle, wstr, MAX_STR); + if (res < 0) + printf("Unable to read serial number string\n"); + printf("Serial Number String: (%d) %ls", wstr[0], wstr); + printf("\n"); + + // Read Indexed String 1 + wstr[0] = 0x0000; + res = hid_get_indexed_string(handle, 1, wstr, MAX_STR); + if (res < 0) + printf("Unable to read indexed string 1\n"); + printf("Indexed String 1: %ls\n", wstr); + + // Set the hid_read() function to be non-blocking. + hid_set_nonblocking(handle, 1); + + // Try to read from the device. There shoud be no + // data here, but execution should not block. + res = hid_read(handle, buf, 17); + + // Send a Feature Report to the device + buf[0] = 0x2; + buf[1] = 0xa0; + buf[2] = 0x0a; + buf[3] = 0x00; + buf[4] = 0x00; + res = hid_send_feature_report(handle, buf, 17); + if (res < 0) { + printf("Unable to send a feature report.\n"); + } + + memset(buf,0,sizeof(buf)); + + // Read a Feature Report from the device + buf[0] = 0x2; + res = hid_get_feature_report(handle, buf, sizeof(buf)); + if (res < 0) { + printf("Unable to get a feature report.\n"); + printf("%ls", hid_error(handle)); + } + else { + // Print out the returned buffer. + printf("Feature Report\n "); + for (i = 0; i < res; i++) + printf("%02hhx ", buf[i]); + printf("\n"); + } + + memset(buf,0,sizeof(buf)); + + // Toggle LED (cmd 0x80). The first byte is the report number (0x1). + buf[0] = 0x1; + buf[1] = 0x80; + res = hid_write(handle, buf, 17); + if (res < 0) { + printf("Unable to write()\n"); + printf("Error: %ls\n", hid_error(handle)); + } + + + // Request state (cmd 0x81). The first byte is the report number (0x1). + buf[0] = 0x1; + buf[1] = 0x81; + hid_write(handle, buf, 17); + if (res < 0) + printf("Unable to write() (2)\n"); + + // Read requested state. hid_read() has been set to be + // non-blocking by the call to hid_set_nonblocking() above. + // This loop demonstrates the non-blocking nature of hid_read(). + res = 0; + while (res == 0) { + res = hid_read(handle, buf, sizeof(buf)); + if (res == 0) + printf("waiting...\n"); + if (res < 0) + printf("Unable to read()\n"); + #ifdef WIN32 + Sleep(500); + #else + usleep(500*1000); + #endif + } + + printf("Data read:\n "); + // Print out the returned buffer. + for (i = 0; i < res; i++) + printf("%02hhx ", buf[i]); + printf("\n"); + + hid_close(handle); + + /* Free static HIDAPI objects. */ + hid_exit(); + +#ifdef WIN32 + system("pause"); +#endif + + return 0; +} diff --git a/Engine/lib/sdl/src/hidapi/ios/Makefile-manual b/Engine/lib/sdl/src/hidapi/ios/Makefile-manual new file mode 100644 index 000000000..939a0777e --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/ios/Makefile-manual @@ -0,0 +1,32 @@ +########################################### +# Simple Makefile for HIDAPI test program +# +# Alan Ott +# Signal 11 Software +# 2010-07-03 +########################################### + +all: hidtest + +CC=gcc +CXX=g++ +COBJS=hid.o +CPPOBJS=../hidtest/hidtest.o +OBJS=$(COBJS) $(CPPOBJS) +CFLAGS+=-I../hidapi -Wall -g -c +LIBS=-framework CoreBluetooth -framework CoreFoundation + + +hidtest: $(OBJS) + g++ -Wall -g $^ $(LIBS) -o hidtest + +$(COBJS): %.o: %.c + $(CC) $(CFLAGS) $< -o $@ + +$(CPPOBJS): %.o: %.cpp + $(CXX) $(CFLAGS) $< -o $@ + +clean: + rm -f *.o hidtest $(CPPOBJS) + +.PHONY: clean diff --git a/Engine/lib/sdl/src/hidapi/ios/Makefile.am b/Engine/lib/sdl/src/hidapi/ios/Makefile.am new file mode 100644 index 000000000..1f8f2cec6 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/ios/Makefile.am @@ -0,0 +1,9 @@ +lib_LTLIBRARIES = libhidapi.la +libhidapi_la_SOURCES = hid.m +libhidapi_la_LDFLAGS = $(LTLDFLAGS) +AM_CPPFLAGS = -I$(top_srcdir)/hidapi/ + +hdrdir = $(includedir)/hidapi +hdr_HEADERS = $(top_srcdir)/hidapi/hidapi.h + +EXTRA_DIST = Makefile-manual diff --git a/Engine/lib/sdl/src/hidapi/ios/hid.m b/Engine/lib/sdl/src/hidapi/ios/hid.m new file mode 100644 index 000000000..a0ca7cd02 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/ios/hid.m @@ -0,0 +1,914 @@ +//======== Copyright (c) 2017 Valve Corporation, All rights reserved. ========= +// +// Purpose: HID device abstraction temporary stub +// +//============================================================================= +#include "../../SDL_internal.h" + +#ifdef SDL_JOYSTICK_HIDAPI + +#include +#include +#import +#import +#include +#include +#include +#include "../hidapi/hidapi.h" + +#define VALVE_USB_VID 0x28DE +#define D0G_BLE2_PID 0x1106 + +typedef uint32_t uint32; +typedef uint64_t uint64; + +// enables detailed NSLog logging of feature reports +#define FEATURE_REPORT_LOGGING 0 + +#define REPORT_SEGMENT_DATA_FLAG 0x80 +#define REPORT_SEGMENT_LAST_FLAG 0x40 + +#define VALVE_SERVICE @"100F6C32-1735-4313-B402-38567131E5F3" + +// (READ/NOTIFICATIONS) +#define VALVE_INPUT_CHAR @"100F6C33-1735-4313-B402-38567131E5F3" + +//  (READ/WRITE) +#define VALVE_REPORT_CHAR @"100F6C34-1735-4313-B402-38567131E5F3" + +// TODO: create CBUUID's in __attribute__((constructor)) rather than doing [CBUUID UUIDWithString:...] everywhere + +#pragma pack(push,1) + +typedef struct +{ + uint8_t segmentHeader; + uint8_t featureReportMessageID; + uint8_t length; + uint8_t settingIdentifier; + union { + uint16_t usPayload; + uint32_t uPayload; + uint64_t ulPayload; + uint8_t ucPayload[15]; + }; +} bluetoothSegment; + +typedef struct { + uint8_t id; + union { + bluetoothSegment segment; + struct { + uint8_t segmentHeader; + uint8_t featureReportMessageID; + uint8_t length; + uint8_t settingIdentifier; + union { + uint16_t usPayload; + uint32_t uPayload; + uint64_t ulPayload; + uint8_t ucPayload[15]; + }; + }; + }; +} hidFeatureReport; + +#pragma pack(pop) + +size_t GetBluetoothSegmentSize(bluetoothSegment *segment) +{ + return segment->length + 3; +} + +#define RingBuffer_cbElem 19 +#define RingBuffer_nElem 4096 + +typedef struct { + int _first, _last; + uint8_t _data[ ( RingBuffer_nElem * RingBuffer_cbElem ) ]; + pthread_mutex_t accessLock; +} RingBuffer; + +static void RingBuffer_init( RingBuffer *this ) +{ + this->_first = -1; + this->_last = 0; + pthread_mutex_init( &this->accessLock, 0 ); +} + +static bool RingBuffer_write( RingBuffer *this, const uint8_t *src ) +{ + pthread_mutex_lock( &this->accessLock ); + memcpy( &this->_data[ this->_last ], src, RingBuffer_cbElem ); + if ( this->_first == -1 ) + { + this->_first = this->_last; + } + this->_last = ( this->_last + RingBuffer_cbElem ) % (RingBuffer_nElem * RingBuffer_cbElem); + if ( this->_last == this->_first ) + { + this->_first = ( this->_first + RingBuffer_cbElem ) % (RingBuffer_nElem * RingBuffer_cbElem); + pthread_mutex_unlock( &this->accessLock ); + return false; + } + pthread_mutex_unlock( &this->accessLock ); + return true; +} + +static bool RingBuffer_read( RingBuffer *this, uint8_t *dst ) +{ + pthread_mutex_lock( &this->accessLock ); + if ( this->_first == -1 ) + { + pthread_mutex_unlock( &this->accessLock ); + return false; + } + memcpy( dst, &this->_data[ this->_first ], RingBuffer_cbElem ); + this->_first = ( this->_first + RingBuffer_cbElem ) % (RingBuffer_nElem * RingBuffer_cbElem); + if ( this->_first == this->_last ) + { + this->_first = -1; + } + pthread_mutex_unlock( &this->accessLock ); + return true; +} + + +#pragma mark HIDBLEDevice Definition + +typedef enum +{ + BLEDeviceWaitState_None, + BLEDeviceWaitState_Waiting, + BLEDeviceWaitState_Complete, + BLEDeviceWaitState_Error +} BLEDeviceWaitState; + +@interface HIDBLEDevice : NSObject +{ + RingBuffer _inputReports; + uint8_t _featureReport[20]; + BLEDeviceWaitState _waitStateForReadFeatureReport; + BLEDeviceWaitState _waitStateForWriteFeatureReport; +} + +@property (nonatomic, readwrite) bool connected; +@property (nonatomic, readwrite) bool ready; + +@property (nonatomic, strong) CBPeripheral *bleSteamController; +@property (nonatomic, strong) CBCharacteristic *bleCharacteristicInput; +@property (nonatomic, strong) CBCharacteristic *bleCharacteristicReport; + +- (id)initWithPeripheral:(CBPeripheral *)peripheral; + +@end + + +@interface HIDBLEManager : NSObject + +@property (nonatomic) int nPendingScans; +@property (nonatomic) int nPendingPairs; +@property (nonatomic, strong) CBCentralManager *centralManager; +@property (nonatomic, strong) NSMapTable *deviceMap; +@property (nonatomic, retain) dispatch_queue_t bleSerialQueue; + ++ (instancetype)sharedInstance; +- (void)startScan:(int)duration; +- (void)stopScan; +- (int)updateConnectedSteamControllers:(BOOL) bForce; +- (void)appWillResignActiveNotification:(NSNotification *)note; +- (void)appDidBecomeActiveNotification:(NSNotification *)note; + +@end + + +// singleton class - access using HIDBLEManager.sharedInstance +@implementation HIDBLEManager + ++ (instancetype)sharedInstance +{ + static HIDBLEManager *sharedInstance = nil; + static dispatch_once_t onceToken; + dispatch_once(&onceToken, ^{ + sharedInstance = [HIDBLEManager new]; + sharedInstance.nPendingScans = 0; + sharedInstance.nPendingPairs = 0; + + [[NSNotificationCenter defaultCenter] addObserver:sharedInstance selector:@selector(appWillResignActiveNotification:) name: UIApplicationWillResignActiveNotification object:nil]; + [[NSNotificationCenter defaultCenter] addObserver:sharedInstance selector:@selector(appDidBecomeActiveNotification:) name:UIApplicationDidBecomeActiveNotification object:nil]; + + // receive reports on a high-priority serial-queue. optionally put writes on the serial queue to avoid logical + // race conditions talking to the controller from multiple threads, although BLE fragmentation/assembly means + // that we can still screw this up. + // most importantly we need to consume reports at a high priority to avoid the OS thinking we aren't really + // listening to the BLE device, as iOS on slower devices may stop delivery of packets to the app WITHOUT ACTUALLY + // DISCONNECTING FROM THE DEVICE if we don't react quickly enough to their delivery. + // see also the error-handling states in the peripheral delegate to re-open the device if it gets closed + sharedInstance.bleSerialQueue = dispatch_queue_create( "com.valvesoftware.steamcontroller.ble", DISPATCH_QUEUE_SERIAL ); + dispatch_set_target_queue( sharedInstance.bleSerialQueue, dispatch_get_global_queue( DISPATCH_QUEUE_PRIORITY_HIGH, 0 ) ); + + // creating a CBCentralManager will always trigger a future centralManagerDidUpdateState: + // where any scanning gets started or connecting to existing peripherals happens, it's never already in a + // powered-on state for a newly launched application. + sharedInstance.centralManager = [[CBCentralManager alloc] initWithDelegate:sharedInstance queue:sharedInstance.bleSerialQueue]; + sharedInstance.deviceMap = [[NSMapTable alloc] initWithKeyOptions:NSMapTableWeakMemory valueOptions:NSMapTableStrongMemory capacity:4]; + }); + return sharedInstance; +} + +// called for NSNotification UIApplicationWillResignActiveNotification +- (void)appWillResignActiveNotification:(NSNotification *)note +{ + // we'll get resign-active notification if pairing is happening. + if ( self.nPendingPairs > 0 ) + return; + + for ( CBPeripheral *peripheral in self.deviceMap ) + { + HIDBLEDevice *steamController = [self.deviceMap objectForKey:peripheral]; + if ( steamController ) + { + steamController.connected = NO; + steamController.ready = NO; + [self.centralManager cancelPeripheralConnection:peripheral]; + } + } + [self.deviceMap removeAllObjects]; +} + +// called for NSNotification UIApplicationDidBecomeActiveNotification +// whenever the application comes back from being inactive, trigger a 20s pairing scan and reconnect +// any devices that may have paired while we were inactive. +- (void)appDidBecomeActiveNotification:(NSNotification *)note +{ + [self updateConnectedSteamControllers:true]; + [self startScan:20]; +} + +- (int)updateConnectedSteamControllers:(BOOL) bForce +{ + static uint64_t s_unLastUpdateTick = 0; + static mach_timebase_info_data_t s_timebase_info; + + if (s_timebase_info.denom == 0) + { + mach_timebase_info( &s_timebase_info ); + } + + uint64_t ticksNow = mach_approximate_time(); + if ( !bForce && ( ( (ticksNow - s_unLastUpdateTick) * s_timebase_info.numer ) / s_timebase_info.denom ) < (5ull * NSEC_PER_SEC) ) + return (int)self.deviceMap.count; + + // we can see previously connected BLE peripherals but can't connect until the CBCentralManager + // is fully powered up - only do work when we are in that state + if ( self.centralManager.state != CBManagerStatePoweredOn ) + return (int)self.deviceMap.count; + + // only update our last-check-time if we actually did work, otherwise there can be a long delay during initial power-up + s_unLastUpdateTick = mach_approximate_time(); + + // if a pair is in-flight, the central manager may still give it back via retrieveConnected... and + // cause the SDL layer to attempt to initialize it while some of its endpoints haven't yet been established + if ( self.nPendingPairs > 0 ) + return (int)self.deviceMap.count; + + NSArray *peripherals = [self.centralManager retrieveConnectedPeripheralsWithServices: @[ [CBUUID UUIDWithString:@"180A"]]]; + for ( CBPeripheral *peripheral in peripherals ) + { + // we already know this peripheral + if ( [self.deviceMap objectForKey: peripheral] != nil ) + continue; + + NSLog( @"connected peripheral: %@", peripheral ); + if ( [peripheral.name isEqualToString:@"SteamController"] ) + { + HIDBLEDevice *steamController = [[HIDBLEDevice alloc] initWithPeripheral:peripheral]; + [self.deviceMap setObject:steamController forKey:peripheral]; + [self.centralManager connectPeripheral:peripheral options:nil]; + } + } + + return (int)self.deviceMap.count; +} + +// manual API for folks to start & stop scanning +- (void)startScan:(int)duration +{ + NSLog( @"BLE: requesting scan for %d seconds", duration ); + @synchronized (self) + { + if ( _nPendingScans++ == 0 ) + { + [self.centralManager scanForPeripheralsWithServices:nil options:nil]; + } + } + + if ( duration != 0 ) + { + dispatch_after( dispatch_time( DISPATCH_TIME_NOW, (int64_t)(duration * NSEC_PER_SEC)), dispatch_get_main_queue(), ^{ + [self stopScan]; + }); + } +} + +- (void)stopScan +{ + NSLog( @"BLE: stopping scan" ); + @synchronized (self) + { + if ( --_nPendingScans <= 0 ) + { + _nPendingScans = 0; + [self.centralManager stopScan]; + } + } +} + + +#pragma mark CBCentralManagerDelegate Implementation + +// called whenever the BLE hardware state changes. +- (void)centralManagerDidUpdateState:(CBCentralManager *)central +{ + switch ( central.state ) + { + case CBCentralManagerStatePoweredOn: + { + NSLog( @"CoreBluetooth BLE hardware is powered on and ready" ); + + // at startup, if we have no already attached peripherals, do a 20s scan for new unpaired devices, + // otherwise callers should occaisionally do additional scans. we don't want to continuously be + // scanning because it drains battery, causes other nearby people to have a hard time pairing their + // Steam Controllers, and may also trigger firmware weirdness when a device attempts to start + // the pairing sequence multiple times concurrently + if ( [self updateConnectedSteamControllers:false] == 0 ) + { + // TODO: we could limit our scan to only peripherals supporting the SteamController service, but + // that service doesn't currently fit in the base advertising packet, we'd need to put it into an + // extended scan packet. Useful optimization downstream, but not currently necessary + // NSArray *services = @[[CBUUID UUIDWithString:VALVE_SERVICE]]; + [self startScan:20]; + } + break; + } + + case CBCentralManagerStatePoweredOff: + NSLog( @"CoreBluetooth BLE hardware is powered off" ); + break; + + case CBCentralManagerStateUnauthorized: + NSLog( @"CoreBluetooth BLE state is unauthorized" ); + break; + + case CBCentralManagerStateUnknown: + NSLog( @"CoreBluetooth BLE state is unknown" ); + break; + + case CBCentralManagerStateUnsupported: + NSLog( @"CoreBluetooth BLE hardware is unsupported on this platform" ); + break; + + case CBCentralManagerStateResetting: + NSLog( @"CoreBluetooth BLE manager is resetting" ); + break; + } +} + +- (void)centralManager:(CBCentralManager *)central didConnectPeripheral:(CBPeripheral *)peripheral +{ + HIDBLEDevice *steamController = [_deviceMap objectForKey:peripheral]; + steamController.connected = YES; + self.nPendingPairs -= 1; +} + +- (void)centralManager:(CBCentralManager *)central didFailToConnectPeripheral:(CBPeripheral *)peripheral error:(NSError *)error +{ + NSLog( @"Failed to connect: %@", error ); + [_deviceMap removeObjectForKey:peripheral]; + self.nPendingPairs -= 1; +} + +- (void)centralManager:(CBCentralManager *)central didDiscoverPeripheral:(CBPeripheral *)peripheral advertisementData:(NSDictionary *)advertisementData RSSI:(NSNumber *)RSSI +{ + NSString *localName = [advertisementData objectForKey:CBAdvertisementDataLocalNameKey]; + NSString *log = [NSString stringWithFormat:@"Found '%@'", localName]; + + if ( [localName isEqualToString:@"SteamController"] ) + { + NSLog( @"%@ : %@ - %@", log, peripheral, advertisementData ); + self.nPendingPairs += 1; + HIDBLEDevice *steamController = [[HIDBLEDevice alloc] initWithPeripheral:peripheral]; + [self.deviceMap setObject:steamController forKey:peripheral]; + [self.centralManager connectPeripheral:peripheral options:nil]; + } +} + +- (void)centralManager:(CBCentralManager *)central didDisconnectPeripheral:(CBPeripheral *)peripheral error:(NSError *)error +{ + HIDBLEDevice *steamController = [self.deviceMap objectForKey:peripheral]; + if ( steamController ) + { + steamController.connected = NO; + steamController.ready = NO; + [self.deviceMap removeObjectForKey:peripheral]; + } +} + +@end + + +// Core Bluetooth devices calling back on event boundaries of their run-loops. so annoying. +static void process_pending_events() +{ + CFRunLoopRunResult res; + do + { + res = CFRunLoopRunInMode( kCFRunLoopDefaultMode, 0.001, FALSE ); + } + while( res != kCFRunLoopRunFinished && res != kCFRunLoopRunTimedOut ); +} + +@implementation HIDBLEDevice + +- (id)init +{ + if ( self = [super init] ) + { + RingBuffer_init( &_inputReports ); + self.bleSteamController = nil; + self.bleCharacteristicInput = nil; + self.bleCharacteristicReport = nil; + _connected = NO; + _ready = NO; + } + return self; +} + +- (id)initWithPeripheral:(CBPeripheral *)peripheral +{ + if ( self = [super init] ) + { + RingBuffer_init( &_inputReports ); + _connected = NO; + _ready = NO; + self.bleSteamController = peripheral; + if ( peripheral ) + { + peripheral.delegate = self; + } + self.bleCharacteristicInput = nil; + self.bleCharacteristicReport = nil; + } + return self; +} + +- (void)setConnected:(bool)connected +{ + _connected = connected; + if ( _connected ) + { + [_bleSteamController discoverServices:nil]; + } + else + { + NSLog( @"Disconnected" ); + } +} + +- (size_t)read_input_report:(uint8_t *)dst +{ + if ( RingBuffer_read( &_inputReports, dst+1 ) ) + { + *dst = 0x03; + return 20; + } + return 0; +} + +- (int)send_report:(const uint8_t *)data length:(size_t)length +{ + [_bleSteamController writeValue:[NSData dataWithBytes:data length:length] forCharacteristic:_bleCharacteristicReport type:CBCharacteristicWriteWithResponse]; + return (int)length; +} + +- (int)send_feature_report:(hidFeatureReport *)report +{ +#if FEATURE_REPORT_LOGGING + uint8_t *reportBytes = (uint8_t *)report; + + NSLog( @"HIDBLE:send_feature_report (%02zu/19) [%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x]", GetBluetoothSegmentSize( report->segment ), + reportBytes[1], reportBytes[2], reportBytes[3], reportBytes[4], reportBytes[5], reportBytes[6], + reportBytes[7], reportBytes[8], reportBytes[9], reportBytes[10], reportBytes[11], reportBytes[12], + reportBytes[13], reportBytes[14], reportBytes[15], reportBytes[16], reportBytes[17], reportBytes[18], + reportBytes[19] ); +#endif + + int sendSize = (int)GetBluetoothSegmentSize( &report->segment ); + if ( sendSize > 20 ) + sendSize = 20; + +#if 1 + // fire-and-forget - we are going to not wait for the response here because all Steam Controller BLE send_feature_report's are ignored, + // except errors. + [_bleSteamController writeValue:[NSData dataWithBytes:&report->segment length:sendSize] forCharacteristic:_bleCharacteristicReport type:CBCharacteristicWriteWithResponse]; + + // pretend we received a result anybody cares about + return 19; + +#else + // this is technically the correct send_feature_report logic if you want to make sure it gets through and is + // acknowledged or errors out + _waitStateForWriteFeatureReport = BLEDeviceWaitState_Waiting; + [_bleSteamController writeValue:[NSData dataWithBytes:&report->segment length:sendSize + ] forCharacteristic:_bleCharacteristicReport type:CBCharacteristicWriteWithResponse]; + + while ( _waitStateForWriteFeatureReport == BLEDeviceWaitState_Waiting ) + { + process_pending_events(); + } + + if ( _waitStateForWriteFeatureReport == BLEDeviceWaitState_Error ) + { + _waitStateForWriteFeatureReport = BLEDeviceWaitState_None; + return -1; + } + + _waitStateForWriteFeatureReport = BLEDeviceWaitState_None; + return 19; +#endif +} + +- (int)get_feature_report:(uint8_t)feature into:(uint8_t *)buffer +{ + _waitStateForReadFeatureReport = BLEDeviceWaitState_Waiting; + [_bleSteamController readValueForCharacteristic:_bleCharacteristicReport]; + + while ( _waitStateForReadFeatureReport == BLEDeviceWaitState_Waiting ) + process_pending_events(); + + if ( _waitStateForReadFeatureReport == BLEDeviceWaitState_Error ) + { + _waitStateForReadFeatureReport = BLEDeviceWaitState_None; + return -1; + } + + memcpy( buffer, _featureReport, sizeof(_featureReport) ); + + _waitStateForReadFeatureReport = BLEDeviceWaitState_None; + +#if FEATURE_REPORT_LOGGING + NSLog( @"HIDBLE:get_feature_report (19) [%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x]", + buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], + buffer[7], buffer[8], buffer[9], buffer[10], buffer[11], buffer[12], + buffer[13], buffer[14], buffer[15], buffer[16], buffer[17], buffer[18], + buffer[19] ); +#endif + + return 19; +} + +#pragma mark CBPeripheralDelegate Implementation + +- (void)peripheral:(CBPeripheral *)peripheral didDiscoverServices:(NSError *)error +{ + for (CBService *service in peripheral.services) + { + NSLog( @"Found Service: %@", service ); + if ( [service.UUID isEqual:[CBUUID UUIDWithString:VALVE_SERVICE]] ) + { + [peripheral discoverCharacteristics:nil forService:service]; + } + } +} + +- (void)peripheral:(CBPeripheral *)peripheral didDiscoverDescriptorsForCharacteristic:(CBCharacteristic *)characteristic error:(NSError *)error +{ + // nothing yet needed here, enable for logging + if ( /* DISABLES CODE */ (0) ) + { + for ( CBDescriptor *descriptor in characteristic.descriptors ) + { + NSLog( @" - Descriptor '%@'", descriptor ); + } + } +} + +- (void)peripheral:(CBPeripheral *)peripheral didDiscoverCharacteristicsForService:(CBService *)service error:(NSError *)error +{ + if ([service.UUID isEqual:[CBUUID UUIDWithString:VALVE_SERVICE]]) + { + for (CBCharacteristic *aChar in service.characteristics) + { + NSLog( @"Found Characteristic %@", aChar ); + + if ( [aChar.UUID isEqual:[CBUUID UUIDWithString:VALVE_INPUT_CHAR]] ) + { + self.bleCharacteristicInput = aChar; + } + else if ( [aChar.UUID isEqual:[CBUUID UUIDWithString:VALVE_REPORT_CHAR]] ) + { + self.bleCharacteristicReport = aChar; + [self.bleSteamController discoverDescriptorsForCharacteristic: aChar]; + } + } + } +} + +- (void)peripheral:(CBPeripheral *)peripheral didUpdateValueForCharacteristic:(CBCharacteristic *)characteristic error:(NSError *)error +{ + static uint64_t s_ticksLastOverflowReport = 0; + + // receiving an input report is the final indicator that the user accepted a pairing + // request and that we successfully established notification. CoreBluetooth has no + // notification of the pairing acknowledgement, which is a bad oversight. + if ( self.ready == NO ) + { + self.ready = YES; + HIDBLEManager.sharedInstance.nPendingPairs -= 1; + } + + if ( [characteristic.UUID isEqual:_bleCharacteristicInput.UUID] ) + { + NSData *data = [characteristic value]; + if ( data.length != 19 ) + { + NSLog( @"HIDBLE: incoming data is %lu bytes should be exactly 19", (unsigned long)data.length ); + } + if ( !RingBuffer_write( &_inputReports, (const uint8_t *)data.bytes ) ) + { + uint64_t ticksNow = mach_approximate_time(); + if ( ticksNow - s_ticksLastOverflowReport > (5ull * NSEC_PER_SEC / 10) ) + { + NSLog( @"HIDBLE: input report buffer overflow" ); + s_ticksLastOverflowReport = ticksNow; + } + } + } + else if ( [characteristic.UUID isEqual:_bleCharacteristicReport.UUID] ) + { + memset( _featureReport, 0, sizeof(_featureReport) ); + + if ( error != nil ) + { + NSLog( @"HIDBLE: get_feature_report error: %@", error ); + _waitStateForReadFeatureReport = BLEDeviceWaitState_Error; + } + else + { + NSData *data = [characteristic value]; + if ( data.length != 20 ) + { + NSLog( @"HIDBLE: incoming data is %lu bytes should be exactly 20", (unsigned long)data.length ); + } + memcpy( _featureReport, data.bytes, MIN( data.length, sizeof(_featureReport) ) ); + _waitStateForReadFeatureReport = BLEDeviceWaitState_Complete; + } + } +} + +- (void)peripheral:(CBPeripheral *)peripheral didWriteValueForCharacteristic:(CBCharacteristic *)characteristic error:(NSError *)error +{ + if ( [characteristic.UUID isEqual:[CBUUID UUIDWithString:VALVE_REPORT_CHAR]] ) + { + if ( error != nil ) + { + NSLog( @"HIDBLE: write_feature_report error: %@", error ); + _waitStateForWriteFeatureReport = BLEDeviceWaitState_Error; + } + else + { + _waitStateForWriteFeatureReport = BLEDeviceWaitState_Complete; + } + } +} + +- (void)peripheral:(CBPeripheral *)peripheral didUpdateNotificationStateForCharacteristic:(CBCharacteristic *)characteristic error:(NSError *)error +{ + NSLog( @"didUpdateNotifcationStateForCharacteristic %@ (%@)", characteristic, error ); +} + +@end + + +#pragma mark hid_api implementation + +struct hid_device_ { + void *device_handle; + int blocking; + hid_device *next; +}; + +int HID_API_EXPORT HID_API_CALL hid_init(void) +{ + return ( HIDBLEManager.sharedInstance == nil ) ? -1 : 0; +} + +int HID_API_EXPORT HID_API_CALL hid_exit(void) +{ + return 0; +} + +void HID_API_EXPORT HID_API_CALL hid_ble_scan( bool bStart ) +{ + HIDBLEManager *bleManager = HIDBLEManager.sharedInstance; + if ( bStart ) + { + [bleManager startScan:0]; + } + else + { + [bleManager stopScan]; + } +} + +hid_device * HID_API_EXPORT hid_open_path( const char *path, int bExclusive /* = false */ ) +{ + hid_device *result = NULL; + NSString *nssPath = [NSString stringWithUTF8String:path]; + HIDBLEManager *bleManager = HIDBLEManager.sharedInstance; + NSEnumerator *devices = [bleManager.deviceMap objectEnumerator]; + + for ( HIDBLEDevice *device in devices ) + { + // we have the device but it hasn't found its service or characteristics until it is connected + if ( !device.ready || !device.connected || !device.bleCharacteristicInput ) + continue; + + if ( [device.bleSteamController.identifier.UUIDString isEqualToString:nssPath] ) + { + result = (hid_device *)malloc( sizeof( hid_device ) ); + memset( result, 0, sizeof( hid_device ) ); + result->device_handle = (void*)CFBridgingRetain( device ); + result->blocking = NO; + // enable reporting input events on the characteristic + [device.bleSteamController setNotifyValue:YES forCharacteristic:device.bleCharacteristicInput]; + return result; + } + } + return result; +} + +void HID_API_EXPORT hid_free_enumeration(struct hid_device_info *devs) +{ + /* This function is identical to the Linux version. Platform independent. */ + struct hid_device_info *d = devs; + while (d) { + struct hid_device_info *next = d->next; + free(d->path); + free(d->serial_number); + free(d->manufacturer_string); + free(d->product_string); + free(d); + d = next; + } +} + +int HID_API_EXPORT hid_set_nonblocking(hid_device *dev, int nonblock) +{ + /* All Nonblocking operation is handled by the library. */ + dev->blocking = !nonblock; + + return 0; +} + +struct hid_device_info HID_API_EXPORT *hid_enumerate(unsigned short vendor_id, unsigned short product_id) +{ @autoreleasepool { + struct hid_device_info *root = NULL; + + if ( ( vendor_id == 0 && product_id == 0 ) || + ( vendor_id == VALVE_USB_VID && product_id == D0G_BLE2_PID ) ) + { + HIDBLEManager *bleManager = HIDBLEManager.sharedInstance; + [bleManager updateConnectedSteamControllers:false]; + NSEnumerator *devices = [bleManager.deviceMap objectEnumerator]; + for ( HIDBLEDevice *device in devices ) + { + // there are several brief windows in connecting to an already paired device and + // one long window waiting for users to confirm pairing where we don't want + // to consider a device ready - if we hand it back to SDL or another + // Steam Controller consumer, their additional SC setup work will fail + // in unusual/silent ways and we can actually corrupt the BLE stack for + // the entire system and kill the appletv remote's Menu button (!) + if ( device.bleSteamController.state != CBPeripheralStateConnected || + device.connected == NO || device.ready == NO ) + { + if ( device.ready == NO && device.bleCharacteristicInput != nil ) + { + // attempt to register for input reports. this call will silently fail + // until the pairing finalizes with user acceptance. oh, apple. + [device.bleSteamController setNotifyValue:YES forCharacteristic:device.bleCharacteristicInput]; + } + continue; + } + struct hid_device_info *device_info = (struct hid_device_info *)malloc( sizeof(struct hid_device_info) ); + memset( device_info, 0, sizeof(struct hid_device_info) ); + device_info->next = root; + root = device_info; + device_info->path = strdup( device.bleSteamController.identifier.UUIDString.UTF8String ); + device_info->vendor_id = VALVE_USB_VID; + device_info->product_id = D0G_BLE2_PID; + device_info->product_string = wcsdup( L"Steam Controller" ); + device_info->manufacturer_string = wcsdup( L"Valve Corporation" ); + } + } + return root; +}} + +int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + static wchar_t s_wszManufacturer[] = L"Valve Corporation"; + wcsncpy( string, s_wszManufacturer, sizeof(s_wszManufacturer)/sizeof(s_wszManufacturer[0]) ); + return 0; +} + +int HID_API_EXPORT_CALL hid_get_product_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + static wchar_t s_wszProduct[] = L"Steam Controller"; + wcsncpy( string, s_wszProduct, sizeof(s_wszProduct)/sizeof(s_wszProduct[0]) ); + return 0; +} + +int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + static wchar_t s_wszSerial[] = L"12345"; + wcsncpy( string, s_wszSerial, sizeof(s_wszSerial)/sizeof(s_wszSerial[0]) ); + return 0; +} + +int HID_API_EXPORT hid_write(hid_device *dev, const unsigned char *data, size_t length) +{ + HIDBLEDevice *device_handle = (__bridge HIDBLEDevice *)dev->device_handle; + + if ( !device_handle.connected ) + return -1; + + return [device_handle send_report:data length:length]; +} + +void HID_API_EXPORT hid_close(hid_device *dev) +{ + HIDBLEDevice *device_handle = CFBridgingRelease( dev->device_handle ); + + // disable reporting input events on the characteristic + if ( device_handle.connected ) { + [device_handle.bleSteamController setNotifyValue:NO forCharacteristic:device_handle.bleCharacteristicInput]; + } + + free( dev ); +} + +int HID_API_EXPORT hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length) +{ + HIDBLEDevice *device_handle = (__bridge HIDBLEDevice *)dev->device_handle; + + if ( !device_handle.connected ) + return -1; + + return [device_handle send_feature_report:(hidFeatureReport *)(void *)data]; +} + +int HID_API_EXPORT hid_get_feature_report(hid_device *dev, unsigned char *data, size_t length) +{ + HIDBLEDevice *device_handle = (__bridge HIDBLEDevice *)dev->device_handle; + + if ( !device_handle.connected ) + return -1; + + size_t written = [device_handle get_feature_report:data[0] into:data]; + + return written == length-1 ? (int)length : (int)written; +} + +int HID_API_EXPORT hid_read(hid_device *dev, unsigned char *data, size_t length) +{ + HIDBLEDevice *device_handle = (__bridge HIDBLEDevice *)dev->device_handle; + + if ( !device_handle.connected ) + return -1; + + return hid_read_timeout(dev, data, length, 0); +} + +int HID_API_EXPORT hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds) +{ + HIDBLEDevice *device_handle = (__bridge HIDBLEDevice *)dev->device_handle; + + if ( !device_handle.connected ) + return -1; + + if ( milliseconds != 0 ) + { + NSLog( @"hid_read_timeout with non-zero wait" ); + } + int result = (int)[device_handle read_input_report:data]; +#if FEATURE_REPORT_LOGGING + NSLog( @"HIDBLE:hid_read_timeout (%d) [%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x]", result, + data[1], data[2], data[3], data[4], data[5], data[6], + data[7], data[8], data[9], data[10], data[11], data[12], + data[13], data[14], data[15], data[16], data[17], data[18], + data[19] ); +#endif + return result; +} + +#endif /* SDL_JOYSTICK_HIDAPI */ diff --git a/Engine/lib/sdl/src/hidapi/libusb/Makefile-manual b/Engine/lib/sdl/src/hidapi/libusb/Makefile-manual new file mode 100644 index 000000000..c0fe86800 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/libusb/Makefile-manual @@ -0,0 +1,18 @@ + + +OS=$(shell uname) + +ifeq ($(OS), Linux) + FILE=Makefile.linux +endif + +ifeq ($(OS), FreeBSD) + FILE=Makefile.freebsd +endif + +ifeq ($(FILE), ) +all: + $(error Your platform ${OS} is not supported by hidapi/libusb at this time.) +endif + +include $(FILE) diff --git a/Engine/lib/sdl/src/hidapi/libusb/Makefile.am b/Engine/lib/sdl/src/hidapi/libusb/Makefile.am new file mode 100644 index 000000000..13c9d3551 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/libusb/Makefile.am @@ -0,0 +1,27 @@ +AM_CPPFLAGS = -I$(top_srcdir)/hidapi $(CFLAGS_LIBUSB) + +if OS_LINUX +lib_LTLIBRARIES = libhidapi-libusb.la +libhidapi_libusb_la_SOURCES = hid.c +libhidapi_libusb_la_LDFLAGS = $(LTLDFLAGS) $(PTHREAD_CFLAGS) +libhidapi_libusb_la_LIBADD = $(LIBS_LIBUSB) +endif + +if OS_FREEBSD +lib_LTLIBRARIES = libhidapi.la +libhidapi_la_SOURCES = hid.c +libhidapi_la_LDFLAGS = $(LTLDFLAGS) +libhidapi_la_LIBADD = $(LIBS_LIBUSB) +endif + +if OS_KFREEBSD +lib_LTLIBRARIES = libhidapi.la +libhidapi_la_SOURCES = hid.c +libhidapi_la_LDFLAGS = $(LTLDFLAGS) +libhidapi_la_LIBADD = $(LIBS_LIBUSB) +endif + +hdrdir = $(includedir)/hidapi +hdr_HEADERS = $(top_srcdir)/hidapi/hidapi.h + +EXTRA_DIST = Makefile-manual diff --git a/Engine/lib/sdl/src/hidapi/libusb/Makefile.freebsd b/Engine/lib/sdl/src/hidapi/libusb/Makefile.freebsd new file mode 100644 index 000000000..5e69e77d4 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/libusb/Makefile.freebsd @@ -0,0 +1,46 @@ +########################################### +# Simple Makefile for HIDAPI test program +# +# Alan Ott +# Signal 11 Software +# 2010-06-01 +########################################### + +all: hidtest libs + +libs: libhidapi.so + +CC ?= cc +CFLAGS ?= -Wall -g -fPIC + +CXX ?= c++ +CXXFLAGS ?= -Wall -g + +COBJS = hid.o +CPPOBJS = ../hidtest/hidtest.o +OBJS = $(COBJS) $(CPPOBJS) +INCLUDES = -I../hidapi -I/usr/local/include +LDFLAGS = -L/usr/local/lib +LIBS = -lusb -liconv -pthread + + +# Console Test Program +hidtest: $(OBJS) + $(CXX) $(CXXFLAGS) $(LDFLAGS) $^ -o $@ $(LIBS) + +# Shared Libs +libhidapi.so: $(COBJS) + $(CC) $(LDFLAGS) -shared -Wl,-soname,$@.0 $^ -o $@ $(LIBS) + +# Objects +$(COBJS): %.o: %.c + $(CC) $(CFLAGS) -c $(INCLUDES) $< -o $@ + +$(CPPOBJS): %.o: %.cpp + $(CXX) $(CXXFLAGS) -c $(INCLUDES) $< -o $@ + + +clean: + rm -f $(OBJS) hidtest libhidapi.so ../hidtest/hidtest.o + +.PHONY: clean libs diff --git a/Engine/lib/sdl/src/hidapi/libusb/Makefile.linux b/Engine/lib/sdl/src/hidapi/libusb/Makefile.linux new file mode 100644 index 000000000..337b556e1 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/libusb/Makefile.linux @@ -0,0 +1,49 @@ +########################################### +# Simple Makefile for HIDAPI test program +# +# Alan Ott +# Signal 11 Software +# 2010-06-01 +########################################### + +all: hidtest-libusb libs + +libs: libhidapi-libusb.so + +CC ?= gcc +CFLAGS ?= -Wall -g -fpic + +CXX ?= g++ +CXXFLAGS ?= -Wall -g -fpic + +LDFLAGS ?= -Wall -g + +COBJS_LIBUSB = hid.o +COBJS = $(COBJS_LIBUSB) +CPPOBJS = ../hidtest/hidtest.o +OBJS = $(COBJS) $(CPPOBJS) +LIBS_USB = `pkg-config libusb-1.0 --libs` -lrt -lpthread +LIBS = $(LIBS_USB) +INCLUDES ?= -I../hidapi `pkg-config libusb-1.0 --cflags` + + +# Console Test Program +hidtest-libusb: $(COBJS_LIBUSB) $(CPPOBJS) + $(CXX) $(LDFLAGS) $^ $(LIBS_USB) -o $@ + +# Shared Libs +libhidapi-libusb.so: $(COBJS_LIBUSB) + $(CC) $(LDFLAGS) $(LIBS_USB) -shared -fpic -Wl,-soname,$@.0 $^ -o $@ + +# Objects +$(COBJS): %.o: %.c + $(CC) $(CFLAGS) -c $(INCLUDES) $< -o $@ + +$(CPPOBJS): %.o: %.cpp + $(CXX) $(CXXFLAGS) -c $(INCLUDES) $< -o $@ + + +clean: + rm -f $(OBJS) hidtest-libusb libhidapi-libusb.so ../hidtest/hidtest.o + +.PHONY: clean libs diff --git a/Engine/lib/sdl/src/hidapi/libusb/hid.c b/Engine/lib/sdl/src/hidapi/libusb/hid.c new file mode 100644 index 000000000..17378fff4 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/libusb/hid.c @@ -0,0 +1,1620 @@ +/******************************************************* + HIDAPI - Multi-Platform library for + communication with HID devices. + + Alan Ott + Signal 11 Software + + 8/22/2009 + Linux Version - 6/2/2010 + Libusb Version - 8/13/2010 + FreeBSD Version - 11/1/2011 + + Copyright 2009, All Rights Reserved. + + At the discretion of the user of this library, + this software may be licensed under the terms of the + GNU General Public License v3, a BSD-Style license, or the + original HIDAPI license as outlined in the LICENSE.txt, + LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt + files located at the root of the source distribution. + These files may also be found in the public source + code repository located at: + http://github.com/signal11/hidapi . +********************************************************/ +#include "../../SDL_internal.h" + +#ifdef SDL_JOYSTICK_HIDAPI + +#ifndef _GNU_SOURCE +#define _GNU_SOURCE /* needed for wcsdup() before glibc 2.10 */ +#endif + +/* C */ +#include +#include +#include +#include +#include +#include + +/* Unix */ +#include +#include +#include +#include +#include +#include +#include +#include + +/* GNU / LibUSB */ +#include +#ifndef __ANDROID__ +#include +#endif + +#include "hidapi.h" + +#ifdef NAMESPACE +namespace NAMESPACE +{ +#endif + +#ifdef __ANDROID__ + +/* Barrier implementation because Android/Bionic don't have pthread_barrier. + This implementation came from Brent Priddy and was posted on + StackOverflow. It is used with his permission. */ +typedef int pthread_barrierattr_t; +typedef struct pthread_barrier { + pthread_mutex_t mutex; + pthread_cond_t cond; + int count; + int trip_count; +} pthread_barrier_t; + +static int pthread_barrier_init(pthread_barrier_t *barrier, const pthread_barrierattr_t *attr, unsigned int count) +{ + if(count == 0) { + errno = EINVAL; + return -1; + } + + if(pthread_mutex_init(&barrier->mutex, 0) < 0) { + return -1; + } + if(pthread_cond_init(&barrier->cond, 0) < 0) { + pthread_mutex_destroy(&barrier->mutex); + return -1; + } + barrier->trip_count = count; + barrier->count = 0; + + return 0; +} + +static int pthread_barrier_destroy(pthread_barrier_t *barrier) +{ + pthread_cond_destroy(&barrier->cond); + pthread_mutex_destroy(&barrier->mutex); + return 0; +} + +static int pthread_barrier_wait(pthread_barrier_t *barrier) +{ + pthread_mutex_lock(&barrier->mutex); + ++(barrier->count); + if(barrier->count >= barrier->trip_count) + { + barrier->count = 0; + pthread_cond_broadcast(&barrier->cond); + pthread_mutex_unlock(&barrier->mutex); + return 1; + } + else + { + pthread_cond_wait(&barrier->cond, &(barrier->mutex)); + pthread_mutex_unlock(&barrier->mutex); + return 0; + } +} + +#endif + +#if defined(__cplusplus) && !defined(NAMESPACE) +extern "C" { +#endif + +#ifdef DEBUG_PRINTF +#define LOG(...) fprintf(stderr, __VA_ARGS__) +#else +#define LOG(...) do {} while (0) +#endif + +#ifndef __FreeBSD__ +#define DETACH_KERNEL_DRIVER +#endif + +/* Uncomment to enable the retrieval of Usage and Usage Page in +hid_enumerate(). Warning, on platforms different from FreeBSD +this is very invasive as it requires the detach +and re-attach of the kernel driver. See comments inside hid_enumerate(). +libusb HIDAPI programs are encouraged to use the interface number +instead to differentiate between interfaces on a composite HID device. */ +/*#define INVASIVE_GET_USAGE*/ + +/* Linked List of input reports received from the device. */ +struct input_report { + uint8_t *data; + size_t len; + struct input_report *next; +}; + + +struct hid_device_ { + /* Handle to the actual device. */ + libusb_device_handle *device_handle; + + /* Endpoint information */ + int input_endpoint; + int output_endpoint; + int input_ep_max_packet_size; + + /* The interface number of the HID */ + int interface; + + /* Indexes of Strings */ + int manufacturer_index; + int product_index; + int serial_index; + + /* Whether blocking reads are used */ + int blocking; /* boolean */ + + /* Read thread objects */ + pthread_t thread; + pthread_mutex_t mutex; /* Protects input_reports */ + pthread_cond_t condition; + pthread_barrier_t barrier; /* Ensures correct startup sequence */ + int shutdown_thread; + int cancelled; + struct libusb_transfer *transfer; + + /* List of received input reports. */ + struct input_report *input_reports; +}; + +static libusb_context *usb_context = NULL; + +uint16_t get_usb_code_for_current_locale(void); +static int return_data(hid_device *dev, unsigned char *data, size_t length); + +static hid_device *new_hid_device(void) +{ + hid_device *dev = (hid_device *)calloc(1, sizeof(hid_device)); + dev->blocking = 1; + + pthread_mutex_init(&dev->mutex, NULL); + pthread_cond_init(&dev->condition, NULL); + pthread_barrier_init(&dev->barrier, NULL, 2); + + return dev; +} + +static void free_hid_device(hid_device *dev) +{ + /* Clean up the thread objects */ + pthread_barrier_destroy(&dev->barrier); + pthread_cond_destroy(&dev->condition); + pthread_mutex_destroy(&dev->mutex); + + /* Free the device itself */ + free(dev); +} + +#if 0 +/*TODO: Implement this funciton on hidapi/libusb.. */ +static void register_error(hid_device *device, const char *op) +{ + +} +#endif + +#ifdef INVASIVE_GET_USAGE +/* Get bytes from a HID Report Descriptor. + Only call with a num_bytes of 0, 1, 2, or 4. */ +static uint32_t get_bytes(uint8_t *rpt, size_t len, size_t num_bytes, size_t cur) +{ + /* Return if there aren't enough bytes. */ + if (cur + num_bytes >= len) + return 0; + + if (num_bytes == 0) + return 0; + else if (num_bytes == 1) { + return rpt[cur+1]; + } + else if (num_bytes == 2) { + return (rpt[cur+2] * 256 + rpt[cur+1]); + } + else if (num_bytes == 4) { + return (rpt[cur+4] * 0x01000000 + + rpt[cur+3] * 0x00010000 + + rpt[cur+2] * 0x00000100 + + rpt[cur+1] * 0x00000001); + } + else + return 0; +} + +/* Retrieves the device's Usage Page and Usage from the report + descriptor. The algorithm is simple, as it just returns the first + Usage and Usage Page that it finds in the descriptor. + The return value is 0 on success and -1 on failure. */ +static int get_usage(uint8_t *report_descriptor, size_t size, + unsigned short *usage_page, unsigned short *usage) +{ + unsigned int i = 0; + int size_code; + int data_len, key_size; + int usage_found = 0, usage_page_found = 0; + + while (i < size) { + int key = report_descriptor[i]; + int key_cmd = key & 0xfc; + + //printf("key: %02hhx\n", key); + + if ((key & 0xf0) == 0xf0) { + /* This is a Long Item. The next byte contains the + length of the data section (value) for this key. + See the HID specification, version 1.11, section + 6.2.2.3, titled "Long Items." */ + if (i+1 < size) + data_len = report_descriptor[i+1]; + else + data_len = 0; /* malformed report */ + key_size = 3; + } + else { + /* This is a Short Item. The bottom two bits of the + key contain the size code for the data section + (value) for this key. Refer to the HID + specification, version 1.11, section 6.2.2.2, + titled "Short Items." */ + size_code = key & 0x3; + switch (size_code) { + case 0: + case 1: + case 2: + data_len = size_code; + break; + case 3: + data_len = 4; + break; + default: + /* Can't ever happen since size_code is & 0x3 */ + data_len = 0; + break; + }; + key_size = 1; + } + + if (key_cmd == 0x4) { + *usage_page = get_bytes(report_descriptor, size, data_len, i); + usage_page_found = 1; + //printf("Usage Page: %x\n", (uint32_t)*usage_page); + } + if (key_cmd == 0x8) { + *usage = get_bytes(report_descriptor, size, data_len, i); + usage_found = 1; + //printf("Usage: %x\n", (uint32_t)*usage); + } + + if (usage_page_found && usage_found) + return 0; /* success */ + + /* Skip over this key and it's associated data */ + i += data_len + key_size; + } + + return -1; /* failure */ +} +#endif /* INVASIVE_GET_USAGE */ + +#if defined(__FreeBSD__) && __FreeBSD__ < 10 +/* The libusb version included in FreeBSD < 10 doesn't have this function. In + mainline libusb, it's inlined in libusb.h. This function will bear a striking + resemblance to that one, because there's about one way to code it. + + Note that the data parameter is Unicode in UTF-16LE encoding. + Return value is the number of bytes in data, or LIBUSB_ERROR_*. + */ +static inline int libusb_get_string_descriptor(libusb_device_handle *dev, + uint8_t descriptor_index, uint16_t lang_id, + unsigned char *data, int length) +{ + return libusb_control_transfer(dev, + LIBUSB_ENDPOINT_IN | 0x0, /* Endpoint 0 IN */ + LIBUSB_REQUEST_GET_DESCRIPTOR, + (LIBUSB_DT_STRING << 8) | descriptor_index, + lang_id, data, (uint16_t) length, 1000); +} + +#endif + + +/* Get the first language the device says it reports. This comes from + USB string #0. */ +static uint16_t get_first_language(libusb_device_handle *dev) +{ + uint16_t buf[32]; + int len; + + /* Get the string from libusb. */ + len = libusb_get_string_descriptor(dev, + 0x0, /* String ID */ + 0x0, /* Language */ + (unsigned char*)buf, + sizeof(buf)); + if (len < 4) + return 0x0; + + return buf[1]; /* First two bytes are len and descriptor type. */ +} + +static int is_language_supported(libusb_device_handle *dev, uint16_t lang) +{ + uint16_t buf[32]; + int len; + int i; + + /* Get the string from libusb. */ + len = libusb_get_string_descriptor(dev, + 0x0, /* String ID */ + 0x0, /* Language */ + (unsigned char*)buf, + sizeof(buf)); + if (len < 4) + return 0x0; + + + len /= 2; /* language IDs are two-bytes each. */ + /* Start at index 1 because there are two bytes of protocol data. */ + for (i = 1; i < len; i++) { + if (buf[i] == lang) + return 1; + } + + return 0; +} + + +/* This function returns a newly allocated wide string containing the USB + device string numbered by the index. The returned string must be freed + by using free(). */ +static wchar_t *get_usb_string(libusb_device_handle *dev, uint8_t idx) +{ + char buf[512]; + int len; + wchar_t *str = NULL; + +#ifndef __ANDROID__ /* we don't use iconv on Android */ + wchar_t wbuf[256]; + /* iconv variables */ + iconv_t ic; + size_t inbytes; + size_t outbytes; + size_t res; +#ifdef __FreeBSD__ + const char *inptr; +#else + char *inptr; +#endif + char *outptr; +#endif + + /* Determine which language to use. */ + uint16_t lang; + lang = get_usb_code_for_current_locale(); + if (!is_language_supported(dev, lang)) + lang = get_first_language(dev); + + /* Get the string from libusb. */ + len = libusb_get_string_descriptor(dev, + idx, + lang, + (unsigned char*)buf, + sizeof(buf)); + if (len < 0) + return NULL; + +#ifdef __ANDROID__ + + /* Bionic does not have iconv support nor wcsdup() function, so it + has to be done manually. The following code will only work for + code points that can be represented as a single UTF-16 character, + and will incorrectly convert any code points which require more + than one UTF-16 character. + + Skip over the first character (2-bytes). */ + len -= 2; + str = malloc((len / 2 + 1) * sizeof(wchar_t)); + int i; + for (i = 0; i < len / 2; i++) { + str[i] = buf[i * 2 + 2] | (buf[i * 2 + 3] << 8); + } + str[len / 2] = 0x00000000; + +#else + + /* buf does not need to be explicitly NULL-terminated because + it is only passed into iconv() which does not need it. */ + + /* Initialize iconv. */ + ic = iconv_open("WCHAR_T", "UTF-16LE"); + if (ic == (iconv_t)-1) { + LOG("iconv_open() failed\n"); + return NULL; + } + + /* Convert to native wchar_t (UTF-32 on glibc/BSD systems). + Skip the first character (2-bytes). */ + inptr = buf+2; + inbytes = len-2; + outptr = (char*) wbuf; + outbytes = sizeof(wbuf); + res = iconv(ic, &inptr, &inbytes, &outptr, &outbytes); + if (res == (size_t)-1) { + LOG("iconv() failed\n"); + goto err; + } + + /* Write the terminating NULL. */ + wbuf[sizeof(wbuf)/sizeof(wbuf[0])-1] = 0x00000000; + if (outbytes >= sizeof(wbuf[0])) + *((wchar_t*)outptr) = 0x00000000; + + /* Allocate and copy the string. */ + str = wcsdup(wbuf); + +err: + iconv_close(ic); + +#endif + + return str; +} + +static char *make_path(libusb_device *dev, int interface_number) +{ + char str[64]; + snprintf(str, sizeof(str), "%04x:%04x:%02x", + libusb_get_bus_number(dev), + libusb_get_device_address(dev), + interface_number); + str[sizeof(str)-1] = '\0'; + + return strdup(str); +} + + +int HID_API_EXPORT hid_init(void) +{ + if (!usb_context) { + const char *locale; + + /* Init Libusb */ + if (libusb_init(&usb_context)) + return -1; + + /* Set the locale if it's not set. */ + locale = setlocale(LC_CTYPE, NULL); + if (!locale) + setlocale(LC_CTYPE, ""); + } + + return 0; +} + +int HID_API_EXPORT hid_exit(void) +{ + if (usb_context) { + libusb_exit(usb_context); + usb_context = NULL; + } + + return 0; +} + +static int is_xbox360(unsigned short vendor_id, const struct libusb_interface_descriptor *intf_desc) +{ + static const int XB360_IFACE_SUBCLASS = 93; + static const int XB360_IFACE_PROTOCOL = 1; /* Wired only */ + static const int SUPPORTED_VENDORS[] = { + 0x0079, /* GPD Win 2 */ + 0x044f, /* Thrustmaster */ + 0x045e, /* Microsoft */ + 0x046d, /* Logitech */ + 0x056e, /* Elecom */ + 0x06a3, /* Saitek */ + 0x0738, /* Mad Catz */ + 0x07ff, /* Mad Catz */ + 0x0e6f, /* Unknown */ + 0x0f0d, /* Hori */ + 0x11c9, /* Nacon */ + 0x12ab, /* Unknown */ + 0x1430, /* RedOctane */ + 0x146b, /* BigBen */ + 0x1532, /* Razer Sabertooth */ + 0x15e4, /* Numark */ + 0x162e, /* Joytech */ + 0x1689, /* Razer Onza */ + 0x1bad, /* Harmonix */ + 0x24c6, /* PowerA */ + }; + + if (intf_desc->bInterfaceNumber == 0 && + intf_desc->bInterfaceClass == LIBUSB_CLASS_VENDOR_SPEC && + intf_desc->bInterfaceSubClass == XB360_IFACE_SUBCLASS && + intf_desc->bInterfaceProtocol == XB360_IFACE_PROTOCOL) { + int i; + for (i = 0; i < sizeof(SUPPORTED_VENDORS)/sizeof(SUPPORTED_VENDORS[0]); ++i) { + if (vendor_id == SUPPORTED_VENDORS[i]) { + return 1; + } + } + } + return 0; +} + +static int is_xboxone(unsigned short vendor_id, const struct libusb_interface_descriptor *intf_desc) +{ + static const int XB1_IFACE_SUBCLASS = 71; + static const int XB1_IFACE_PROTOCOL = 208; + static const int SUPPORTED_VENDORS[] = { + 0x045e, /* Microsoft */ + 0x0738, /* Mad Catz */ + 0x0e6f, /* Unknown */ + 0x0f0d, /* Hori */ + 0x1532, /* Razer Wildcat */ + 0x24c6, /* PowerA */ + }; + + if (intf_desc->bInterfaceNumber == 0 && + intf_desc->bInterfaceClass == LIBUSB_CLASS_VENDOR_SPEC && + intf_desc->bInterfaceSubClass == XB1_IFACE_SUBCLASS && + intf_desc->bInterfaceProtocol == XB1_IFACE_PROTOCOL) { + int i; + for (i = 0; i < sizeof(SUPPORTED_VENDORS)/sizeof(SUPPORTED_VENDORS[0]); ++i) { + if (vendor_id == SUPPORTED_VENDORS[i]) { + return 1; + } + } + } + return 0; +} + +static int should_enumerate_interface(unsigned short vendor_id, const struct libusb_interface_descriptor *intf_desc) +{ + if (intf_desc->bInterfaceClass == LIBUSB_CLASS_HID) + return 1; + + /* Also enumerate Xbox 360 controllers */ + if (is_xbox360(vendor_id, intf_desc)) + { + /* hid_write() to Xbox 360 controllers doesn't seem to work on Linux: + - xpad 1-2:1.0: xpad_try_sending_next_out_packet - usb_submit_urb failed with result -2 + Xbox 360 controller support is good on Linux anyway, so we'll ignore this for now. + return 1; + */ + } + + /* Also enumerate Xbox One controllers */ + if (is_xboxone(vendor_id, intf_desc)) + return 1; + + return 0; +} + +struct hid_device_info HID_API_EXPORT *hid_enumerate(unsigned short vendor_id, unsigned short product_id) +{ + libusb_device **devs; + libusb_device *dev; + libusb_device_handle *handle; + ssize_t num_devs; + int i = 0; + + struct hid_device_info *root = NULL; /* return object */ + struct hid_device_info *cur_dev = NULL; + + if(hid_init() < 0) + return NULL; + + num_devs = libusb_get_device_list(usb_context, &devs); + if (num_devs < 0) + return NULL; + while ((dev = devs[i++]) != NULL) { + struct libusb_device_descriptor desc; + struct libusb_config_descriptor *conf_desc = NULL; + int j, k; + int interface_num = 0; + + int res = libusb_get_device_descriptor(dev, &desc); + unsigned short dev_vid = desc.idVendor; + unsigned short dev_pid = desc.idProduct; + + res = libusb_get_active_config_descriptor(dev, &conf_desc); + if (res < 0) + libusb_get_config_descriptor(dev, 0, &conf_desc); + if (conf_desc) { + for (j = 0; j < conf_desc->bNumInterfaces; j++) { + const struct libusb_interface *intf = &conf_desc->interface[j]; + for (k = 0; k < intf->num_altsetting; k++) { + const struct libusb_interface_descriptor *intf_desc; + intf_desc = &intf->altsetting[k]; + if (should_enumerate_interface(dev_vid, intf_desc)) { + interface_num = intf_desc->bInterfaceNumber; + + /* Check the VID/PID against the arguments */ + if ((vendor_id == 0x0 || vendor_id == dev_vid) && + (product_id == 0x0 || product_id == dev_pid)) { + struct hid_device_info *tmp; + + /* VID/PID match. Create the record. */ + tmp = (struct hid_device_info *)calloc(1, sizeof(struct hid_device_info)); + if (cur_dev) { + cur_dev->next = tmp; + } + else { + root = tmp; + } + cur_dev = tmp; + + /* Fill out the record */ + cur_dev->next = NULL; + cur_dev->path = make_path(dev, interface_num); + + res = libusb_open(dev, &handle); + + if (res >= 0) { + /* Serial Number */ + if (desc.iSerialNumber > 0) + cur_dev->serial_number = + get_usb_string(handle, desc.iSerialNumber); + + /* Manufacturer and Product strings */ + if (desc.iManufacturer > 0) + cur_dev->manufacturer_string = + get_usb_string(handle, desc.iManufacturer); + if (desc.iProduct > 0) + cur_dev->product_string = + get_usb_string(handle, desc.iProduct); + +#ifdef INVASIVE_GET_USAGE +{ + /* + This section is removed because it is too + invasive on the system. Getting a Usage Page + and Usage requires parsing the HID Report + descriptor. Getting a HID Report descriptor + involves claiming the interface. Claiming the + interface involves detaching the kernel driver. + Detaching the kernel driver is hard on the system + because it will unclaim interfaces (if another + app has them claimed) and the re-attachment of + the driver will sometimes change /dev entry names. + It is for these reasons that this section is + #if 0. For composite devices, use the interface + field in the hid_device_info struct to distinguish + between interfaces. */ + unsigned char data[256]; +#ifdef DETACH_KERNEL_DRIVER + int detached = 0; + /* Usage Page and Usage */ + res = libusb_kernel_driver_active(handle, interface_num); + if (res == 1) { + res = libusb_detach_kernel_driver(handle, interface_num); + if (res < 0) + LOG("Couldn't detach kernel driver, even though a kernel driver was attached."); + else + detached = 1; + } +#endif + res = libusb_claim_interface(handle, interface_num); + if (res >= 0) { + /* Get the HID Report Descriptor. */ + res = libusb_control_transfer(handle, LIBUSB_ENDPOINT_IN|LIBUSB_RECIPIENT_INTERFACE, LIBUSB_REQUEST_GET_DESCRIPTOR, (LIBUSB_DT_REPORT << 8)|interface_num, 0, data, sizeof(data), 5000); + if (res >= 0) { + unsigned short page=0, usage=0; + /* Parse the usage and usage page + out of the report descriptor. */ + get_usage(data, res, &page, &usage); + cur_dev->usage_page = page; + cur_dev->usage = usage; + } + else + LOG("libusb_control_transfer() for getting the HID report failed with %d\n", res); + + /* Release the interface */ + res = libusb_release_interface(handle, interface_num); + if (res < 0) + LOG("Can't release the interface.\n"); + } + else + LOG("Can't claim interface %d\n", res); +#ifdef DETACH_KERNEL_DRIVER + /* Re-attach kernel driver if necessary. */ + if (detached) { + res = libusb_attach_kernel_driver(handle, interface_num); + if (res < 0) + LOG("Couldn't re-attach kernel driver.\n"); + } +#endif +} +#endif /* INVASIVE_GET_USAGE */ + + libusb_close(handle); + } + /* VID/PID */ + cur_dev->vendor_id = dev_vid; + cur_dev->product_id = dev_pid; + + /* Release Number */ + cur_dev->release_number = desc.bcdDevice; + + /* Interface Number */ + cur_dev->interface_number = interface_num; + } + } + } /* altsettings */ + } /* interfaces */ + libusb_free_config_descriptor(conf_desc); + } + } + + libusb_free_device_list(devs, 1); + + return root; +} + +void HID_API_EXPORT hid_free_enumeration(struct hid_device_info *devs) +{ + struct hid_device_info *d = devs; + while (d) { + struct hid_device_info *next = d->next; + free(d->path); + free(d->serial_number); + free(d->manufacturer_string); + free(d->product_string); + free(d); + d = next; + } +} + +hid_device * hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number) +{ + struct hid_device_info *devs, *cur_dev; + const char *path_to_open = NULL; + hid_device *handle = NULL; + + devs = hid_enumerate(vendor_id, product_id); + cur_dev = devs; + while (cur_dev) { + if (cur_dev->vendor_id == vendor_id && + cur_dev->product_id == product_id) { + if (serial_number) { + if (cur_dev->serial_number && + wcscmp(serial_number, cur_dev->serial_number) == 0) { + path_to_open = cur_dev->path; + break; + } + } + else { + path_to_open = cur_dev->path; + break; + } + } + cur_dev = cur_dev->next; + } + + if (path_to_open) { + /* Open the device */ + handle = hid_open_path(path_to_open, 0); + } + + hid_free_enumeration(devs); + + return handle; +} + +static void read_callback(struct libusb_transfer *transfer) +{ + hid_device *dev = (hid_device *)transfer->user_data; + int res; + + if (transfer->status == LIBUSB_TRANSFER_COMPLETED) { + + struct input_report *rpt = (struct input_report *)malloc(sizeof(*rpt)); + rpt->data = (uint8_t *)malloc(transfer->actual_length); + memcpy(rpt->data, transfer->buffer, transfer->actual_length); + rpt->len = transfer->actual_length; + rpt->next = NULL; + + pthread_mutex_lock(&dev->mutex); + + /* Attach the new report object to the end of the list. */ + if (dev->input_reports == NULL) { + /* The list is empty. Put it at the root. */ + dev->input_reports = rpt; + pthread_cond_signal(&dev->condition); + } + else { + /* Find the end of the list and attach. */ + struct input_report *cur = dev->input_reports; + int num_queued = 0; + while (cur->next != NULL) { + cur = cur->next; + num_queued++; + } + cur->next = rpt; + + /* Pop one off if we've reached 30 in the queue. This + way we don't grow forever if the user never reads + anything from the device. */ + if (num_queued > 30) { + return_data(dev, NULL, 0); + } + } + pthread_mutex_unlock(&dev->mutex); + } + else if (transfer->status == LIBUSB_TRANSFER_CANCELLED) { + dev->shutdown_thread = 1; + dev->cancelled = 1; + return; + } + else if (transfer->status == LIBUSB_TRANSFER_NO_DEVICE) { + dev->shutdown_thread = 1; + dev->cancelled = 1; + return; + } + else if (transfer->status == LIBUSB_TRANSFER_TIMED_OUT) { + //LOG("Timeout (normal)\n"); + } + else { + LOG("Unknown transfer code: %d\n", transfer->status); + } + + /* Re-submit the transfer object. */ + res = libusb_submit_transfer(transfer); + if (res != 0) { + LOG("Unable to submit URB. libusb error code: %d\n", res); + dev->shutdown_thread = 1; + dev->cancelled = 1; + } +} + + +static void *read_thread(void *param) +{ + hid_device *dev = (hid_device *)param; + unsigned char *buf; + const size_t length = dev->input_ep_max_packet_size; + + /* Set up the transfer object. */ + buf = (unsigned char *)malloc(length); + dev->transfer = libusb_alloc_transfer(0); + libusb_fill_interrupt_transfer(dev->transfer, + dev->device_handle, + dev->input_endpoint, + buf, + length, + read_callback, + dev, + 5000/*timeout*/); + + /* Make the first submission. Further submissions are made + from inside read_callback() */ + libusb_submit_transfer(dev->transfer); + + /* Notify the main thread that the read thread is up and running. */ + pthread_barrier_wait(&dev->barrier); + + /* Handle all the events. */ + while (!dev->shutdown_thread) { + int res; + res = libusb_handle_events(usb_context); + if (res < 0) { + /* There was an error. */ + LOG("read_thread(): libusb reports error # %d\n", res); + + /* Break out of this loop only on fatal error.*/ + if (res != LIBUSB_ERROR_BUSY && + res != LIBUSB_ERROR_TIMEOUT && + res != LIBUSB_ERROR_OVERFLOW && + res != LIBUSB_ERROR_INTERRUPTED) { + break; + } + } + } + + /* Cancel any transfer that may be pending. This call will fail + if no transfers are pending, but that's OK. */ + libusb_cancel_transfer(dev->transfer); + + while (!dev->cancelled) + libusb_handle_events_completed(usb_context, &dev->cancelled); + + /* Now that the read thread is stopping, Wake any threads which are + waiting on data (in hid_read_timeout()). Do this under a mutex to + make sure that a thread which is about to go to sleep waiting on + the condition actually will go to sleep before the condition is + signaled. */ + pthread_mutex_lock(&dev->mutex); + pthread_cond_broadcast(&dev->condition); + pthread_mutex_unlock(&dev->mutex); + + /* The dev->transfer->buffer and dev->transfer objects are cleaned up + in hid_close(). They are not cleaned up here because this thread + could end either due to a disconnect or due to a user + call to hid_close(). In both cases the objects can be safely + cleaned up after the call to pthread_join() (in hid_close()), but + since hid_close() calls libusb_cancel_transfer(), on these objects, + they can not be cleaned up here. */ + + return NULL; +} + + +hid_device * HID_API_EXPORT hid_open_path(const char *path, int bExclusive) +{ + hid_device *dev = NULL; + + libusb_device **devs; + libusb_device *usb_dev; + int res; + int d = 0; + int good_open = 0; + + if(hid_init() < 0) + return NULL; + + dev = new_hid_device(); + + libusb_get_device_list(usb_context, &devs); + while ((usb_dev = devs[d++]) != NULL) { + struct libusb_device_descriptor desc; + struct libusb_config_descriptor *conf_desc = NULL; + int i,j,k; + libusb_get_device_descriptor(usb_dev, &desc); + + if (libusb_get_active_config_descriptor(usb_dev, &conf_desc) < 0) + continue; + for (j = 0; j < conf_desc->bNumInterfaces; j++) { + const struct libusb_interface *intf = &conf_desc->interface[j]; + for (k = 0; k < intf->num_altsetting; k++) { + const struct libusb_interface_descriptor *intf_desc; + intf_desc = &intf->altsetting[k]; + if (should_enumerate_interface(desc.idVendor, intf_desc)) { + char *dev_path = make_path(usb_dev, intf_desc->bInterfaceNumber); + if (!strcmp(dev_path, path)) { + /* Matched Paths. Open this device */ + + /* OPEN HERE */ + res = libusb_open(usb_dev, &dev->device_handle); + if (res < 0) { + LOG("can't open device\n"); + free(dev_path); + break; + } + good_open = 1; +#ifdef DETACH_KERNEL_DRIVER + /* Detach the kernel driver, but only if the + device is managed by the kernel */ + if (libusb_kernel_driver_active(dev->device_handle, intf_desc->bInterfaceNumber) == 1) { + res = libusb_detach_kernel_driver(dev->device_handle, intf_desc->bInterfaceNumber); + if (res < 0) { + libusb_close(dev->device_handle); + LOG("Unable to detach Kernel Driver\n"); + free(dev_path); + good_open = 0; + break; + } + } +#endif + res = libusb_claim_interface(dev->device_handle, intf_desc->bInterfaceNumber); + if (res < 0) { + LOG("can't claim interface %d: %d\n", intf_desc->bInterfaceNumber, res); + free(dev_path); + libusb_close(dev->device_handle); + good_open = 0; + break; + } + + /* Store off the string descriptor indexes */ + dev->manufacturer_index = desc.iManufacturer; + dev->product_index = desc.iProduct; + dev->serial_index = desc.iSerialNumber; + + /* Store off the interface number */ + dev->interface = intf_desc->bInterfaceNumber; + + /* Find the INPUT and OUTPUT endpoints. An + OUTPUT endpoint is not required. */ + for (i = 0; i < intf_desc->bNumEndpoints; i++) { + const struct libusb_endpoint_descriptor *ep + = &intf_desc->endpoint[i]; + + /* Determine the type and direction of this + endpoint. */ + int is_interrupt = + (ep->bmAttributes & LIBUSB_TRANSFER_TYPE_MASK) + == LIBUSB_TRANSFER_TYPE_INTERRUPT; + int is_output = + (ep->bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) + == LIBUSB_ENDPOINT_OUT; + int is_input = + (ep->bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) + == LIBUSB_ENDPOINT_IN; + + /* Decide whether to use it for input or output. */ + if (dev->input_endpoint == 0 && + is_interrupt && is_input) { + /* Use this endpoint for INPUT */ + dev->input_endpoint = ep->bEndpointAddress; + dev->input_ep_max_packet_size = ep->wMaxPacketSize; + } + if (dev->output_endpoint == 0 && + is_interrupt && is_output) { + /* Use this endpoint for OUTPUT */ + dev->output_endpoint = ep->bEndpointAddress; + } + } + + pthread_create(&dev->thread, NULL, read_thread, dev); + + /* Wait here for the read thread to be initialized. */ + pthread_barrier_wait(&dev->barrier); + + } + free(dev_path); + } + } + } + libusb_free_config_descriptor(conf_desc); + + } + + libusb_free_device_list(devs, 1); + + /* If we have a good handle, return it. */ + if (good_open) { + return dev; + } + else { + /* Unable to open any devices. */ + free_hid_device(dev); + return NULL; + } +} + + +int HID_API_EXPORT hid_write(hid_device *dev, const unsigned char *data, size_t length) +{ + int res; + int report_number = data[0]; + int skipped_report_id = 0; + + if (report_number == 0x0) { + data++; + length--; + skipped_report_id = 1; + } + + + if (dev->output_endpoint <= 0) { + /* No interrupt out endpoint. Use the Control Endpoint */ + res = libusb_control_transfer(dev->device_handle, + LIBUSB_REQUEST_TYPE_CLASS|LIBUSB_RECIPIENT_INTERFACE|LIBUSB_ENDPOINT_OUT, + 0x09/*HID Set_Report*/, + (2/*HID output*/ << 8) | report_number, + dev->interface, + (unsigned char *)data, length, + 1000/*timeout millis*/); + + if (res < 0) + return -1; + + if (skipped_report_id) + length++; + + return length; + } + else { + /* Use the interrupt out endpoint */ + int actual_length; + res = libusb_interrupt_transfer(dev->device_handle, + dev->output_endpoint, + (unsigned char*)data, + length, + &actual_length, 1000); + + if (res < 0) + return -1; + + if (skipped_report_id) + actual_length++; + + return actual_length; + } +} + +/* Helper function, to simplify hid_read(). + This should be called with dev->mutex locked. */ +static int return_data(hid_device *dev, unsigned char *data, size_t length) +{ + /* Copy the data out of the linked list item (rpt) into the + return buffer (data), and delete the liked list item. */ + struct input_report *rpt = dev->input_reports; + size_t len = (length < rpt->len)? length: rpt->len; + if (data && len > 0) + memcpy(data, rpt->data, len); + dev->input_reports = rpt->next; + free(rpt->data); + free(rpt); + return len; +} + +static void cleanup_mutex(void *param) +{ + hid_device *dev = (hid_device *)param; + pthread_mutex_unlock(&dev->mutex); +} + + +int HID_API_EXPORT hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds) +{ + int bytes_read = -1; + +#if 0 + int transferred; + int res = libusb_interrupt_transfer(dev->device_handle, dev->input_endpoint, data, length, &transferred, 5000); + LOG("transferred: %d\n", transferred); + return transferred; +#endif + + pthread_mutex_lock(&dev->mutex); + pthread_cleanup_push(&cleanup_mutex, dev); + + /* There's an input report queued up. Return it. */ + if (dev->input_reports) { + /* Return the first one */ + bytes_read = return_data(dev, data, length); + goto ret; + } + + if (dev->shutdown_thread) { + /* This means the device has been disconnected. + An error code of -1 should be returned. */ + bytes_read = -1; + goto ret; + } + + if (milliseconds == -1) { + /* Blocking */ + while (!dev->input_reports && !dev->shutdown_thread) { + pthread_cond_wait(&dev->condition, &dev->mutex); + } + if (dev->input_reports) { + bytes_read = return_data(dev, data, length); + } + } + else if (milliseconds > 0) { + /* Non-blocking, but called with timeout. */ + int res; + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += milliseconds / 1000; + ts.tv_nsec += (milliseconds % 1000) * 1000000; + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec++; + ts.tv_nsec -= 1000000000L; + } + + while (!dev->input_reports && !dev->shutdown_thread) { + res = pthread_cond_timedwait(&dev->condition, &dev->mutex, &ts); + if (res == 0) { + if (dev->input_reports) { + bytes_read = return_data(dev, data, length); + break; + } + + /* If we're here, there was a spurious wake up + or the read thread was shutdown. Run the + loop again (ie: don't break). */ + } + else if (res == ETIMEDOUT) { + /* Timed out. */ + bytes_read = 0; + break; + } + else { + /* Error. */ + bytes_read = -1; + break; + } + } + } + else { + /* Purely non-blocking */ + bytes_read = 0; + } + +ret: + pthread_mutex_unlock(&dev->mutex); + pthread_cleanup_pop(0); + + return bytes_read; +} + +int HID_API_EXPORT hid_read(hid_device *dev, unsigned char *data, size_t length) +{ + return hid_read_timeout(dev, data, length, dev->blocking ? -1 : 0); +} + +int HID_API_EXPORT hid_set_nonblocking(hid_device *dev, int nonblock) +{ + dev->blocking = !nonblock; + + return 0; +} + + +int HID_API_EXPORT hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length) +{ + int res = -1; + int skipped_report_id = 0; + int report_number = data[0]; + + if (report_number == 0x0) { + data++; + length--; + skipped_report_id = 1; + } + + res = libusb_control_transfer(dev->device_handle, + LIBUSB_REQUEST_TYPE_CLASS|LIBUSB_RECIPIENT_INTERFACE|LIBUSB_ENDPOINT_OUT, + 0x09/*HID set_report*/, + (3/*HID feature*/ << 8) | report_number, + dev->interface, + (unsigned char *)data, length, + 1000/*timeout millis*/); + + if (res < 0) + return -1; + + /* Account for the report ID */ + if (skipped_report_id) + length++; + + return length; +} + +int HID_API_EXPORT hid_get_feature_report(hid_device *dev, unsigned char *data, size_t length) +{ + int res = -1; + int skipped_report_id = 0; + int report_number = data[0]; + + if (report_number == 0x0) { + /* Offset the return buffer by 1, so that the report ID + will remain in byte 0. */ + data++; + length--; + skipped_report_id = 1; + } + res = libusb_control_transfer(dev->device_handle, + LIBUSB_REQUEST_TYPE_CLASS|LIBUSB_RECIPIENT_INTERFACE|LIBUSB_ENDPOINT_IN, + 0x01/*HID get_report*/, + (3/*HID feature*/ << 8) | report_number, + dev->interface, + (unsigned char *)data, length, + 1000/*timeout millis*/); + + if (res < 0) + return -1; + + if (skipped_report_id) + res++; + + return res; +} + + +void HID_API_EXPORT hid_close(hid_device *dev) +{ + if (!dev) + return; + + /* Cause read_thread() to stop. */ + dev->shutdown_thread = 1; + libusb_cancel_transfer(dev->transfer); + + /* Wait for read_thread() to end. */ + pthread_join(dev->thread, NULL); + + /* Clean up the Transfer objects allocated in read_thread(). */ + free(dev->transfer->buffer); + libusb_free_transfer(dev->transfer); + + /* release the interface */ + libusb_release_interface(dev->device_handle, dev->interface); + + /* Close the handle */ + libusb_close(dev->device_handle); + + /* Clear out the queue of received reports. */ + pthread_mutex_lock(&dev->mutex); + while (dev->input_reports) { + return_data(dev, NULL, 0); + } + pthread_mutex_unlock(&dev->mutex); + + free_hid_device(dev); +} + + +int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return hid_get_indexed_string(dev, dev->manufacturer_index, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_product_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return hid_get_indexed_string(dev, dev->product_index, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return hid_get_indexed_string(dev, dev->serial_index, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *dev, int string_index, wchar_t *string, size_t maxlen) +{ + wchar_t *str; + + str = get_usb_string(dev->device_handle, string_index); + if (str) { + wcsncpy(string, str, maxlen); + string[maxlen-1] = L'\0'; + free(str); + return 0; + } + else + return -1; +} + + +HID_API_EXPORT const wchar_t * HID_API_CALL hid_error(hid_device *dev) +{ + return NULL; +} + + +struct lang_map_entry { + const char *name; + const char *string_code; + uint16_t usb_code; +}; + +#define LANG(name,code,usb_code) { name, code, usb_code } +static struct lang_map_entry lang_map[] = { + LANG("Afrikaans", "af", 0x0436), + LANG("Albanian", "sq", 0x041C), + LANG("Arabic - United Arab Emirates", "ar_ae", 0x3801), + LANG("Arabic - Bahrain", "ar_bh", 0x3C01), + LANG("Arabic - Algeria", "ar_dz", 0x1401), + LANG("Arabic - Egypt", "ar_eg", 0x0C01), + LANG("Arabic - Iraq", "ar_iq", 0x0801), + LANG("Arabic - Jordan", "ar_jo", 0x2C01), + LANG("Arabic - Kuwait", "ar_kw", 0x3401), + LANG("Arabic - Lebanon", "ar_lb", 0x3001), + LANG("Arabic - Libya", "ar_ly", 0x1001), + LANG("Arabic - Morocco", "ar_ma", 0x1801), + LANG("Arabic - Oman", "ar_om", 0x2001), + LANG("Arabic - Qatar", "ar_qa", 0x4001), + LANG("Arabic - Saudi Arabia", "ar_sa", 0x0401), + LANG("Arabic - Syria", "ar_sy", 0x2801), + LANG("Arabic - Tunisia", "ar_tn", 0x1C01), + LANG("Arabic - Yemen", "ar_ye", 0x2401), + LANG("Armenian", "hy", 0x042B), + LANG("Azeri - Latin", "az_az", 0x042C), + LANG("Azeri - Cyrillic", "az_az", 0x082C), + LANG("Basque", "eu", 0x042D), + LANG("Belarusian", "be", 0x0423), + LANG("Bulgarian", "bg", 0x0402), + LANG("Catalan", "ca", 0x0403), + LANG("Chinese - China", "zh_cn", 0x0804), + LANG("Chinese - Hong Kong SAR", "zh_hk", 0x0C04), + LANG("Chinese - Macau SAR", "zh_mo", 0x1404), + LANG("Chinese - Singapore", "zh_sg", 0x1004), + LANG("Chinese - Taiwan", "zh_tw", 0x0404), + LANG("Croatian", "hr", 0x041A), + LANG("Czech", "cs", 0x0405), + LANG("Danish", "da", 0x0406), + LANG("Dutch - Netherlands", "nl_nl", 0x0413), + LANG("Dutch - Belgium", "nl_be", 0x0813), + LANG("English - Australia", "en_au", 0x0C09), + LANG("English - Belize", "en_bz", 0x2809), + LANG("English - Canada", "en_ca", 0x1009), + LANG("English - Caribbean", "en_cb", 0x2409), + LANG("English - Ireland", "en_ie", 0x1809), + LANG("English - Jamaica", "en_jm", 0x2009), + LANG("English - New Zealand", "en_nz", 0x1409), + LANG("English - Phillippines", "en_ph", 0x3409), + LANG("English - Southern Africa", "en_za", 0x1C09), + LANG("English - Trinidad", "en_tt", 0x2C09), + LANG("English - Great Britain", "en_gb", 0x0809), + LANG("English - United States", "en_us", 0x0409), + LANG("Estonian", "et", 0x0425), + LANG("Farsi", "fa", 0x0429), + LANG("Finnish", "fi", 0x040B), + LANG("Faroese", "fo", 0x0438), + LANG("French - France", "fr_fr", 0x040C), + LANG("French - Belgium", "fr_be", 0x080C), + LANG("French - Canada", "fr_ca", 0x0C0C), + LANG("French - Luxembourg", "fr_lu", 0x140C), + LANG("French - Switzerland", "fr_ch", 0x100C), + LANG("Gaelic - Ireland", "gd_ie", 0x083C), + LANG("Gaelic - Scotland", "gd", 0x043C), + LANG("German - Germany", "de_de", 0x0407), + LANG("German - Austria", "de_at", 0x0C07), + LANG("German - Liechtenstein", "de_li", 0x1407), + LANG("German - Luxembourg", "de_lu", 0x1007), + LANG("German - Switzerland", "de_ch", 0x0807), + LANG("Greek", "el", 0x0408), + LANG("Hebrew", "he", 0x040D), + LANG("Hindi", "hi", 0x0439), + LANG("Hungarian", "hu", 0x040E), + LANG("Icelandic", "is", 0x040F), + LANG("Indonesian", "id", 0x0421), + LANG("Italian - Italy", "it_it", 0x0410), + LANG("Italian - Switzerland", "it_ch", 0x0810), + LANG("Japanese", "ja", 0x0411), + LANG("Korean", "ko", 0x0412), + LANG("Latvian", "lv", 0x0426), + LANG("Lithuanian", "lt", 0x0427), + LANG("F.Y.R.O. Macedonia", "mk", 0x042F), + LANG("Malay - Malaysia", "ms_my", 0x043E), + LANG("Malay – Brunei", "ms_bn", 0x083E), + LANG("Maltese", "mt", 0x043A), + LANG("Marathi", "mr", 0x044E), + LANG("Norwegian - Bokml", "no_no", 0x0414), + LANG("Norwegian - Nynorsk", "no_no", 0x0814), + LANG("Polish", "pl", 0x0415), + LANG("Portuguese - Portugal", "pt_pt", 0x0816), + LANG("Portuguese - Brazil", "pt_br", 0x0416), + LANG("Raeto-Romance", "rm", 0x0417), + LANG("Romanian - Romania", "ro", 0x0418), + LANG("Romanian - Republic of Moldova", "ro_mo", 0x0818), + LANG("Russian", "ru", 0x0419), + LANG("Russian - Republic of Moldova", "ru_mo", 0x0819), + LANG("Sanskrit", "sa", 0x044F), + LANG("Serbian - Cyrillic", "sr_sp", 0x0C1A), + LANG("Serbian - Latin", "sr_sp", 0x081A), + LANG("Setsuana", "tn", 0x0432), + LANG("Slovenian", "sl", 0x0424), + LANG("Slovak", "sk", 0x041B), + LANG("Sorbian", "sb", 0x042E), + LANG("Spanish - Spain (Traditional)", "es_es", 0x040A), + LANG("Spanish - Argentina", "es_ar", 0x2C0A), + LANG("Spanish - Bolivia", "es_bo", 0x400A), + LANG("Spanish - Chile", "es_cl", 0x340A), + LANG("Spanish - Colombia", "es_co", 0x240A), + LANG("Spanish - Costa Rica", "es_cr", 0x140A), + LANG("Spanish - Dominican Republic", "es_do", 0x1C0A), + LANG("Spanish - Ecuador", "es_ec", 0x300A), + LANG("Spanish - Guatemala", "es_gt", 0x100A), + LANG("Spanish - Honduras", "es_hn", 0x480A), + LANG("Spanish - Mexico", "es_mx", 0x080A), + LANG("Spanish - Nicaragua", "es_ni", 0x4C0A), + LANG("Spanish - Panama", "es_pa", 0x180A), + LANG("Spanish - Peru", "es_pe", 0x280A), + LANG("Spanish - Puerto Rico", "es_pr", 0x500A), + LANG("Spanish - Paraguay", "es_py", 0x3C0A), + LANG("Spanish - El Salvador", "es_sv", 0x440A), + LANG("Spanish - Uruguay", "es_uy", 0x380A), + LANG("Spanish - Venezuela", "es_ve", 0x200A), + LANG("Southern Sotho", "st", 0x0430), + LANG("Swahili", "sw", 0x0441), + LANG("Swedish - Sweden", "sv_se", 0x041D), + LANG("Swedish - Finland", "sv_fi", 0x081D), + LANG("Tamil", "ta", 0x0449), + LANG("Tatar", "tt", 0X0444), + LANG("Thai", "th", 0x041E), + LANG("Turkish", "tr", 0x041F), + LANG("Tsonga", "ts", 0x0431), + LANG("Ukrainian", "uk", 0x0422), + LANG("Urdu", "ur", 0x0420), + LANG("Uzbek - Cyrillic", "uz_uz", 0x0843), + LANG("Uzbek – Latin", "uz_uz", 0x0443), + LANG("Vietnamese", "vi", 0x042A), + LANG("Xhosa", "xh", 0x0434), + LANG("Yiddish", "yi", 0x043D), + LANG("Zulu", "zu", 0x0435), + LANG(NULL, NULL, 0x0), +}; + +uint16_t get_usb_code_for_current_locale(void) +{ + char *locale; + char search_string[64]; + char *ptr; + struct lang_map_entry *lang; + + /* Get the current locale. */ + locale = setlocale(0, NULL); + if (!locale) + return 0x0; + + /* Make a copy of the current locale string. */ + strncpy(search_string, locale, sizeof(search_string)); + search_string[sizeof(search_string)-1] = '\0'; + + /* Chop off the encoding part, and make it lower case. */ + ptr = search_string; + while (*ptr) { + *ptr = tolower(*ptr); + if (*ptr == '.') { + *ptr = '\0'; + break; + } + ptr++; + } + + /* Find the entry which matches the string code of our locale. */ + lang = lang_map; + while (lang->string_code) { + if (!strcmp(lang->string_code, search_string)) { + return lang->usb_code; + } + lang++; + } + + /* There was no match. Find with just the language only. */ + /* Chop off the variant. Chop it off at the '_'. */ + ptr = search_string; + while (*ptr) { + *ptr = tolower(*ptr); + if (*ptr == '_') { + *ptr = '\0'; + break; + } + ptr++; + } + +#if 0 /* TODO: Do we need this? */ + /* Find the entry which matches the string code of our language. */ + lang = lang_map; + while (lang->string_code) { + if (!strcmp(lang->string_code, search_string)) { + return lang->usb_code; + } + lang++; + } +#endif + + /* Found nothing. */ + return 0x0; +} + +#if defined(__cplusplus) && !defined(NAMESPACE) +} +#endif + +#ifdef NAMESPACE +} +#endif + +#endif /* SDL_JOYSTICK_HIDAPI */ diff --git a/Engine/lib/sdl/src/hidapi/libusb/hidusb.cpp b/Engine/lib/sdl/src/hidapi/libusb/hidusb.cpp new file mode 100644 index 000000000..500630616 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/libusb/hidusb.cpp @@ -0,0 +1,3 @@ + +#define NAMESPACE HIDUSB +#include "hid.c" diff --git a/Engine/lib/sdl/src/hidapi/linux/Makefile-manual b/Engine/lib/sdl/src/hidapi/linux/Makefile-manual new file mode 100644 index 000000000..04ce1de45 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/linux/Makefile-manual @@ -0,0 +1,49 @@ +########################################### +# Simple Makefile for HIDAPI test program +# +# Alan Ott +# Signal 11 Software +# 2010-06-01 +########################################### + +all: hidtest-hidraw libs + +libs: libhidapi-hidraw.so + +CC ?= gcc +CFLAGS ?= -Wall -g -fpic + +CXX ?= g++ +CXXFLAGS ?= -Wall -g -fpic + +LDFLAGS ?= -Wall -g + + +COBJS = hid.o +CPPOBJS = ../hidtest/hidtest.o +OBJS = $(COBJS) $(CPPOBJS) +LIBS_UDEV = `pkg-config libudev --libs` -lrt +LIBS = $(LIBS_UDEV) +INCLUDES ?= -I../hidapi `pkg-config libusb-1.0 --cflags` + + +# Console Test Program +hidtest-hidraw: $(COBJS) $(CPPOBJS) + $(CXX) $(LDFLAGS) $^ $(LIBS_UDEV) -o $@ + +# Shared Libs +libhidapi-hidraw.so: $(COBJS) + $(CC) $(LDFLAGS) $(LIBS_UDEV) -shared -fpic -Wl,-soname,$@.0 $^ -o $@ + +# Objects +$(COBJS): %.o: %.c + $(CC) $(CFLAGS) -c $(INCLUDES) $< -o $@ + +$(CPPOBJS): %.o: %.cpp + $(CXX) $(CXXFLAGS) -c $(INCLUDES) $< -o $@ + + +clean: + rm -f $(OBJS) hidtest-hidraw libhidapi-hidraw.so ../hidtest/hidtest.o + +.PHONY: clean libs diff --git a/Engine/lib/sdl/src/hidapi/linux/Makefile.am b/Engine/lib/sdl/src/hidapi/linux/Makefile.am new file mode 100644 index 000000000..230eeb75a --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/linux/Makefile.am @@ -0,0 +1,10 @@ +lib_LTLIBRARIES = libhidapi-hidraw.la +libhidapi_hidraw_la_SOURCES = hid.c +libhidapi_hidraw_la_LDFLAGS = $(LTLDFLAGS) +AM_CPPFLAGS = -I$(top_srcdir)/hidapi/ $(CFLAGS_HIDRAW) +libhidapi_hidraw_la_LIBADD = $(LIBS_HIDRAW) + +hdrdir = $(includedir)/hidapi +hdr_HEADERS = $(top_srcdir)/hidapi/hidapi.h + +EXTRA_DIST = Makefile-manual diff --git a/Engine/lib/sdl/src/hidapi/linux/README.txt b/Engine/lib/sdl/src/hidapi/linux/README.txt new file mode 100644 index 000000000..800669495 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/linux/README.txt @@ -0,0 +1,59 @@ + +There are two implementations of HIDAPI for Linux. One (linux/hid.c) uses the +Linux hidraw driver, and the other (libusb/hid.c) uses libusb. Which one you +use depends on your application. Complete functionality of the hidraw +version depends on patches to the Linux kernel which are not currently in +the mainline. These patches have to do with sending and receiving feature +reports. The libusb implementation uses libusb to talk directly to the +device, bypassing any Linux HID driver. The disadvantage of the libusb +version is that it will only work with USB devices, while the hidraw +implementation will work with Bluetooth devices as well. + +To use HIDAPI, simply drop either linux/hid.c or libusb/hid.c into your +application and build using the build parameters in the Makefile. + + +Libusb Implementation notes +---------------------------- +For the libusb implementation, libusb-1.0 must be installed. Libusb 1.0 is +different than the legacy libusb 0.1 which is installed on many systems. To +install libusb-1.0 on Ubuntu and other Debian-based systems, run: + sudo apt-get install libusb-1.0-0-dev + + +Hidraw Implementation notes +---------------------------- +For the hidraw implementation, libudev headers and libraries are required to +build hidapi programs. To install libudev libraries on Ubuntu, +and other Debian-based systems, run: + sudo apt-get install libudev-dev + +On Redhat-based systems, run the following as root: + yum install libudev-devel + +Unfortunately, the hidraw driver, which the linux version of hidapi is based +on, contains bugs in kernel versions < 2.6.36, which the client application +should be aware of. + +Bugs (hidraw implementation only): +----------------------------------- +On Kernel versions < 2.6.34, if your device uses numbered reports, an extra +byte will be returned at the beginning of all reports returned from read() +for hidraw devices. This is worked around in the libary. No action should be +necessary in the client library. + +On Kernel versions < 2.6.35, reports will only be sent using a Set_Report +transfer on the CONTROL endpoint. No data will ever be sent on an Interrupt +Out endpoint if one exists. This is fixed in 2.6.35. In 2.6.35, OUTPUT +reports will be sent to the device on the first INTERRUPT OUT endpoint if it +exists; If it does not exist, OUTPUT reports will be sent on the CONTROL +endpoint. + +On Kernel versions < 2.6.36, add an extra byte containing the report number +to sent reports if numbered reports are used, and the device does not +contain an INTERRPUT OUT endpoint for OUTPUT transfers. For example, if +your device uses numbered reports and wants to send {0x2 0xff 0xff 0xff} to +the device (0x2 is the report number), you must send {0x2 0x2 0xff 0xff +0xff}. If your device has the optional Interrupt OUT endpoint, this does not +apply (but really on 2.6.35 only, because 2.6.34 won't use the interrupt +out endpoint). diff --git a/Engine/lib/sdl/src/hidapi/linux/hid.c b/Engine/lib/sdl/src/hidapi/linux/hid.c new file mode 100644 index 000000000..b78e009f0 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/linux/hid.c @@ -0,0 +1,898 @@ +/******************************************************* + HIDAPI - Multi-Platform library for + communication with HID devices. + + Alan Ott + Signal 11 Software + + 8/22/2009 + Linux Version - 6/2/2009 + + Copyright 2009, All Rights Reserved. + + At the discretion of the user of this library, + this software may be licensed under the terms of the + GNU General Public License v3, a BSD-Style license, or the + original HIDAPI license as outlined in the LICENSE.txt, + LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt + files located at the root of the source distribution. + These files may also be found in the public source + code repository located at: + http://github.com/signal11/hidapi . +********************************************************/ +#include "../../SDL_internal.h" + +#ifdef SDL_JOYSTICK_HIDAPI + +#ifndef _GNU_SOURCE +#define _GNU_SOURCE /* needed for wcsdup() before glibc 2.10 */ +#endif + +/* C */ +#include +#include +#include +#include +#include + +/* Unix */ +#include +#include +#include +#include +#include +#include +#include + +/* Linux */ +#include +#include +#include +#include + +#include "hidapi.h" + +#ifdef NAMESPACE +namespace NAMESPACE +{ +#endif + +/* Definitions from linux/hidraw.h. Since these are new, some distros + may not have header files which contain them. */ +#ifndef HIDIOCSFEATURE +#define HIDIOCSFEATURE(len) _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x06, len) +#endif +#ifndef HIDIOCGFEATURE +#define HIDIOCGFEATURE(len) _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x07, len) +#endif + +/* USB HID device property names */ +const char *device_string_names[] = { + "manufacturer", + "product", + "serial", +}; + +/* Symbolic names for the properties above */ +enum device_string_id { + DEVICE_STRING_MANUFACTURER, + DEVICE_STRING_PRODUCT, + DEVICE_STRING_SERIAL, + + DEVICE_STRING_COUNT, +}; + +struct hid_device_ { + int device_handle; + int blocking; + int uses_numbered_reports; + int is_bluetooth; +}; + + +static __u32 kernel_version = 0; + +static __u32 detect_kernel_version(void) +{ + struct utsname name; + int major, minor, release; + int ret; + + uname(&name); + ret = sscanf(name.release, "%d.%d.%d", &major, &minor, &release); + if (ret == 3) { + return KERNEL_VERSION(major, minor, release); + } + + ret = sscanf(name.release, "%d.%d", &major, &minor); + if (ret == 2) { + return KERNEL_VERSION(major, minor, 0); + } + + printf("Couldn't determine kernel version from version string \"%s\"\n", name.release); + return 0; +} + +static hid_device *new_hid_device(void) +{ + hid_device *dev = (hid_device *)calloc(1, sizeof(hid_device)); + dev->device_handle = -1; + dev->blocking = 1; + dev->uses_numbered_reports = 0; + dev->is_bluetooth = 0; + + return dev; +} + + +/* The caller must free the returned string with free(). */ +static wchar_t *utf8_to_wchar_t(const char *utf8) +{ + wchar_t *ret = NULL; + + if (utf8) { + size_t wlen = mbstowcs(NULL, utf8, 0); + if ((size_t) -1 == wlen) { + return wcsdup(L""); + } + ret = (wchar_t *)calloc(wlen+1, sizeof(wchar_t)); + mbstowcs(ret, utf8, wlen+1); + ret[wlen] = 0x0000; + } + + return ret; +} + +/* Get an attribute value from a udev_device and return it as a whar_t + string. The returned string must be freed with free() when done.*/ +static wchar_t *copy_udev_string(struct udev_device *dev, const char *udev_name) +{ + return utf8_to_wchar_t(udev_device_get_sysattr_value(dev, udev_name)); +} + +/* uses_numbered_reports() returns 1 if report_descriptor describes a device + which contains numbered reports. */ +static int uses_numbered_reports(__u8 *report_descriptor, __u32 size) { + unsigned int i = 0; + int size_code; + int data_len, key_size; + + while (i < size) { + int key = report_descriptor[i]; + + /* Check for the Report ID key */ + if (key == 0x85/*Report ID*/) { + /* This device has a Report ID, which means it uses + numbered reports. */ + return 1; + } + + //printf("key: %02hhx\n", key); + + if ((key & 0xf0) == 0xf0) { + /* This is a Long Item. The next byte contains the + length of the data section (value) for this key. + See the HID specification, version 1.11, section + 6.2.2.3, titled "Long Items." */ + if (i+1 < size) + data_len = report_descriptor[i+1]; + else + data_len = 0; /* malformed report */ + key_size = 3; + } + else { + /* This is a Short Item. The bottom two bits of the + key contain the size code for the data section + (value) for this key. Refer to the HID + specification, version 1.11, section 6.2.2.2, + titled "Short Items." */ + size_code = key & 0x3; + switch (size_code) { + case 0: + case 1: + case 2: + data_len = size_code; + break; + case 3: + data_len = 4; + break; + default: + /* Can't ever happen since size_code is & 0x3 */ + data_len = 0; + break; + }; + key_size = 1; + } + + /* Skip over this key and it's associated data */ + i += data_len + key_size; + } + + /* Didn't find a Report ID key. Device doesn't use numbered reports. */ + return 0; +} + +/* + * The caller is responsible for free()ing the (newly-allocated) character + * strings pointed to by serial_number_utf8 and product_name_utf8 after use. + */ +static int +parse_uevent_info(const char *uevent, int *bus_type, + unsigned short *vendor_id, unsigned short *product_id, + char **serial_number_utf8, char **product_name_utf8) +{ + char *tmp = strdup(uevent); + char *saveptr = NULL; + char *line; + char *key; + char *value; + + int found_id = 0; + int found_serial = 0; + int found_name = 0; + + line = strtok_r(tmp, "\n", &saveptr); + while (line != NULL) { + /* line: "KEY=value" */ + key = line; + value = strchr(line, '='); + if (!value) { + goto next_line; + } + *value = '\0'; + value++; + + if (strcmp(key, "HID_ID") == 0) { + /** + * type vendor product + * HID_ID=0003:000005AC:00008242 + **/ + int ret = sscanf(value, "%x:%hx:%hx", bus_type, vendor_id, product_id); + if (ret == 3) { + found_id = 1; + } + } else if (strcmp(key, "HID_NAME") == 0) { + /* The caller has to free the product name */ + *product_name_utf8 = strdup(value); + found_name = 1; + } else if (strcmp(key, "HID_UNIQ") == 0) { + /* The caller has to free the serial number */ + *serial_number_utf8 = strdup(value); + found_serial = 1; + } + +next_line: + line = strtok_r(NULL, "\n", &saveptr); + } + + free(tmp); + return (found_id && found_name && found_serial); +} + +static int is_bluetooth(hid_device *dev) +{ + struct udev *udev; + struct udev_device *udev_dev, *hid_dev; + struct stat s; + int ret = -1; + + /* Create the udev object */ + udev = udev_new(); + if (!udev) { + printf("Can't create udev\n"); + return -1; + } + + /* Get the dev_t (major/minor numbers) from the file handle. */ + ret = fstat(dev->device_handle, &s); + if (-1 == ret) { + udev_unref(udev); + return ret; + } + + /* Open a udev device from the dev_t. 'c' means character device. */ + udev_dev = udev_device_new_from_devnum(udev, 'c', s.st_rdev); + if (udev_dev) { + hid_dev = udev_device_get_parent_with_subsystem_devtype( + udev_dev, + "hid", + NULL); + if (hid_dev) { + unsigned short dev_vid; + unsigned short dev_pid; + int bus_type; + char *serial_number_utf8 = NULL; + char *product_name_utf8 = NULL; + + ret = parse_uevent_info( + udev_device_get_sysattr_value(hid_dev, "uevent"), + &bus_type, + &dev_vid, + &dev_pid, + &serial_number_utf8, + &product_name_utf8); + free(serial_number_utf8); + free(product_name_utf8); + + ret = (bus_type == BUS_BLUETOOTH); + + /* hid_dev doesn't need to be (and can't be) unref'd. + I'm not sure why, but it'll throw double-free() errors. */ + } + udev_device_unref(udev_dev); + } + + udev_unref(udev); + + return ret; +} + + +static int get_device_string(hid_device *dev, enum device_string_id key, wchar_t *string, size_t maxlen) +{ + struct udev *udev; + struct udev_device *udev_dev, *parent, *hid_dev; + struct stat s; + int ret = -1; + char *serial_number_utf8 = NULL; + char *product_name_utf8 = NULL; + char *tmp; + + /* Create the udev object */ + udev = udev_new(); + if (!udev) { + printf("Can't create udev\n"); + return -1; + } + + /* Get the dev_t (major/minor numbers) from the file handle. */ + ret = fstat(dev->device_handle, &s); + if (-1 == ret) { + udev_unref(udev); + return ret; + } + /* Open a udev device from the dev_t. 'c' means character device. */ + udev_dev = udev_device_new_from_devnum(udev, 'c', s.st_rdev); + if (udev_dev) { + hid_dev = udev_device_get_parent_with_subsystem_devtype( + udev_dev, + "hid", + NULL); + if (hid_dev) { + unsigned short dev_vid; + unsigned short dev_pid; + int bus_type; + size_t retm; + + ret = parse_uevent_info( + udev_device_get_sysattr_value(hid_dev, "uevent"), + &bus_type, + &dev_vid, + &dev_pid, + &serial_number_utf8, + &product_name_utf8); + + if (bus_type == BUS_BLUETOOTH) { + switch (key) { + case DEVICE_STRING_MANUFACTURER: + wcsncpy(string, L"", maxlen); + ret = 0; + break; + case DEVICE_STRING_PRODUCT: + retm = mbstowcs(string, product_name_utf8, maxlen); + ret = (retm == (size_t)-1)? -1: 0; + break; + case DEVICE_STRING_SERIAL: + /* Bluetooth serial numbers are often the bluetooth device address + and we want that with the colons stripped out, which is the correct + serial number for PS4 controllers + */ + while ((tmp = strchr(serial_number_utf8, ':')) != NULL) { + memmove(tmp, tmp+1, strlen(tmp)); + } + retm = mbstowcs(string, serial_number_utf8, maxlen); + ret = (retm == (size_t)-1)? -1: 0; + break; + case DEVICE_STRING_COUNT: + default: + ret = -1; + break; + } + } + else { + /* This is a USB device. Find its parent USB Device node. */ + parent = udev_device_get_parent_with_subsystem_devtype( + udev_dev, + "usb", + "usb_device"); + if (parent) { + const char *str; + const char *key_str = NULL; + + if (key >= 0 && key < DEVICE_STRING_COUNT) { + key_str = device_string_names[key]; + } else { + ret = -1; + goto end; + } + + str = udev_device_get_sysattr_value(parent, key_str); + if (str) { + /* Convert the string from UTF-8 to wchar_t */ + retm = mbstowcs(string, str, maxlen); + ret = (retm == (size_t)-1)? -1: 0; + goto end; + } + } + } + } + } + +end: + free(serial_number_utf8); + free(product_name_utf8); + + udev_device_unref(udev_dev); + /* parent and hid_dev don't need to be (and can't be) unref'd. + I'm not sure why, but they'll throw double-free() errors. */ + udev_unref(udev); + + return ret; +} + +int HID_API_EXPORT hid_init(void) +{ + const char *locale; + + /* Set the locale if it's not set. */ + locale = setlocale(LC_CTYPE, NULL); + if (!locale) + setlocale(LC_CTYPE, ""); + + kernel_version = detect_kernel_version(); + + return 0; +} + +int HID_API_EXPORT hid_exit(void) +{ + /* Nothing to do for this in the Linux/hidraw implementation. */ + return 0; +} + + +struct hid_device_info HID_API_EXPORT *hid_enumerate(unsigned short vendor_id, unsigned short product_id) +{ + struct udev *udev; + struct udev_enumerate *enumerate; + struct udev_list_entry *devices, *dev_list_entry; + + struct hid_device_info *root = NULL; /* return object */ + struct hid_device_info *cur_dev = NULL; + struct hid_device_info *prev_dev = NULL; /* previous device */ + + hid_init(); + + /* Create the udev object */ + udev = udev_new(); + if (!udev) { + printf("Can't create udev\n"); + return NULL; + } + + /* Create a list of the devices in the 'hidraw' subsystem. */ + enumerate = udev_enumerate_new(udev); + udev_enumerate_add_match_subsystem(enumerate, "hidraw"); + udev_enumerate_scan_devices(enumerate); + devices = udev_enumerate_get_list_entry(enumerate); + /* For each item, see if it matches the vid/pid, and if so + create a udev_device record for it */ + udev_list_entry_foreach(dev_list_entry, devices) { + const char *sysfs_path; + const char *dev_path; + const char *str; + struct udev_device *raw_dev; /* The device's hidraw udev node. */ + struct udev_device *hid_dev; /* The device's HID udev node. */ + struct udev_device *usb_dev; /* The device's USB udev node. */ + struct udev_device *intf_dev; /* The device's interface (in the USB sense). */ + unsigned short dev_vid; + unsigned short dev_pid; + char *serial_number_utf8 = NULL; + char *product_name_utf8 = NULL; + int bus_type; + int result; + + /* Get the filename of the /sys entry for the device + and create a udev_device object (dev) representing it */ + sysfs_path = udev_list_entry_get_name(dev_list_entry); + raw_dev = udev_device_new_from_syspath(udev, sysfs_path); + dev_path = udev_device_get_devnode(raw_dev); + + hid_dev = udev_device_get_parent_with_subsystem_devtype( + raw_dev, + "hid", + NULL); + + if (!hid_dev) { + /* Unable to find parent hid device. */ + goto next; + } + + result = parse_uevent_info( + udev_device_get_sysattr_value(hid_dev, "uevent"), + &bus_type, + &dev_vid, + &dev_pid, + &serial_number_utf8, + &product_name_utf8); + + if (!result) { + /* parse_uevent_info() failed for at least one field. */ + goto next; + } + + if (bus_type != BUS_USB && bus_type != BUS_BLUETOOTH) { + /* We only know how to handle USB and BT devices. */ + goto next; + } + + if (access(dev_path, R_OK|W_OK) != 0) { + /* We can't open this device, ignore it */ + goto next; + } + + /* Check the VID/PID against the arguments */ + if ((vendor_id == 0x0 || vendor_id == dev_vid) && + (product_id == 0x0 || product_id == dev_pid)) { + struct hid_device_info *tmp; + + /* VID/PID match. Create the record. */ + tmp = (struct hid_device_info *)malloc(sizeof(struct hid_device_info)); + if (cur_dev) { + cur_dev->next = tmp; + } + else { + root = tmp; + } + prev_dev = cur_dev; + cur_dev = tmp; + + /* Fill out the record */ + cur_dev->next = NULL; + cur_dev->path = dev_path? strdup(dev_path): NULL; + + /* VID/PID */ + cur_dev->vendor_id = dev_vid; + cur_dev->product_id = dev_pid; + + /* Serial Number */ + cur_dev->serial_number = utf8_to_wchar_t(serial_number_utf8); + + /* Release Number */ + cur_dev->release_number = 0x0; + + /* Interface Number */ + cur_dev->interface_number = -1; + + switch (bus_type) { + case BUS_USB: + /* The device pointed to by raw_dev contains information about + the hidraw device. In order to get information about the + USB device, get the parent device with the + subsystem/devtype pair of "usb"/"usb_device". This will + be several levels up the tree, but the function will find + it. */ + usb_dev = udev_device_get_parent_with_subsystem_devtype( + raw_dev, + "usb", + "usb_device"); + + if (!usb_dev) { + /* Free this device */ + free(cur_dev->serial_number); + free(cur_dev->path); + free(cur_dev); + + /* Take it off the device list. */ + if (prev_dev) { + prev_dev->next = NULL; + cur_dev = prev_dev; + } + else { + cur_dev = root = NULL; + } + + goto next; + } + + /* Manufacturer and Product strings */ + cur_dev->manufacturer_string = copy_udev_string(usb_dev, device_string_names[DEVICE_STRING_MANUFACTURER]); + cur_dev->product_string = copy_udev_string(usb_dev, device_string_names[DEVICE_STRING_PRODUCT]); + + /* Release Number */ + str = udev_device_get_sysattr_value(usb_dev, "bcdDevice"); + cur_dev->release_number = (str)? strtol(str, NULL, 16): 0x0; + + /* Get a handle to the interface's udev node. */ + intf_dev = udev_device_get_parent_with_subsystem_devtype( + raw_dev, + "usb", + "usb_interface"); + if (intf_dev) { + str = udev_device_get_sysattr_value(intf_dev, "bInterfaceNumber"); + cur_dev->interface_number = (str)? strtol(str, NULL, 16): -1; + } + + break; + + case BUS_BLUETOOTH: + /* Manufacturer and Product strings */ + cur_dev->manufacturer_string = wcsdup(L""); + cur_dev->product_string = utf8_to_wchar_t(product_name_utf8); + + break; + + default: + /* Unknown device type - this should never happen, as we + * check for USB and Bluetooth devices above */ + break; + } + } + + next: + free(serial_number_utf8); + free(product_name_utf8); + udev_device_unref(raw_dev); + /* hid_dev, usb_dev and intf_dev don't need to be (and can't be) + unref()d. It will cause a double-free() error. I'm not + sure why. */ + } + /* Free the enumerator and udev objects. */ + udev_enumerate_unref(enumerate); + udev_unref(udev); + + return root; +} + +void HID_API_EXPORT hid_free_enumeration(struct hid_device_info *devs) +{ + struct hid_device_info *d = devs; + while (d) { + struct hid_device_info *next = d->next; + free(d->path); + free(d->serial_number); + free(d->manufacturer_string); + free(d->product_string); + free(d); + d = next; + } +} + +hid_device * hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number) +{ + struct hid_device_info *devs, *cur_dev; + const char *path_to_open = NULL; + hid_device *handle = NULL; + + devs = hid_enumerate(vendor_id, product_id); + cur_dev = devs; + while (cur_dev) { + if (cur_dev->vendor_id == vendor_id && + cur_dev->product_id == product_id) { + if (serial_number) { + if (wcscmp(serial_number, cur_dev->serial_number) == 0) { + path_to_open = cur_dev->path; + break; + } + } + else { + path_to_open = cur_dev->path; + break; + } + } + cur_dev = cur_dev->next; + } + + if (path_to_open) { + /* Open the device */ + handle = hid_open_path(path_to_open, 0); + } + + hid_free_enumeration(devs); + + return handle; +} + +hid_device * HID_API_EXPORT hid_open_path(const char *path, int bExclusive) +{ + hid_device *dev = NULL; + + hid_init(); + + dev = new_hid_device(); + + /* OPEN HERE */ + dev->device_handle = open(path, O_RDWR); + + /* If we have a good handle, return it. */ + if (dev->device_handle > 0) { + + /* Get the report descriptor */ + int res, desc_size = 0; + struct hidraw_report_descriptor rpt_desc; + + memset(&rpt_desc, 0x0, sizeof(rpt_desc)); + + /* Get Report Descriptor Size */ + res = ioctl(dev->device_handle, HIDIOCGRDESCSIZE, &desc_size); + if (res < 0) + perror("HIDIOCGRDESCSIZE"); + + + /* Get Report Descriptor */ + rpt_desc.size = desc_size; + res = ioctl(dev->device_handle, HIDIOCGRDESC, &rpt_desc); + if (res < 0) { + perror("HIDIOCGRDESC"); + } else { + /* Determine if this device uses numbered reports. */ + dev->uses_numbered_reports = + uses_numbered_reports(rpt_desc.value, + rpt_desc.size); + } + + dev->is_bluetooth = (is_bluetooth(dev) == 1); + + return dev; + } + else { + /* Unable to open any devices. */ + free(dev); + return NULL; + } +} + + +int HID_API_EXPORT hid_write(hid_device *dev, const unsigned char *data, size_t length) +{ + int bytes_written; + + bytes_written = write(dev->device_handle, data, length); + + return bytes_written; +} + + +int HID_API_EXPORT hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds) +{ + int bytes_read; + + if (milliseconds >= 0) { + /* Milliseconds is either 0 (non-blocking) or > 0 (contains + a valid timeout). In both cases we want to call poll() + and wait for data to arrive. Don't rely on non-blocking + operation (O_NONBLOCK) since some kernels don't seem to + properly report device disconnection through read() when + in non-blocking mode. */ + int ret; + struct pollfd fds; + + fds.fd = dev->device_handle; + fds.events = POLLIN; + fds.revents = 0; + ret = poll(&fds, 1, milliseconds); + if (ret == -1 || ret == 0) { + /* Error or timeout */ + return ret; + } + else { + /* Check for errors on the file descriptor. This will + indicate a device disconnection. */ + if (fds.revents & (POLLERR | POLLHUP | POLLNVAL)) + return -1; + } + } + + bytes_read = read(dev->device_handle, data, length); + if (bytes_read < 0 && (errno == EAGAIN || errno == EINPROGRESS)) + bytes_read = 0; + + if (bytes_read >= 0 && + kernel_version != 0 && + kernel_version < KERNEL_VERSION(2,6,34) && + dev->uses_numbered_reports) { + /* Work around a kernel bug. Chop off the first byte. */ + memmove(data, data+1, bytes_read); + bytes_read--; + } + + return bytes_read; +} + +int HID_API_EXPORT hid_read(hid_device *dev, unsigned char *data, size_t length) +{ + return hid_read_timeout(dev, data, length, (dev->blocking)? -1: 0); +} + +int HID_API_EXPORT hid_set_nonblocking(hid_device *dev, int nonblock) +{ + /* Do all non-blocking in userspace using poll(), since it looks + like there's a bug in the kernel in some versions where + read() will not return -1 on disconnection of the USB device */ + + dev->blocking = !nonblock; + return 0; /* Success */ +} + + +int HID_API_EXPORT hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length) +{ + int res; + + res = ioctl(dev->device_handle, HIDIOCSFEATURE(length), data); + if (res < 0) + perror("ioctl (SFEATURE)"); + + return res; +} + +int HID_API_EXPORT hid_get_feature_report(hid_device *dev, unsigned char *data, size_t length) +{ + int res; + + /* It looks like HIDIOCGFEATURE() on Bluetooth devices doesn't return the report number */ + if (dev->is_bluetooth) { + data[1] = data[0]; + ++data; + --length; + } + res = ioctl(dev->device_handle, HIDIOCGFEATURE(length), data); + if (res < 0) + perror("ioctl (GFEATURE)"); + else if (dev->is_bluetooth) + ++res; + + return res; +} + + +void HID_API_EXPORT hid_close(hid_device *dev) +{ + if (!dev) + return; + close(dev->device_handle); + free(dev); +} + + +int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_device_string(dev, DEVICE_STRING_MANUFACTURER, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_product_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_device_string(dev, DEVICE_STRING_PRODUCT, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_device_string(dev, DEVICE_STRING_SERIAL, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *dev, int string_index, wchar_t *string, size_t maxlen) +{ + return -1; +} + + +HID_API_EXPORT const wchar_t * HID_API_CALL hid_error(hid_device *dev) +{ + return NULL; +} + +#ifdef NAMESPACE +} +#endif + +#endif /* SDL_JOYSTICK_HIDAPI */ diff --git a/Engine/lib/sdl/src/hidapi/linux/hid.cpp b/Engine/lib/sdl/src/hidapi/linux/hid.cpp new file mode 100644 index 000000000..841f34fb4 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/linux/hid.cpp @@ -0,0 +1,333 @@ +//=================== Copyright Valve Corporation, All rights reserved. ======= +// +// Purpose: A wrapper around both the libusb and hidraw versions of HIDAPI +// +// The libusb version doesn't support Bluetooth, but not all Linux +// distributions allow access to /dev/hidraw* +// +// This merges the two, at a small performance cost, until distributions +// have granted access to /dev/hidraw* +// +//============================================================================= + +#define NAMESPACE HIDRAW +#include "../hidapi/hidapi.h" +#undef NAMESPACE +#undef HIDAPI_H__ + +#define NAMESPACE HIDUSB +#include "../hidapi/hidapi.h" +#undef NAMESPACE +#undef HIDAPI_H__ + +#include "../hidapi/hidapi.h" + +#include "../../../public/tier1/utlvector.h" +#include "../../../public/tier1/utlhashmap.h" + + +template +void CopyHIDDeviceInfo( T *pSrc, struct hid_device_info *pDst ) +{ + pDst->path = pSrc->path ? strdup( pSrc->path ) : NULL; + pDst->vendor_id = pSrc->vendor_id; + pDst->product_id = pSrc->product_id; + pDst->serial_number = pSrc->serial_number ? wcsdup( pSrc->serial_number ) : NULL; + pDst->release_number = pSrc->release_number; + pDst->manufacturer_string = pSrc->manufacturer_string ? wcsdup( pSrc->manufacturer_string ) : NULL; + pDst->product_string = pSrc->product_string ? wcsdup( pSrc->product_string ) : NULL; + pDst->usage_page = pSrc->usage_page; + pDst->usage = pSrc->usage; + pDst->interface_number = pSrc->interface_number; + pDst->next = NULL; +} + +extern "C" +{ + +enum EHIDAPIType +{ + k_EHIDAPIUnknown, + k_EHIDAPIRAW, + k_EHIDAPIUSB +}; + +static CUtlHashMap s_hashDeviceToAPI; + +static EHIDAPIType GetAPIForDevice( hid_device *pDevice ) +{ + int iIndex = s_hashDeviceToAPI.Find( (uintptr_t)pDevice ); + if ( iIndex != -1 ) + { + return s_hashDeviceToAPI[ iIndex ]; + } + return k_EHIDAPIUnknown; +} + +struct hid_device_info HID_API_EXPORT * HID_API_CALL hid_enumerate(unsigned short vendor_id, unsigned short product_id) +{ + struct HIDUSB::hid_device_info *usb_devs = HIDUSB::hid_enumerate( vendor_id, product_id ); + struct HIDUSB::hid_device_info *usb_dev; + struct HIDRAW::hid_device_info *raw_devs = HIDRAW::hid_enumerate( vendor_id, product_id ); + struct HIDRAW::hid_device_info *raw_dev; + struct hid_device_info *devs = NULL, *last = NULL, *new_dev; + + for ( usb_dev = usb_devs; usb_dev; usb_dev = usb_dev->next ) + { + bool bFound = false; + for ( raw_dev = raw_devs; raw_dev; raw_dev = raw_dev->next ) + { + if ( usb_dev->vendor_id == raw_dev->vendor_id && usb_dev->product_id == raw_dev->product_id ) + { + bFound = true; + break; + } + } + +//printf("%s USB device VID/PID 0x%.4x/0x%.4x, %ls %ls\n", bFound ? "Found matching" : "Added new", usb_dev->vendor_id, usb_dev->product_id, usb_dev->manufacturer_string, usb_dev->product_string ); + + if ( !bFound ) + { + new_dev = new struct hid_device_info; + CopyHIDDeviceInfo( usb_dev, new_dev ); + + if ( last ) + { + last->next = new_dev; + } + else + { + devs = new_dev; + } + last = new_dev; + } + } + HIDUSB::hid_free_enumeration( usb_devs ); + + for ( raw_dev = raw_devs; raw_dev; raw_dev = raw_dev->next ) + { + new_dev = new struct hid_device_info; + CopyHIDDeviceInfo( raw_dev, new_dev ); + new_dev->next = NULL; + + if ( last ) + { + last->next = new_dev; + } + else + { + devs = new_dev; + } + last = new_dev; + } + HIDRAW::hid_free_enumeration( raw_devs ); + + return devs; +} + +void HID_API_EXPORT HID_API_CALL hid_free_enumeration(struct hid_device_info *devs) +{ + while ( devs ) + { + struct hid_device_info *next = devs->next; + free( devs->path ); + free( devs->serial_number ); + free( devs->manufacturer_string ); + free( devs->product_string ); + delete devs; + devs = next; + } +} + +HID_API_EXPORT hid_device * HID_API_CALL hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number) +{ + hid_device *pDevice = NULL; + if ( ( pDevice = (hid_device *)HIDRAW::hid_open( vendor_id, product_id, serial_number ) ) != NULL ) + { + s_hashDeviceToAPI.Insert( (uintptr_t)pDevice, k_EHIDAPIRAW ); + return pDevice; + } + if ( ( pDevice = (hid_device *)HIDUSB::hid_open( vendor_id, product_id, serial_number ) ) != NULL ) + { + s_hashDeviceToAPI.Insert( (uintptr_t)pDevice, k_EHIDAPIUSB ); + return pDevice; + } + return NULL; +} + +HID_API_EXPORT hid_device * HID_API_CALL hid_open_path(const char *path, int bExclusive) +{ + hid_device *pDevice = NULL; + if ( ( pDevice = (hid_device *)HIDRAW::hid_open_path( path, bExclusive ) ) != NULL ) + { + s_hashDeviceToAPI.Insert( (uintptr_t)pDevice, k_EHIDAPIRAW ); + return pDevice; + } + if ( ( pDevice = (hid_device *)HIDUSB::hid_open_path( path, bExclusive ) ) != NULL ) + { + s_hashDeviceToAPI.Insert( (uintptr_t)pDevice, k_EHIDAPIUSB ); + return pDevice; + } + return NULL; +} + +int HID_API_EXPORT HID_API_CALL hid_write(hid_device *device, const unsigned char *data, size_t length) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_write( (HIDRAW::hid_device*)device, data, length ); + case k_EHIDAPIUSB: + return HIDUSB::hid_write( (HIDUSB::hid_device*)device, data, length ); + default: + return -1; + } +} + +int HID_API_EXPORT HID_API_CALL hid_read_timeout(hid_device *device, unsigned char *data, size_t length, int milliseconds) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_read_timeout( (HIDRAW::hid_device*)device, data, length, milliseconds ); + case k_EHIDAPIUSB: + return HIDUSB::hid_read_timeout( (HIDUSB::hid_device*)device, data, length, milliseconds ); + default: + return -1; + } +} + +int HID_API_EXPORT HID_API_CALL hid_read(hid_device *device, unsigned char *data, size_t length) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_read( (HIDRAW::hid_device*)device, data, length ); + case k_EHIDAPIUSB: + return HIDUSB::hid_read( (HIDUSB::hid_device*)device, data, length ); + default: + return -1; + } +} + +int HID_API_EXPORT HID_API_CALL hid_set_nonblocking(hid_device *device, int nonblock) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_set_nonblocking( (HIDRAW::hid_device*)device, nonblock ); + case k_EHIDAPIUSB: + return HIDUSB::hid_set_nonblocking( (HIDUSB::hid_device*)device, nonblock ); + default: + return -1; + } +} + +int HID_API_EXPORT HID_API_CALL hid_send_feature_report(hid_device *device, const unsigned char *data, size_t length) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_send_feature_report( (HIDRAW::hid_device*)device, data, length ); + case k_EHIDAPIUSB: + return HIDUSB::hid_send_feature_report( (HIDUSB::hid_device*)device, data, length ); + default: + return -1; + } +} + +int HID_API_EXPORT HID_API_CALL hid_get_feature_report(hid_device *device, unsigned char *data, size_t length) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_get_feature_report( (HIDRAW::hid_device*)device, data, length ); + case k_EHIDAPIUSB: + return HIDUSB::hid_get_feature_report( (HIDUSB::hid_device*)device, data, length ); + default: + return -1; + } +} + +void HID_API_EXPORT HID_API_CALL hid_close(hid_device *device) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + HIDRAW::hid_close( (HIDRAW::hid_device*)device ); + break; + case k_EHIDAPIUSB: + HIDUSB::hid_close( (HIDUSB::hid_device*)device ); + break; + default: + break; + } + s_hashDeviceToAPI.Remove( (uintptr_t)device ); +} + +int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *device, wchar_t *string, size_t maxlen) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_get_manufacturer_string( (HIDRAW::hid_device*)device, string, maxlen ); + case k_EHIDAPIUSB: + return HIDUSB::hid_get_manufacturer_string( (HIDUSB::hid_device*)device, string, maxlen ); + default: + return -1; + } +} + +int HID_API_EXPORT_CALL hid_get_product_string(hid_device *device, wchar_t *string, size_t maxlen) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_get_product_string( (HIDRAW::hid_device*)device, string, maxlen ); + case k_EHIDAPIUSB: + return HIDUSB::hid_get_product_string( (HIDUSB::hid_device*)device, string, maxlen ); + default: + return -1; + } +} + +int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *device, wchar_t *string, size_t maxlen) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_get_serial_number_string( (HIDRAW::hid_device*)device, string, maxlen ); + case k_EHIDAPIUSB: + return HIDUSB::hid_get_serial_number_string( (HIDUSB::hid_device*)device, string, maxlen ); + default: + return -1; + } +} + +int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *device, int string_index, wchar_t *string, size_t maxlen) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_get_indexed_string( (HIDRAW::hid_device*)device, string_index, string, maxlen ); + case k_EHIDAPIUSB: + return HIDUSB::hid_get_indexed_string( (HIDUSB::hid_device*)device, string_index, string, maxlen ); + default: + return -1; + } +} + +HID_API_EXPORT const wchar_t* HID_API_CALL hid_error(hid_device *device) +{ + switch ( GetAPIForDevice( device ) ) + { + case k_EHIDAPIRAW: + return HIDRAW::hid_error( (HIDRAW::hid_device*)device ); + case k_EHIDAPIUSB: + return HIDUSB::hid_error( (HIDUSB::hid_device*)device ); + default: + return NULL; + } +} + +} diff --git a/Engine/lib/sdl/src/hidapi/linux/hidraw.cpp b/Engine/lib/sdl/src/hidapi/linux/hidraw.cpp new file mode 100644 index 000000000..1bf6fad24 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/linux/hidraw.cpp @@ -0,0 +1,3 @@ + +#define NAMESPACE HIDRAW +#include "hid.c" diff --git a/Engine/lib/sdl/src/hidapi/m4/ax_pthread.m4 b/Engine/lib/sdl/src/hidapi/m4/ax_pthread.m4 new file mode 100644 index 000000000..d90de34d1 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/m4/ax_pthread.m4 @@ -0,0 +1,309 @@ +# =========================================================================== +# http://www.gnu.org/software/autoconf-archive/ax_pthread.html +# =========================================================================== +# +# SYNOPSIS +# +# AX_PTHREAD([ACTION-IF-FOUND[, ACTION-IF-NOT-FOUND]]) +# +# DESCRIPTION +# +# This macro figures out how to build C programs using POSIX threads. It +# sets the PTHREAD_LIBS output variable to the threads library and linker +# flags, and the PTHREAD_CFLAGS output variable to any special C compiler +# flags that are needed. (The user can also force certain compiler +# flags/libs to be tested by setting these environment variables.) +# +# Also sets PTHREAD_CC to any special C compiler that is needed for +# multi-threaded programs (defaults to the value of CC otherwise). (This +# is necessary on AIX to use the special cc_r compiler alias.) +# +# NOTE: You are assumed to not only compile your program with these flags, +# but also link it with them as well. e.g. you should link with +# $PTHREAD_CC $CFLAGS $PTHREAD_CFLAGS $LDFLAGS ... $PTHREAD_LIBS $LIBS +# +# If you are only building threads programs, you may wish to use these +# variables in your default LIBS, CFLAGS, and CC: +# +# LIBS="$PTHREAD_LIBS $LIBS" +# CFLAGS="$CFLAGS $PTHREAD_CFLAGS" +# CC="$PTHREAD_CC" +# +# In addition, if the PTHREAD_CREATE_JOINABLE thread-attribute constant +# has a nonstandard name, defines PTHREAD_CREATE_JOINABLE to that name +# (e.g. PTHREAD_CREATE_UNDETACHED on AIX). +# +# Also HAVE_PTHREAD_PRIO_INHERIT is defined if pthread is found and the +# PTHREAD_PRIO_INHERIT symbol is defined when compiling with +# PTHREAD_CFLAGS. +# +# ACTION-IF-FOUND is a list of shell commands to run if a threads library +# is found, and ACTION-IF-NOT-FOUND is a list of commands to run it if it +# is not found. If ACTION-IF-FOUND is not specified, the default action +# will define HAVE_PTHREAD. +# +# Please let the authors know if this macro fails on any platform, or if +# you have any other suggestions or comments. This macro was based on work +# by SGJ on autoconf scripts for FFTW (http://www.fftw.org/) (with help +# from M. Frigo), as well as ac_pthread and hb_pthread macros posted by +# Alejandro Forero Cuervo to the autoconf macro repository. We are also +# grateful for the helpful feedback of numerous users. +# +# Updated for Autoconf 2.68 by Daniel Richard G. +# +# LICENSE +# +# Copyright (c) 2008 Steven G. Johnson +# Copyright (c) 2011 Daniel Richard G. +# +# This program is free software: you can redistribute it and/or modify it +# under the terms of the GNU General Public License as published by the +# Free Software Foundation, either version 3 of the License, or (at your +# option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +# Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program. If not, see . +# +# As a special exception, the respective Autoconf Macro's copyright owner +# gives unlimited permission to copy, distribute and modify the configure +# scripts that are the output of Autoconf when processing the Macro. You +# need not follow the terms of the GNU General Public License when using +# or distributing such scripts, even though portions of the text of the +# Macro appear in them. The GNU General Public License (GPL) does govern +# all other use of the material that constitutes the Autoconf Macro. +# +# This special exception to the GPL applies to versions of the Autoconf +# Macro released by the Autoconf Archive. When you make and distribute a +# modified version of the Autoconf Macro, you may extend this special +# exception to the GPL to apply to your modified version as well. + +#serial 18 + +AU_ALIAS([ACX_PTHREAD], [AX_PTHREAD]) +AC_DEFUN([AX_PTHREAD], [ +AC_REQUIRE([AC_CANONICAL_HOST]) +AC_LANG_PUSH([C]) +ax_pthread_ok=no + +# We used to check for pthread.h first, but this fails if pthread.h +# requires special compiler flags (e.g. on True64 or Sequent). +# It gets checked for in the link test anyway. + +# First of all, check if the user has set any of the PTHREAD_LIBS, +# etcetera environment variables, and if threads linking works using +# them: +if test x"$PTHREAD_LIBS$PTHREAD_CFLAGS" != x; then + save_CFLAGS="$CFLAGS" + CFLAGS="$CFLAGS $PTHREAD_CFLAGS" + save_LIBS="$LIBS" + LIBS="$PTHREAD_LIBS $LIBS" + AC_MSG_CHECKING([for pthread_join in LIBS=$PTHREAD_LIBS with CFLAGS=$PTHREAD_CFLAGS]) + AC_TRY_LINK_FUNC(pthread_join, ax_pthread_ok=yes) + AC_MSG_RESULT($ax_pthread_ok) + if test x"$ax_pthread_ok" = xno; then + PTHREAD_LIBS="" + PTHREAD_CFLAGS="" + fi + LIBS="$save_LIBS" + CFLAGS="$save_CFLAGS" +fi + +# We must check for the threads library under a number of different +# names; the ordering is very important because some systems +# (e.g. DEC) have both -lpthread and -lpthreads, where one of the +# libraries is broken (non-POSIX). + +# Create a list of thread flags to try. Items starting with a "-" are +# C compiler flags, and other items are library names, except for "none" +# which indicates that we try without any flags at all, and "pthread-config" +# which is a program returning the flags for the Pth emulation library. + +ax_pthread_flags="pthreads none -Kthread -kthread lthread -pthread -pthreads -mthreads pthread --thread-safe -mt pthread-config" + +# The ordering *is* (sometimes) important. Some notes on the +# individual items follow: + +# pthreads: AIX (must check this before -lpthread) +# none: in case threads are in libc; should be tried before -Kthread and +# other compiler flags to prevent continual compiler warnings +# -Kthread: Sequent (threads in libc, but -Kthread needed for pthread.h) +# -kthread: FreeBSD kernel threads (preferred to -pthread since SMP-able) +# lthread: LinuxThreads port on FreeBSD (also preferred to -pthread) +# -pthread: Linux/gcc (kernel threads), BSD/gcc (userland threads) +# -pthreads: Solaris/gcc +# -mthreads: Mingw32/gcc, Lynx/gcc +# -mt: Sun Workshop C (may only link SunOS threads [-lthread], but it +# doesn't hurt to check since this sometimes defines pthreads too; +# also defines -D_REENTRANT) +# ... -mt is also the pthreads flag for HP/aCC +# pthread: Linux, etcetera +# --thread-safe: KAI C++ +# pthread-config: use pthread-config program (for GNU Pth library) + +case ${host_os} in + solaris*) + + # On Solaris (at least, for some versions), libc contains stubbed + # (non-functional) versions of the pthreads routines, so link-based + # tests will erroneously succeed. (We need to link with -pthreads/-mt/ + # -lpthread.) (The stubs are missing pthread_cleanup_push, or rather + # a function called by this macro, so we could check for that, but + # who knows whether they'll stub that too in a future libc.) So, + # we'll just look for -pthreads and -lpthread first: + + ax_pthread_flags="-pthreads pthread -mt -pthread $ax_pthread_flags" + ;; + + darwin*) + ax_pthread_flags="-pthread $ax_pthread_flags" + ;; +esac + +if test x"$ax_pthread_ok" = xno; then +for flag in $ax_pthread_flags; do + + case $flag in + none) + AC_MSG_CHECKING([whether pthreads work without any flags]) + ;; + + -*) + AC_MSG_CHECKING([whether pthreads work with $flag]) + PTHREAD_CFLAGS="$flag" + ;; + + pthread-config) + AC_CHECK_PROG(ax_pthread_config, pthread-config, yes, no) + if test x"$ax_pthread_config" = xno; then continue; fi + PTHREAD_CFLAGS="`pthread-config --cflags`" + PTHREAD_LIBS="`pthread-config --ldflags` `pthread-config --libs`" + ;; + + *) + AC_MSG_CHECKING([for the pthreads library -l$flag]) + PTHREAD_LIBS="-l$flag" + ;; + esac + + save_LIBS="$LIBS" + save_CFLAGS="$CFLAGS" + LIBS="$PTHREAD_LIBS $LIBS" + CFLAGS="$CFLAGS $PTHREAD_CFLAGS" + + # Check for various functions. We must include pthread.h, + # since some functions may be macros. (On the Sequent, we + # need a special flag -Kthread to make this header compile.) + # We check for pthread_join because it is in -lpthread on IRIX + # while pthread_create is in libc. We check for pthread_attr_init + # due to DEC craziness with -lpthreads. We check for + # pthread_cleanup_push because it is one of the few pthread + # functions on Solaris that doesn't have a non-functional libc stub. + # We try pthread_create on general principles. + AC_LINK_IFELSE([AC_LANG_PROGRAM([#include + static void routine(void *a) { a = 0; } + static void *start_routine(void *a) { return a; }], + [pthread_t th; pthread_attr_t attr; + pthread_create(&th, 0, start_routine, 0); + pthread_join(th, 0); + pthread_attr_init(&attr); + pthread_cleanup_push(routine, 0); + pthread_cleanup_pop(0) /* ; */])], + [ax_pthread_ok=yes], + []) + + LIBS="$save_LIBS" + CFLAGS="$save_CFLAGS" + + AC_MSG_RESULT($ax_pthread_ok) + if test "x$ax_pthread_ok" = xyes; then + break; + fi + + PTHREAD_LIBS="" + PTHREAD_CFLAGS="" +done +fi + +# Various other checks: +if test "x$ax_pthread_ok" = xyes; then + save_LIBS="$LIBS" + LIBS="$PTHREAD_LIBS $LIBS" + save_CFLAGS="$CFLAGS" + CFLAGS="$CFLAGS $PTHREAD_CFLAGS" + + # Detect AIX lossage: JOINABLE attribute is called UNDETACHED. + AC_MSG_CHECKING([for joinable pthread attribute]) + attr_name=unknown + for attr in PTHREAD_CREATE_JOINABLE PTHREAD_CREATE_UNDETACHED; do + AC_LINK_IFELSE([AC_LANG_PROGRAM([#include ], + [int attr = $attr; return attr /* ; */])], + [attr_name=$attr; break], + []) + done + AC_MSG_RESULT($attr_name) + if test "$attr_name" != PTHREAD_CREATE_JOINABLE; then + AC_DEFINE_UNQUOTED(PTHREAD_CREATE_JOINABLE, $attr_name, + [Define to necessary symbol if this constant + uses a non-standard name on your system.]) + fi + + AC_MSG_CHECKING([if more special flags are required for pthreads]) + flag=no + case ${host_os} in + aix* | freebsd* | darwin*) flag="-D_THREAD_SAFE";; + osf* | hpux*) flag="-D_REENTRANT";; + solaris*) + if test "$GCC" = "yes"; then + flag="-D_REENTRANT" + else + flag="-mt -D_REENTRANT" + fi + ;; + esac + AC_MSG_RESULT(${flag}) + if test "x$flag" != xno; then + PTHREAD_CFLAGS="$flag $PTHREAD_CFLAGS" + fi + + AC_CACHE_CHECK([for PTHREAD_PRIO_INHERIT], + ax_cv_PTHREAD_PRIO_INHERIT, [ + AC_LINK_IFELSE([ + AC_LANG_PROGRAM([[#include ]], [[int i = PTHREAD_PRIO_INHERIT;]])], + [ax_cv_PTHREAD_PRIO_INHERIT=yes], + [ax_cv_PTHREAD_PRIO_INHERIT=no]) + ]) + AS_IF([test "x$ax_cv_PTHREAD_PRIO_INHERIT" = "xyes"], + AC_DEFINE([HAVE_PTHREAD_PRIO_INHERIT], 1, [Have PTHREAD_PRIO_INHERIT.])) + + LIBS="$save_LIBS" + CFLAGS="$save_CFLAGS" + + # More AIX lossage: must compile with xlc_r or cc_r + if test x"$GCC" != xyes; then + AC_CHECK_PROGS(PTHREAD_CC, xlc_r cc_r, ${CC}) + else + PTHREAD_CC=$CC + fi +else + PTHREAD_CC="$CC" +fi + +AC_SUBST(PTHREAD_LIBS) +AC_SUBST(PTHREAD_CFLAGS) +AC_SUBST(PTHREAD_CC) + +# Finally, execute ACTION-IF-FOUND/ACTION-IF-NOT-FOUND: +if test x"$ax_pthread_ok" = xyes; then + ifelse([$1],,AC_DEFINE(HAVE_PTHREAD,1,[Define if you have POSIX threads libraries and header files.]),[$1]) + : +else + ax_pthread_ok=no + $2 +fi +AC_LANG_POP +])dnl AX_PTHREAD diff --git a/Engine/lib/sdl/src/hidapi/m4/pkg.m4 b/Engine/lib/sdl/src/hidapi/m4/pkg.m4 new file mode 100644 index 000000000..0048a3fa0 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/m4/pkg.m4 @@ -0,0 +1,157 @@ +# pkg.m4 - Macros to locate and utilise pkg-config. -*- Autoconf -*- +# +# Copyright © 2004 Scott James Remnant . +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +# General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# As a special exception to the GNU General Public License, if you +# distribute this file as part of a program that contains a +# configuration script generated by Autoconf, you may include it under +# the same distribution terms that you use for the rest of that program. + +# PKG_PROG_PKG_CONFIG([MIN-VERSION]) +# ---------------------------------- +AC_DEFUN([PKG_PROG_PKG_CONFIG], +[m4_pattern_forbid([^_?PKG_[A-Z_]+$]) +m4_pattern_allow([^PKG_CONFIG(_PATH)?$]) +AC_ARG_VAR([PKG_CONFIG], [path to pkg-config utility])dnl +if test "x$ac_cv_env_PKG_CONFIG_set" != "xset"; then + AC_PATH_TOOL([PKG_CONFIG], [pkg-config]) +fi +if test -n "$PKG_CONFIG"; then + _pkg_min_version=m4_default([$1], [0.9.0]) + AC_MSG_CHECKING([pkg-config is at least version $_pkg_min_version]) + if $PKG_CONFIG --atleast-pkgconfig-version $_pkg_min_version; then + AC_MSG_RESULT([yes]) + else + AC_MSG_RESULT([no]) + PKG_CONFIG="" + fi + +fi[]dnl +])# PKG_PROG_PKG_CONFIG + +# PKG_CHECK_EXISTS(MODULES, [ACTION-IF-FOUND], [ACTION-IF-NOT-FOUND]) +# +# Check to see whether a particular set of modules exists. Similar +# to PKG_CHECK_MODULES(), but does not set variables or print errors. +# +# +# Similar to PKG_CHECK_MODULES, make sure that the first instance of +# this or PKG_CHECK_MODULES is called, or make sure to call +# PKG_CHECK_EXISTS manually +# -------------------------------------------------------------- +AC_DEFUN([PKG_CHECK_EXISTS], +[AC_REQUIRE([PKG_PROG_PKG_CONFIG])dnl +if test -n "$PKG_CONFIG" && \ + AC_RUN_LOG([$PKG_CONFIG --exists --print-errors "$1"]); then + m4_ifval([$2], [$2], [:]) +m4_ifvaln([$3], [else + $3])dnl +fi]) + + +# _PKG_CONFIG([VARIABLE], [COMMAND], [MODULES]) +# --------------------------------------------- +m4_define([_PKG_CONFIG], +[if test -n "$PKG_CONFIG"; then + if test -n "$$1"; then + pkg_cv_[]$1="$$1" + else + PKG_CHECK_EXISTS([$3], + [pkg_cv_[]$1=`$PKG_CONFIG --[]$2 "$3" 2>/dev/null`], + [pkg_failed=yes]) + fi +else + pkg_failed=untried +fi[]dnl +])# _PKG_CONFIG + +# _PKG_SHORT_ERRORS_SUPPORTED +# ----------------------------- +AC_DEFUN([_PKG_SHORT_ERRORS_SUPPORTED], +[AC_REQUIRE([PKG_PROG_PKG_CONFIG]) +if $PKG_CONFIG --atleast-pkgconfig-version 0.20; then + _pkg_short_errors_supported=yes +else + _pkg_short_errors_supported=no +fi[]dnl +])# _PKG_SHORT_ERRORS_SUPPORTED + + +# PKG_CHECK_MODULES(VARIABLE-PREFIX, MODULES, [ACTION-IF-FOUND], +# [ACTION-IF-NOT-FOUND]) +# +# +# Note that if there is a possibility the first call to +# PKG_CHECK_MODULES might not happen, you should be sure to include an +# explicit call to PKG_PROG_PKG_CONFIG in your configure.ac +# +# +# -------------------------------------------------------------- +AC_DEFUN([PKG_CHECK_MODULES], +[AC_REQUIRE([PKG_PROG_PKG_CONFIG])dnl +AC_ARG_VAR([$1][_CFLAGS], [C compiler flags for $1, overriding pkg-config])dnl +AC_ARG_VAR([$1][_LIBS], [linker flags for $1, overriding pkg-config])dnl + +pkg_failed=no +AC_MSG_CHECKING([for $1]) + +_PKG_CONFIG([$1][_CFLAGS], [cflags], [$2]) +_PKG_CONFIG([$1][_LIBS], [libs], [$2]) + +m4_define([_PKG_TEXT], [Alternatively, you may set the environment variables $1[]_CFLAGS +and $1[]_LIBS to avoid the need to call pkg-config. +See the pkg-config man page for more details.]) + +if test $pkg_failed = yes; then + _PKG_SHORT_ERRORS_SUPPORTED + if test $_pkg_short_errors_supported = yes; then + $1[]_PKG_ERRORS=`$PKG_CONFIG --short-errors --errors-to-stdout --print-errors "$2"` + else + $1[]_PKG_ERRORS=`$PKG_CONFIG --errors-to-stdout --print-errors "$2"` + fi + # Put the nasty error message in config.log where it belongs + echo "$$1[]_PKG_ERRORS" >&AS_MESSAGE_LOG_FD + + ifelse([$4], , [AC_MSG_ERROR(dnl +[Package requirements ($2) were not met: + +$$1_PKG_ERRORS + +Consider adjusting the PKG_CONFIG_PATH environment variable if you +installed software in a non-standard prefix. + +_PKG_TEXT +])], + [AC_MSG_RESULT([no]) + $4]) +elif test $pkg_failed = untried; then + ifelse([$4], , [AC_MSG_FAILURE(dnl +[The pkg-config script could not be found or is too old. Make sure it +is in your PATH or set the PKG_CONFIG environment variable to the full +path to pkg-config. + +_PKG_TEXT + +To get pkg-config, see .])], + [$4]) +else + $1[]_CFLAGS=$pkg_cv_[]$1[]_CFLAGS + $1[]_LIBS=$pkg_cv_[]$1[]_LIBS + AC_MSG_RESULT([yes]) + ifelse([$3], , :, [$3]) +fi[]dnl +])# PKG_CHECK_MODULES diff --git a/Engine/lib/sdl/src/hidapi/mac/Makefile-manual b/Engine/lib/sdl/src/hidapi/mac/Makefile-manual new file mode 100644 index 000000000..5399b5a70 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/mac/Makefile-manual @@ -0,0 +1,32 @@ +########################################### +# Simple Makefile for HIDAPI test program +# +# Alan Ott +# Signal 11 Software +# 2010-07-03 +########################################### + +all: hidtest + +CC=gcc +CXX=g++ +COBJS=hid.o +CPPOBJS=../hidtest/hidtest.o +OBJS=$(COBJS) $(CPPOBJS) +CFLAGS+=-I../hidapi -Wall -g -c +LIBS=-framework IOKit -framework CoreFoundation + + +hidtest: $(OBJS) + g++ -Wall -g $^ $(LIBS) -o hidtest + +$(COBJS): %.o: %.c + $(CC) $(CFLAGS) $< -o $@ + +$(CPPOBJS): %.o: %.cpp + $(CXX) $(CFLAGS) $< -o $@ + +clean: + rm -f *.o hidtest $(CPPOBJS) + +.PHONY: clean diff --git a/Engine/lib/sdl/src/hidapi/mac/Makefile.am b/Engine/lib/sdl/src/hidapi/mac/Makefile.am new file mode 100644 index 000000000..23d96e08f --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/mac/Makefile.am @@ -0,0 +1,9 @@ +lib_LTLIBRARIES = libhidapi.la +libhidapi_la_SOURCES = hid.c +libhidapi_la_LDFLAGS = $(LTLDFLAGS) +AM_CPPFLAGS = -I$(top_srcdir)/hidapi/ + +hdrdir = $(includedir)/hidapi +hdr_HEADERS = $(top_srcdir)/hidapi/hidapi.h + +EXTRA_DIST = Makefile-manual diff --git a/Engine/lib/sdl/src/hidapi/mac/hid.c b/Engine/lib/sdl/src/hidapi/mac/hid.c new file mode 100644 index 000000000..d462d26e3 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/mac/hid.c @@ -0,0 +1,1191 @@ +/******************************************************* + HIDAPI - Multi-Platform library for + communication with HID devices. + + Alan Ott + Signal 11 Software + + 2010-07-03 + + Copyright 2010, All Rights Reserved. + + At the discretion of the user of this library, + this software may be licensed under the terms of the + GNU Public License v3, a BSD-Style license, or the + original HIDAPI license as outlined in the LICENSE.txt, + LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt + files located at the root of the source distribution. + These files may also be found in the public source + code repository located at: + http://github.com/signal11/hidapi . + ********************************************************/ +#include "../../SDL_internal.h" + +#ifdef SDL_JOYSTICK_HIDAPI + +/* See Apple Technical Note TN2187 for details on IOHidManager. */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hidapi.h" + +/* Barrier implementation because Mac OSX doesn't have pthread_barrier. + It also doesn't have clock_gettime(). So much for POSIX and SUSv2. + This implementation came from Brent Priddy and was posted on + StackOverflow. It is used with his permission. */ +typedef int pthread_barrierattr_t; +typedef struct pthread_barrier { + pthread_mutex_t mutex; + pthread_cond_t cond; + int count; + int trip_count; +} pthread_barrier_t; + +static int pthread_barrier_init(pthread_barrier_t *barrier, const pthread_barrierattr_t *attr, unsigned int count) +{ + if(count == 0) { + errno = EINVAL; + return -1; + } + + if(pthread_mutex_init(&barrier->mutex, 0) < 0) { + return -1; + } + if(pthread_cond_init(&barrier->cond, 0) < 0) { + pthread_mutex_destroy(&barrier->mutex); + return -1; + } + barrier->trip_count = count; + barrier->count = 0; + + return 0; +} + +static int pthread_barrier_destroy(pthread_barrier_t *barrier) +{ + pthread_cond_destroy(&barrier->cond); + pthread_mutex_destroy(&barrier->mutex); + return 0; +} + +static int pthread_barrier_wait(pthread_barrier_t *barrier) +{ + pthread_mutex_lock(&barrier->mutex); + ++(barrier->count); + if(barrier->count >= barrier->trip_count) + { + barrier->count = 0; + pthread_cond_broadcast(&barrier->cond); + pthread_mutex_unlock(&barrier->mutex); + return 1; + } + else + { + pthread_cond_wait(&barrier->cond, &(barrier->mutex)); + pthread_mutex_unlock(&barrier->mutex); + return 0; + } +} + +static int return_data(hid_device *dev, unsigned char *data, size_t length); + +/* Linked List of input reports received from the device. */ +struct input_report { + uint8_t *data; + size_t len; + struct input_report *next; +}; + +struct hid_device_ { + IOHIDDeviceRef device_handle; + int blocking; + int uses_numbered_reports; + int disconnected; + CFStringRef run_loop_mode; + CFRunLoopRef run_loop; + CFRunLoopSourceRef source; + uint8_t *input_report_buf; + CFIndex max_input_report_len; + struct input_report *input_reports; + + pthread_t thread; + pthread_mutex_t mutex; /* Protects input_reports */ + pthread_cond_t condition; + pthread_barrier_t barrier; /* Ensures correct startup sequence */ + pthread_barrier_t shutdown_barrier; /* Ensures correct shutdown sequence */ + int shutdown_thread; + + hid_device *next; +}; + +/* Static list of all the devices open. This way when a device gets + disconnected, its hid_device structure can be marked as disconnected + from hid_device_removal_callback(). */ +static hid_device *device_list = NULL; +static pthread_mutex_t device_list_mutex = PTHREAD_MUTEX_INITIALIZER; + +static hid_device *new_hid_device(void) +{ + hid_device *dev = (hid_device*)calloc(1, sizeof(hid_device)); + dev->device_handle = NULL; + dev->blocking = 1; + dev->uses_numbered_reports = 0; + dev->disconnected = 0; + dev->run_loop_mode = NULL; + dev->run_loop = NULL; + dev->source = NULL; + dev->input_report_buf = NULL; + dev->input_reports = NULL; + dev->shutdown_thread = 0; + dev->next = NULL; + + /* Thread objects */ + pthread_mutex_init(&dev->mutex, NULL); + pthread_cond_init(&dev->condition, NULL); + pthread_barrier_init(&dev->barrier, NULL, 2); + pthread_barrier_init(&dev->shutdown_barrier, NULL, 2); + + /* Add the new record to the device_list. */ + pthread_mutex_lock(&device_list_mutex); + if (!device_list) + device_list = dev; + else { + hid_device *d = device_list; + while (d) { + if (!d->next) { + d->next = dev; + break; + } + d = d->next; + } + } + pthread_mutex_unlock(&device_list_mutex); + + return dev; +} + +static void free_hid_device(hid_device *dev) +{ + if (!dev) + return; + + /* Delete any input reports still left over. */ + struct input_report *rpt = dev->input_reports; + while (rpt) { + struct input_report *next = rpt->next; + free(rpt->data); + free(rpt); + rpt = next; + } + + /* Free the string and the report buffer. The check for NULL + is necessary here as CFRelease() doesn't handle NULL like + free() and others do. */ + if (dev->run_loop_mode) + CFRelease(dev->run_loop_mode); + if (dev->source) + CFRelease(dev->source); + free(dev->input_report_buf); + + /* Clean up the thread objects */ + pthread_barrier_destroy(&dev->shutdown_barrier); + pthread_barrier_destroy(&dev->barrier); + pthread_cond_destroy(&dev->condition); + pthread_mutex_destroy(&dev->mutex); + + /* Remove it from the device list. */ + pthread_mutex_lock(&device_list_mutex); + hid_device *d = device_list; + if (d == dev) { + device_list = d->next; + } + else { + while (d) { + if (d->next == dev) { + d->next = d->next->next; + break; + } + + d = d->next; + } + } + pthread_mutex_unlock(&device_list_mutex); + + /* Free the structure itself. */ + free(dev); +} + +static IOHIDManagerRef hid_mgr = 0x0; + + +#if 0 +static void register_error(hid_device *device, const char *op) +{ + +} +#endif + + +static int32_t get_int_property(IOHIDDeviceRef device, CFStringRef key) +{ + CFTypeRef ref; + int32_t value; + + ref = IOHIDDeviceGetProperty(device, key); + if (ref) { + if (CFGetTypeID(ref) == CFNumberGetTypeID()) { + CFNumberGetValue((CFNumberRef) ref, kCFNumberSInt32Type, &value); + return value; + } + } + return 0; +} + +static unsigned short get_vendor_id(IOHIDDeviceRef device) +{ + return get_int_property(device, CFSTR(kIOHIDVendorIDKey)); +} + +static unsigned short get_product_id(IOHIDDeviceRef device) +{ + return get_int_property(device, CFSTR(kIOHIDProductIDKey)); +} + + +static int32_t get_max_report_length(IOHIDDeviceRef device) +{ + return get_int_property(device, CFSTR(kIOHIDMaxInputReportSizeKey)); +} + +static int get_string_property(IOHIDDeviceRef device, CFStringRef prop, wchar_t *buf, size_t len) +{ + CFStringRef str; + + if (!len) + return 0; + + str = (CFStringRef)IOHIDDeviceGetProperty(device, prop); + + buf[0] = 0; + + if (str) { + len --; + + CFIndex str_len = CFStringGetLength(str); + CFRange range; + range.location = 0; + range.length = (str_len > len)? len: str_len; + CFIndex used_buf_len; + CFIndex chars_copied; + chars_copied = CFStringGetBytes(str, + range, + kCFStringEncodingUTF32LE, + (char)'?', + FALSE, + (UInt8*)buf, + len, + &used_buf_len); + + buf[chars_copied] = 0; + return (int)chars_copied; + } + else + return 0; + +} + +static int get_string_property_utf8(IOHIDDeviceRef device, CFStringRef prop, char *buf, size_t len) +{ + CFStringRef str; + if (!len) + return 0; + + str = (CFStringRef)IOHIDDeviceGetProperty(device, prop); + + buf[0] = 0; + + if (str) { + len--; + + CFIndex str_len = CFStringGetLength(str); + CFRange range; + range.location = 0; + range.length = (str_len > len)? len: str_len; + CFIndex used_buf_len; + CFIndex chars_copied; + chars_copied = CFStringGetBytes(str, + range, + kCFStringEncodingUTF8, + (char)'?', + FALSE, + (UInt8*)buf, + len, + &used_buf_len); + + buf[chars_copied] = 0; + return (int)used_buf_len; + } + else + return 0; +} + + +static int get_serial_number(IOHIDDeviceRef device, wchar_t *buf, size_t len) +{ + return get_string_property(device, CFSTR(kIOHIDSerialNumberKey), buf, len); +} + +static int get_manufacturer_string(IOHIDDeviceRef device, wchar_t *buf, size_t len) +{ + return get_string_property(device, CFSTR(kIOHIDManufacturerKey), buf, len); +} + +static int get_product_string(IOHIDDeviceRef device, wchar_t *buf, size_t len) +{ + return get_string_property(device, CFSTR(kIOHIDProductKey), buf, len); +} + + +/* Implementation of wcsdup() for Mac. */ +static wchar_t *dup_wcs(const wchar_t *s) +{ + size_t len = wcslen(s); + wchar_t *ret = (wchar_t *)malloc((len+1)*sizeof(wchar_t)); + wcscpy(ret, s); + + return ret; +} + + +static int make_path(IOHIDDeviceRef device, char *buf, size_t len) +{ + int res; + unsigned short vid, pid; + char transport[32]; + + buf[0] = '\0'; + + res = get_string_property_utf8( + device, CFSTR(kIOHIDTransportKey), + transport, sizeof(transport)); + + if (!res) + return -1; + + vid = get_vendor_id(device); + pid = get_product_id(device); + + res = snprintf(buf, len, "%s_%04hx_%04hx_%p", + transport, vid, pid, device); + + + buf[len-1] = '\0'; + return res+1; +} + +/* Initialize the IOHIDManager. Return 0 for success and -1 for failure. */ +static int init_hid_manager(void) +{ + + /* Initialize all the HID Manager Objects */ + hid_mgr = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); + if (hid_mgr) { + IOHIDManagerSetDeviceMatching(hid_mgr, NULL); + IOHIDManagerScheduleWithRunLoop(hid_mgr, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); + return 0; + } + + return -1; +} + +/* Initialize the IOHIDManager if necessary. This is the public function, and + it is safe to call this function repeatedly. Return 0 for success and -1 + for failure. */ +int HID_API_EXPORT hid_init(void) +{ + if (!hid_mgr) { + return init_hid_manager(); + } + + /* Already initialized. */ + return 0; +} + +int HID_API_EXPORT hid_exit(void) +{ + if (hid_mgr) { + /* Close the HID manager. */ + IOHIDManagerClose(hid_mgr, kIOHIDOptionsTypeNone); + CFRelease(hid_mgr); + hid_mgr = NULL; + } + + return 0; +} + +static void process_pending_events() { + SInt32 res; + do { + res = CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0.001, FALSE); + } while(res != kCFRunLoopRunFinished && res != kCFRunLoopRunTimedOut); +} + +struct hid_device_info HID_API_EXPORT *hid_enumerate(unsigned short vendor_id, unsigned short product_id) +{ + struct hid_device_info *root = NULL; // return object + struct hid_device_info *cur_dev = NULL; + CFIndex num_devices; + int i; + + setlocale(LC_ALL,""); + + /* Set up the HID Manager if it hasn't been done */ + if (hid_init() < 0) + return NULL; + + /* give the IOHIDManager a chance to update itself */ + process_pending_events(); + + /* Get a list of the Devices */ + CFSetRef device_set = IOHIDManagerCopyDevices(hid_mgr); + if (!device_set) + return NULL; + + /* Convert the list into a C array so we can iterate easily. */ + num_devices = CFSetGetCount(device_set); + if (!num_devices) { + CFRelease(device_set); + return NULL; + } + IOHIDDeviceRef *device_array = (IOHIDDeviceRef*)calloc(num_devices, sizeof(IOHIDDeviceRef)); + CFSetGetValues(device_set, (const void **) device_array); + + /* Iterate over each device, making an entry for it. */ + for (i = 0; i < num_devices; i++) { + unsigned short dev_vid; + unsigned short dev_pid; +#define BUF_LEN 256 + wchar_t buf[BUF_LEN]; + char cbuf[BUF_LEN]; + + IOHIDDeviceRef dev = device_array[i]; + + if (!dev) { + continue; + } + dev_vid = get_vendor_id(dev); + dev_pid = get_product_id(dev); + + /* Check the VID/PID against the arguments */ + if ((vendor_id == 0x0 && product_id == 0x0) || + (vendor_id == dev_vid && product_id == dev_pid)) { + struct hid_device_info *tmp; + size_t len; + + /* VID/PID match. Create the record. */ + tmp = (struct hid_device_info *)malloc(sizeof(struct hid_device_info)); + if (cur_dev) { + cur_dev->next = tmp; + } + else { + root = tmp; + } + cur_dev = tmp; + + // Get the Usage Page and Usage for this device. + cur_dev->usage_page = get_int_property(dev, CFSTR(kIOHIDPrimaryUsagePageKey)); + cur_dev->usage = get_int_property(dev, CFSTR(kIOHIDPrimaryUsageKey)); + + /* Fill out the record */ + cur_dev->next = NULL; + len = make_path(dev, cbuf, sizeof(cbuf)); + cur_dev->path = strdup(cbuf); + + /* Serial Number */ + get_serial_number(dev, buf, BUF_LEN); + cur_dev->serial_number = dup_wcs(buf); + + /* Manufacturer and Product strings */ + get_manufacturer_string(dev, buf, BUF_LEN); + cur_dev->manufacturer_string = dup_wcs(buf); + get_product_string(dev, buf, BUF_LEN); + cur_dev->product_string = dup_wcs(buf); + + /* VID/PID */ + cur_dev->vendor_id = dev_vid; + cur_dev->product_id = dev_pid; + + /* Release Number */ + cur_dev->release_number = get_int_property(dev, CFSTR(kIOHIDVersionNumberKey)); + + /* Interface Number (Unsupported on Mac)*/ + cur_dev->interface_number = -1; + } + } + + free(device_array); + CFRelease(device_set); + + return root; +} + +void HID_API_EXPORT hid_free_enumeration(struct hid_device_info *devs) +{ + /* This function is identical to the Linux version. Platform independent. */ + struct hid_device_info *d = devs; + while (d) { + struct hid_device_info *next = d->next; + free(d->path); + free(d->serial_number); + free(d->manufacturer_string); + free(d->product_string); + free(d); + d = next; + } +} + +hid_device * HID_API_EXPORT hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number) +{ + /* This function is identical to the Linux version. Platform independent. */ + struct hid_device_info *devs, *cur_dev; + const char *path_to_open = NULL; + hid_device * handle = NULL; + + devs = hid_enumerate(vendor_id, product_id); + cur_dev = devs; + while (cur_dev) { + if (cur_dev->vendor_id == vendor_id && + cur_dev->product_id == product_id) { + if (serial_number) { + if (wcscmp(serial_number, cur_dev->serial_number) == 0) { + path_to_open = cur_dev->path; + break; + } + } + else { + path_to_open = cur_dev->path; + break; + } + } + cur_dev = cur_dev->next; + } + + if (path_to_open) { + /* Open the device */ + handle = hid_open_path(path_to_open, 0); + } + + hid_free_enumeration(devs); + + return handle; +} + +static void hid_device_removal_callback(void *context, IOReturn result, + void *sender, IOHIDDeviceRef dev_ref) +{ + /* Stop the Run Loop for this device. */ + pthread_mutex_lock(&device_list_mutex); + hid_device *d = device_list; + while (d) { + if (d->device_handle == dev_ref) { + d->disconnected = 1; + CFRunLoopStop(d->run_loop); + } + + d = d->next; + } + pthread_mutex_unlock(&device_list_mutex); +} + +/* The Run Loop calls this function for each input report received. + This function puts the data into a linked list to be picked up by + hid_read(). */ +static void hid_report_callback(void *context, IOReturn result, void *sender, + IOHIDReportType report_type, uint32_t report_id, + uint8_t *report, CFIndex report_length) +{ + struct input_report *rpt; + hid_device *dev = (hid_device *)context; + + /* Make a new Input Report object */ + rpt = (struct input_report *)calloc(1, sizeof(struct input_report)); + rpt->data = (uint8_t *)calloc(1, report_length); + memcpy(rpt->data, report, report_length); + rpt->len = report_length; + rpt->next = NULL; + + /* Lock this section */ + pthread_mutex_lock(&dev->mutex); + + /* Attach the new report object to the end of the list. */ + if (dev->input_reports == NULL) { + /* The list is empty. Put it at the root. */ + dev->input_reports = rpt; + } + else { + /* Find the end of the list and attach. */ + struct input_report *cur = dev->input_reports; + int num_queued = 0; + while (cur->next != NULL) { + cur = cur->next; + num_queued++; + } + cur->next = rpt; + + /* Pop one off if we've reached 30 in the queue. This + way we don't grow forever if the user never reads + anything from the device. */ + if (num_queued > 30) { + return_data(dev, NULL, 0); + } + } + + /* Signal a waiting thread that there is data. */ + pthread_cond_signal(&dev->condition); + + /* Unlock */ + pthread_mutex_unlock(&dev->mutex); + +} + +/* This gets called when the read_thred's run loop gets signaled by + hid_close(), and serves to stop the read_thread's run loop. */ +static void perform_signal_callback(void *context) +{ + hid_device *dev = (hid_device *)context; + CFRunLoopStop(dev->run_loop); //TODO: CFRunLoopGetCurrent() +} + +static void *read_thread(void *param) +{ + hid_device *dev = (hid_device *)param; + + /* Move the device's run loop to this thread. */ + IOHIDDeviceScheduleWithRunLoop(dev->device_handle, CFRunLoopGetCurrent(), dev->run_loop_mode); + + /* Create the RunLoopSource which is used to signal the + event loop to stop when hid_close() is called. */ + CFRunLoopSourceContext ctx; + memset(&ctx, 0, sizeof(ctx)); + ctx.version = 0; + ctx.info = dev; + ctx.perform = &perform_signal_callback; + dev->source = CFRunLoopSourceCreate(kCFAllocatorDefault, 0/*order*/, &ctx); + CFRunLoopAddSource(CFRunLoopGetCurrent(), dev->source, dev->run_loop_mode); + + /* Store off the Run Loop so it can be stopped from hid_close() + and on device disconnection. */ + dev->run_loop = CFRunLoopGetCurrent(); + + /* Notify the main thread that the read thread is up and running. */ + pthread_barrier_wait(&dev->barrier); + + /* Run the Event Loop. CFRunLoopRunInMode() will dispatch HID input + reports into the hid_report_callback(). */ + SInt32 code; + while (!dev->shutdown_thread && !dev->disconnected) { + code = CFRunLoopRunInMode(dev->run_loop_mode, 1000/*sec*/, FALSE); + /* Return if the device has been disconnected */ + if (code == kCFRunLoopRunFinished) { + dev->disconnected = 1; + break; + } + + + /* Break if The Run Loop returns Finished or Stopped. */ + if (code != kCFRunLoopRunTimedOut && + code != kCFRunLoopRunHandledSource) { + /* There was some kind of error. Setting + shutdown seems to make sense, but + there may be something else more appropriate */ + dev->shutdown_thread = 1; + break; + } + } + + /* Now that the read thread is stopping, Wake any threads which are + waiting on data (in hid_read_timeout()). Do this under a mutex to + make sure that a thread which is about to go to sleep waiting on + the condition acutally will go to sleep before the condition is + signaled. */ + pthread_mutex_lock(&dev->mutex); + pthread_cond_broadcast(&dev->condition); + pthread_mutex_unlock(&dev->mutex); + + /* Wait here until hid_close() is called and makes it past + the call to CFRunLoopWakeUp(). This thread still needs to + be valid when that function is called on the other thread. */ + pthread_barrier_wait(&dev->shutdown_barrier); + + return NULL; +} + +hid_device * HID_API_EXPORT hid_open_path(const char *path, int bExclusive) +{ + int i; + hid_device *dev = NULL; + CFIndex num_devices; + + dev = new_hid_device(); + + /* Set up the HID Manager if it hasn't been done */ + if (hid_init() < 0) + return NULL; + + /* give the IOHIDManager a chance to update itself */ + process_pending_events(); + + CFSetRef device_set = IOHIDManagerCopyDevices(hid_mgr); + + num_devices = CFSetGetCount(device_set); + IOHIDDeviceRef *device_array = (IOHIDDeviceRef *)calloc(num_devices, sizeof(IOHIDDeviceRef)); + CFSetGetValues(device_set, (const void **) device_array); + for (i = 0; i < num_devices; i++) { + char cbuf[BUF_LEN]; + size_t len; + IOHIDDeviceRef os_dev = device_array[i]; + + len = make_path(os_dev, cbuf, sizeof(cbuf)); + if (!strcmp(cbuf, path)) { + // Matched Paths. Open this Device. + IOReturn ret = IOHIDDeviceOpen(os_dev, kIOHIDOptionsTypeNone); + if (ret == kIOReturnSuccess) { + char str[32]; + + free(device_array); + CFRelease(device_set); + dev->device_handle = os_dev; + + /* Create the buffers for receiving data */ + dev->max_input_report_len = (CFIndex) get_max_report_length(os_dev); + dev->input_report_buf = (uint8_t *)calloc(dev->max_input_report_len, sizeof(uint8_t)); + + /* Create the Run Loop Mode for this device. + printing the reference seems to work. */ + sprintf(str, "HIDAPI_%p", os_dev); + dev->run_loop_mode = + CFStringCreateWithCString(NULL, str, kCFStringEncodingASCII); + + /* Attach the device to a Run Loop */ + IOHIDDeviceRegisterInputReportCallback( + os_dev, dev->input_report_buf, dev->max_input_report_len, + &hid_report_callback, dev); + IOHIDManagerRegisterDeviceRemovalCallback(hid_mgr, hid_device_removal_callback, NULL); + + /* Start the read thread */ + pthread_create(&dev->thread, NULL, read_thread, dev); + + /* Wait here for the read thread to be initialized. */ + pthread_barrier_wait(&dev->barrier); + + return dev; + } + else { + goto return_error; + } + } + } + +return_error: + free(device_array); + CFRelease(device_set); + free_hid_device(dev); + return NULL; +} + +static int set_report(hid_device *dev, IOHIDReportType type, const unsigned char *data, size_t length) +{ + const char *pass_through_magic = "MAGIC0"; + size_t pass_through_magic_length = strlen(pass_through_magic); + unsigned char report_id = data[0]; + const unsigned char *data_to_send; + size_t length_to_send; + IOReturn res; + + /* Return if the device has been disconnected. */ + if (dev->disconnected) + return -1; + + if (report_id == 0x0) { + /* Not using numbered Reports. + Don't send the report number. */ + data_to_send = data+1; + length_to_send = length-1; + } + else if (length > 6 && memcmp(data, pass_through_magic, pass_through_magic_length) == 0) { + report_id = data[pass_through_magic_length]; + data_to_send = data+pass_through_magic_length; + length_to_send = length-pass_through_magic_length; + } + else { + /* Using numbered Reports. + Send the Report Number */ + data_to_send = data; + length_to_send = length; + } + + if (!dev->disconnected) { + res = IOHIDDeviceSetReport(dev->device_handle, + type, + report_id, /* Report ID*/ + data_to_send, length_to_send); + + if (res == kIOReturnSuccess) { + return (int)length; + } + else if (res == kIOReturnUnsupported) { + /*printf("kIOReturnUnsupported\n");*/ + return -1; + } + else { + /*printf("0x%x\n", res);*/ + return -1; + } + } + + return -1; +} + +int HID_API_EXPORT hid_write(hid_device *dev, const unsigned char *data, size_t length) +{ + return set_report(dev, kIOHIDReportTypeOutput, data, length); +} + +/* Helper function, so that this isn't duplicated in hid_read(). */ +static int return_data(hid_device *dev, unsigned char *data, size_t length) +{ + /* Copy the data out of the linked list item (rpt) into the + return buffer (data), and delete the liked list item. */ + struct input_report *rpt = dev->input_reports; + size_t len = (length < rpt->len)? length: rpt->len; + memcpy(data, rpt->data, len); + dev->input_reports = rpt->next; + free(rpt->data); + free(rpt); + return (int)len; +} + +static int cond_wait(const hid_device *dev, pthread_cond_t *cond, pthread_mutex_t *mutex) +{ + while (!dev->input_reports) { + int res = pthread_cond_wait(cond, mutex); + if (res != 0) + return res; + + /* A res of 0 means we may have been signaled or it may + be a spurious wakeup. Check to see that there's acutally + data in the queue before returning, and if not, go back + to sleep. See the pthread_cond_timedwait() man page for + details. */ + + if (dev->shutdown_thread || dev->disconnected) + return -1; + } + + return 0; +} + +static int cond_timedwait(const hid_device *dev, pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime) +{ + while (!dev->input_reports) { + int res = pthread_cond_timedwait(cond, mutex, abstime); + if (res != 0) + return res; + + /* A res of 0 means we may have been signaled or it may + be a spurious wakeup. Check to see that there's acutally + data in the queue before returning, and if not, go back + to sleep. See the pthread_cond_timedwait() man page for + details. */ + + if (dev->shutdown_thread || dev->disconnected) + return -1; + } + + return 0; + +} + +int HID_API_EXPORT hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds) +{ + int bytes_read = -1; + + /* Lock the access to the report list. */ + pthread_mutex_lock(&dev->mutex); + + /* There's an input report queued up. Return it. */ + if (dev->input_reports) { + /* Return the first one */ + bytes_read = return_data(dev, data, length); + goto ret; + } + + /* Return if the device has been disconnected. */ + if (dev->disconnected) { + bytes_read = -1; + goto ret; + } + + if (dev->shutdown_thread) { + /* This means the device has been closed (or there + has been an error. An error code of -1 should + be returned. */ + bytes_read = -1; + goto ret; + } + + /* There is no data. Go to sleep and wait for data. */ + + if (milliseconds == -1) { + /* Blocking */ + int res; + res = cond_wait(dev, &dev->condition, &dev->mutex); + if (res == 0) + bytes_read = return_data(dev, data, length); + else { + /* There was an error, or a device disconnection. */ + bytes_read = -1; + } + } + else if (milliseconds > 0) { + /* Non-blocking, but called with timeout. */ + int res; + struct timespec ts; + struct timeval tv; + gettimeofday(&tv, NULL); + TIMEVAL_TO_TIMESPEC(&tv, &ts); + ts.tv_sec += milliseconds / 1000; + ts.tv_nsec += (milliseconds % 1000) * 1000000; + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec++; + ts.tv_nsec -= 1000000000L; + } + + res = cond_timedwait(dev, &dev->condition, &dev->mutex, &ts); + if (res == 0) + bytes_read = return_data(dev, data, length); + else if (res == ETIMEDOUT) + bytes_read = 0; + else + bytes_read = -1; + } + else { + /* Purely non-blocking */ + bytes_read = 0; + } + +ret: + /* Unlock */ + pthread_mutex_unlock(&dev->mutex); + return bytes_read; +} + +int HID_API_EXPORT hid_read(hid_device *dev, unsigned char *data, size_t length) +{ + return hid_read_timeout(dev, data, length, (dev->blocking)? -1: 0); +} + +int HID_API_EXPORT hid_set_nonblocking(hid_device *dev, int nonblock) +{ + /* All Nonblocking operation is handled by the library. */ + dev->blocking = !nonblock; + + return 0; +} + +int HID_API_EXPORT hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length) +{ + return set_report(dev, kIOHIDReportTypeFeature, data, length); +} + +int HID_API_EXPORT hid_get_feature_report(hid_device *dev, unsigned char *data, size_t length) +{ + CFIndex len = length; + IOReturn res; + + /* Return if the device has been unplugged. */ + if (dev->disconnected) + return -1; + + int skipped_report_id = 0; + int report_number = data[0]; + if (report_number == 0x0) { + /* Offset the return buffer by 1, so that the report ID + will remain in byte 0. */ + data++; + len--; + skipped_report_id = 1; + } + + res = IOHIDDeviceGetReport(dev->device_handle, + kIOHIDReportTypeFeature, + report_number, /* Report ID */ + data, &len); + if (res != kIOReturnSuccess) + return -1; + + if (skipped_report_id) + len++; + + return (int)len; +} + + +void HID_API_EXPORT hid_close(hid_device *dev) +{ + if (!dev) + return; + + /* Disconnect the report callback before close. */ + if (!dev->disconnected) { + IOHIDDeviceRegisterInputReportCallback( + dev->device_handle, dev->input_report_buf, dev->max_input_report_len, + NULL, dev); + IOHIDManagerRegisterDeviceRemovalCallback(hid_mgr, NULL, dev); + IOHIDDeviceUnscheduleFromRunLoop(dev->device_handle, dev->run_loop, dev->run_loop_mode); + IOHIDDeviceScheduleWithRunLoop(dev->device_handle, CFRunLoopGetMain(), kCFRunLoopDefaultMode); + } + + /* Cause read_thread() to stop. */ + dev->shutdown_thread = 1; + + /* Wake up the run thread's event loop so that the thread can exit. */ + CFRunLoopSourceSignal(dev->source); + CFRunLoopWakeUp(dev->run_loop); + + /* Notify the read thread that it can shut down now. */ + pthread_barrier_wait(&dev->shutdown_barrier); + + /* Wait for read_thread() to end. */ + pthread_join(dev->thread, NULL); + + /* Close the OS handle to the device, but only if it's not + been unplugged. If it's been unplugged, then calling + IOHIDDeviceClose() will crash. */ + if (!dev->disconnected) { + IOHIDDeviceClose(dev->device_handle, kIOHIDOptionsTypeNone); + } + + /* Clear out the queue of received reports. */ + pthread_mutex_lock(&dev->mutex); + while (dev->input_reports) { + return_data(dev, NULL, 0); + } + pthread_mutex_unlock(&dev->mutex); + + free_hid_device(dev); +} + +int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_manufacturer_string(dev->device_handle, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_product_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_product_string(dev->device_handle, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_serial_number(dev->device_handle, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *dev, int string_index, wchar_t *string, size_t maxlen) +{ + // TODO: + + return 0; +} + + +HID_API_EXPORT const wchar_t * HID_API_CALL hid_error(hid_device *dev) +{ + // TODO: + + return NULL; +} + + + + + + +#if 0 +static int32_t get_location_id(IOHIDDeviceRef device) +{ + return get_int_property(device, CFSTR(kIOHIDLocationIDKey)); +} + +static int32_t get_usage(IOHIDDeviceRef device) +{ + int32_t res; + res = get_int_property(device, CFSTR(kIOHIDDeviceUsageKey)); + if (!res) + res = get_int_property(device, CFSTR(kIOHIDPrimaryUsageKey)); + return res; +} + +static int32_t get_usage_page(IOHIDDeviceRef device) +{ + int32_t res; + res = get_int_property(device, CFSTR(kIOHIDDeviceUsagePageKey)); + if (!res) + res = get_int_property(device, CFSTR(kIOHIDPrimaryUsagePageKey)); + return res; +} + +static int get_transport(IOHIDDeviceRef device, wchar_t *buf, size_t len) +{ + return get_string_property(device, CFSTR(kIOHIDTransportKey), buf, len); +} + + +int main(void) +{ + IOHIDManagerRef mgr; + int i; + + mgr = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); + IOHIDManagerSetDeviceMatching(mgr, NULL); + IOHIDManagerOpen(mgr, kIOHIDOptionsTypeNone); + + CFSetRef device_set = IOHIDManagerCopyDevices(mgr); + + CFIndex num_devices = CFSetGetCount(device_set); + IOHIDDeviceRef *device_array = calloc(num_devices, sizeof(IOHIDDeviceRef)); + CFSetGetValues(device_set, (const void **) device_array); + + setlocale(LC_ALL, ""); + + for (i = 0; i < num_devices; i++) { + IOHIDDeviceRef dev = device_array[i]; + printf("Device: %p\n", dev); + printf(" %04hx %04hx\n", get_vendor_id(dev), get_product_id(dev)); + + wchar_t serial[256], buf[256]; + char cbuf[256]; + get_serial_number(dev, serial, 256); + + + printf(" Serial: %ls\n", serial); + printf(" Loc: %ld\n", get_location_id(dev)); + get_transport(dev, buf, 256); + printf(" Trans: %ls\n", buf); + make_path(dev, cbuf, 256); + printf(" Path: %s\n", cbuf); + + } + + return 0; +} +#endif + +#endif /* SDL_JOYSTICK_HIDAPI */ diff --git a/Engine/lib/sdl/src/hidapi/pc/hidapi-hidraw.pc.in b/Engine/lib/sdl/src/hidapi/pc/hidapi-hidraw.pc.in new file mode 100644 index 000000000..e20558d5a --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/pc/hidapi-hidraw.pc.in @@ -0,0 +1,10 @@ +prefix=@prefix@ +exec_prefix=@exec_prefix@ +libdir=@libdir@ +includedir=@includedir@ + +Name: hidapi-hidraw +Description: C Library for USB/Bluetooth HID device access from Linux, Mac OS X, FreeBSD, and Windows. This is the hidraw implementation. +Version: @VERSION@ +Libs: -L${libdir} -lhidapi-hidraw +Cflags: -I${includedir}/hidapi diff --git a/Engine/lib/sdl/src/hidapi/pc/hidapi-libusb.pc.in b/Engine/lib/sdl/src/hidapi/pc/hidapi-libusb.pc.in new file mode 100644 index 000000000..2e4950655 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/pc/hidapi-libusb.pc.in @@ -0,0 +1,10 @@ +prefix=@prefix@ +exec_prefix=@exec_prefix@ +libdir=@libdir@ +includedir=@includedir@ + +Name: hidapi-libusb +Description: C Library for USB HID device access from Linux, Mac OS X, FreeBSD, and Windows. This is the libusb implementation. +Version: @VERSION@ +Libs: -L${libdir} -lhidapi-libusb +Cflags: -I${includedir}/hidapi diff --git a/Engine/lib/sdl/src/hidapi/pc/hidapi.pc.in b/Engine/lib/sdl/src/hidapi/pc/hidapi.pc.in new file mode 100644 index 000000000..5835c99bf --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/pc/hidapi.pc.in @@ -0,0 +1,10 @@ +prefix=@prefix@ +exec_prefix=@exec_prefix@ +libdir=@libdir@ +includedir=@includedir@ + +Name: hidapi +Description: C Library for USB/Bluetooth HID device access from Linux, Mac OS X, FreeBSD, and Windows. +Version: @VERSION@ +Libs: -L${libdir} -lhidapi +Cflags: -I${includedir}/hidapi diff --git a/Engine/lib/sdl/src/hidapi/testgui/Makefile-manual b/Engine/lib/sdl/src/hidapi/testgui/Makefile-manual new file mode 100644 index 000000000..3f61705f2 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/Makefile-manual @@ -0,0 +1,26 @@ + + +OS=$(shell uname) + +ifeq ($(OS), Darwin) + FILE=Makefile.mac +endif + +ifneq (,$(findstring MINGW,$(OS))) + FILE=Makefile.mingw +endif + +ifeq ($(OS), Linux) + FILE=Makefile.linux +endif + +ifeq ($(OS), FreeBSD) + FILE=Makefile.freebsd +endif + +ifeq ($(FILE), ) +all: + $(error Your platform ${OS} is not supported at this time.) +endif + +include $(FILE) diff --git a/Engine/lib/sdl/src/hidapi/testgui/Makefile.am b/Engine/lib/sdl/src/hidapi/testgui/Makefile.am new file mode 100644 index 000000000..1c02f3f2c --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/Makefile.am @@ -0,0 +1,43 @@ + +AM_CPPFLAGS = -I$(top_srcdir)/hidapi/ $(CFLAGS_TESTGUI) + +if OS_LINUX +## Linux +bin_PROGRAMS = hidapi-hidraw-testgui hidapi-libusb-testgui + +hidapi_hidraw_testgui_SOURCES = test.cpp +hidapi_hidraw_testgui_LDADD = $(top_builddir)/linux/libhidapi-hidraw.la $(LIBS_TESTGUI) + +hidapi_libusb_testgui_SOURCES = test.cpp +hidapi_libusb_testgui_LDADD = $(top_builddir)/libusb/libhidapi-libusb.la $(LIBS_TESTGUI) +else +## Other OS's +bin_PROGRAMS = hidapi-testgui + +hidapi_testgui_SOURCES = test.cpp +hidapi_testgui_LDADD = $(top_builddir)/$(backend)/libhidapi.la $(LIBS_TESTGUI) +endif + +if OS_DARWIN +hidapi_testgui_SOURCES = test.cpp mac_support_cocoa.m mac_support.h +# Rules for copying the binary and its dependencies into the app bundle. +TestGUI.app/Contents/MacOS/hidapi-testgui$(EXEEXT): hidapi-testgui$(EXEEXT) + $(srcdir)/copy_to_bundle.sh + +all: all-am TestGUI.app/Contents/MacOS/hidapi-testgui$(EXEEXT) + +endif + +EXTRA_DIST = \ + copy_to_bundle.sh \ + Makefile-manual \ + Makefile.freebsd \ + Makefile.linux \ + Makefile.mac \ + Makefile.mingw \ + TestGUI.app.in \ + testgui.sln \ + testgui.vcproj + +distclean-local: + rm -rf TestGUI.app diff --git a/Engine/lib/sdl/src/hidapi/testgui/Makefile.freebsd b/Engine/lib/sdl/src/hidapi/testgui/Makefile.freebsd new file mode 100644 index 000000000..09a24737c --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/Makefile.freebsd @@ -0,0 +1,33 @@ +########################################### +# Simple Makefile for HIDAPI test program +# +# Alan Ott +# Signal 11 Software +# 2010-06-01 +########################################### + +all: testgui + +CC=cc +CXX=c++ +COBJS=../libusb/hid.o +CPPOBJS=test.o +OBJS=$(COBJS) $(CPPOBJS) +CFLAGS=-I../hidapi -I/usr/local/include `fox-config --cflags` -Wall -g -c +LDFLAGS= -L/usr/local/lib +LIBS= -lusb -liconv `fox-config --libs` -pthread + + +testgui: $(OBJS) + $(CXX) -Wall -g $^ $(LDFLAGS) -o $@ $(LIBS) + +$(COBJS): %.o: %.c + $(CC) $(CFLAGS) $< -o $@ + +$(CPPOBJS): %.o: %.cpp + $(CXX) $(CFLAGS) $< -o $@ + +clean: + rm *.o testgui + +.PHONY: clean diff --git a/Engine/lib/sdl/src/hidapi/testgui/Makefile.linux b/Engine/lib/sdl/src/hidapi/testgui/Makefile.linux new file mode 100644 index 000000000..d32e16317 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/Makefile.linux @@ -0,0 +1,32 @@ +########################################### +# Simple Makefile for HIDAPI test program +# +# Alan Ott +# Signal 11 Software +# 2010-06-01 +########################################### + +all: testgui + +CC=gcc +CXX=g++ +COBJS=../libusb/hid.o +CPPOBJS=test.o +OBJS=$(COBJS) $(CPPOBJS) +CFLAGS=-I../hidapi -Wall -g -c `fox-config --cflags` `pkg-config libusb-1.0 --cflags` +LIBS=-ludev -lrt -lpthread `fox-config --libs` `pkg-config libusb-1.0 --libs` + + +testgui: $(OBJS) + g++ -Wall -g $^ $(LIBS) -o testgui + +$(COBJS): %.o: %.c + $(CC) $(CFLAGS) $< -o $@ + +$(CPPOBJS): %.o: %.cpp + $(CXX) $(CFLAGS) $< -o $@ + +clean: + rm *.o testgui + +.PHONY: clean diff --git a/Engine/lib/sdl/src/hidapi/testgui/Makefile.mac b/Engine/lib/sdl/src/hidapi/testgui/Makefile.mac new file mode 100644 index 000000000..cda7d49e9 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/Makefile.mac @@ -0,0 +1,46 @@ +########################################### +# Simple Makefile for HIDAPI test program +# +# Alan Ott +# Signal 11 Software +# 2010-07-03 +########################################### + +all: hidapi-testgui + +CC=gcc +CXX=g++ +COBJS=../mac/hid.o +CPPOBJS=test.o +OBJCOBJS=mac_support_cocoa.o +OBJS=$(COBJS) $(CPPOBJS) $(OBJCOBJS) +CFLAGS=-I../hidapi -Wall -g -c `fox-config --cflags` +LDFLAGS=-L/usr/X11R6/lib +LIBS=`fox-config --libs` -framework IOKit -framework CoreFoundation -framework Cocoa + + +hidapi-testgui: $(OBJS) TestGUI.app + g++ -Wall -g $(OBJS) $(LIBS) $(LDFLAGS) -o hidapi-testgui + ./copy_to_bundle.sh + #cp TestGUI.app/Contents/MacOS/hidapi-testgui TestGUI.app/Contents/MacOS/tg + #cp start.sh TestGUI.app/Contents/MacOS/hidapi-testgui + +$(COBJS): %.o: %.c + $(CC) $(CFLAGS) $< -o $@ + +$(CPPOBJS): %.o: %.cpp + $(CXX) $(CFLAGS) $< -o $@ + +$(OBJCOBJS): %.o: %.m + $(CXX) $(CFLAGS) -x objective-c++ $< -o $@ + +TestGUI.app: TestGUI.app.in + rm -Rf TestGUI.app + mkdir -p TestGUI.app + cp -R TestGUI.app.in/ TestGUI.app + +clean: + rm -f $(OBJS) hidapi-testgui + rm -Rf TestGUI.app + +.PHONY: clean diff --git a/Engine/lib/sdl/src/hidapi/testgui/Makefile.mingw b/Engine/lib/sdl/src/hidapi/testgui/Makefile.mingw new file mode 100644 index 000000000..df0f69d1d --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/Makefile.mingw @@ -0,0 +1,32 @@ +########################################### +# Simple Makefile for HIDAPI test program +# +# Alan Ott +# Signal 11 Software +# 2010-06-01 +########################################### + +all: hidapi-testgui + +CC=gcc +CXX=g++ +COBJS=../windows/hid.o +CPPOBJS=test.o +OBJS=$(COBJS) $(CPPOBJS) +CFLAGS=-I../hidapi -I../../hidapi-externals/fox/include -g -c +LIBS= -mwindows -lsetupapi -L../../hidapi-externals/fox/lib -Wl,-Bstatic -lFOX-1.6 -Wl,-Bdynamic -lgdi32 -Wl,--enable-auto-import -static-libgcc -static-libstdc++ -lkernel32 + + +hidapi-testgui: $(OBJS) + g++ -g $^ $(LIBS) -o hidapi-testgui + +$(COBJS): %.o: %.c + $(CC) $(CFLAGS) $< -o $@ + +$(CPPOBJS): %.o: %.cpp + $(CXX) $(CFLAGS) $< -o $@ + +clean: + rm -f *.o hidapi-testgui.exe + +.PHONY: clean diff --git a/Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/Info.plist b/Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/Info.plist new file mode 100644 index 000000000..ab473d53a --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/Info.plist @@ -0,0 +1,28 @@ + + + + + CFBundleDevelopmentRegion + English + CFBundleDisplayName + + CFBundleExecutable + hidapi-testgui + CFBundleIconFile + Signal11.icns + CFBundleIdentifier + us.signal11.hidtestgui + CFBundleInfoDictionaryVersion + 6.0 + CFBundleName + testgui + CFBundlePackageType + APPL + CFBundleSignature + ???? + CFBundleVersion + 1.0 + CSResourcesFileMapped + + + diff --git a/Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/PkgInfo b/Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/PkgInfo new file mode 100644 index 000000000..bd04210fb --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/PkgInfo @@ -0,0 +1 @@ +APPL???? \ No newline at end of file diff --git a/Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/Resources/English.lproj/InfoPlist.strings b/Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/Resources/English.lproj/InfoPlist.strings new file mode 100644 index 0000000000000000000000000000000000000000..dea12de4cad936a6204d4da70d2ca96aef900b31 GIT binary patch literal 92 zcmW-Z!3uyN5C!M#S9tbN-x2r|Q3;V~qzLu#)x*oq?lA28G2*azG7B@2orjH8u89{# bCX+-f2F*!V&^~bXzEEWk)pxI)ej3aVcM}l{ literal 0 HcmV?d00001 diff --git a/Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/Resources/Signal11.icns b/Engine/lib/sdl/src/hidapi/testgui/TestGUI.app.in/Contents/Resources/Signal11.icns new file mode 100644 index 0000000000000000000000000000000000000000..bb6b7bd5f9ed6c2ae2c91c202e5024b4811fc00b GIT binary patch literal 21918 zcmeHMy^b735biq%5D*f~b3j6fcn5d^M<;}n^8@V8{@^?IY!C?h1U?H15(j`EfPh!v z0VXFSCm6n}>YwWAo}Jn4yOWW6(M(rYe_vH~P0xHMbN2AjxiO#KoSlF8fidR%m09#F zTqnk~{SucqmUiXP5dqc|+k@t5Y!si=AvuaA>*~#b{n3{cx~G5g6A?`o0G2FvBMg#G5H6y8}Ly8!#yFSqxSy2?%$E0s?3RIsqSaBpA_ytOA z`-JozZ#A1qk(P+Gq-RFY`+GSIQ=Ch*sqLSm{Rk9+X}|PIMzQG2j*EJMd5l}ZpW?1? z7`_}}x0vU#IB>M61JkKn*}zNegx)Rc3hnF)etM5+=e$)p+ERrBqm6umg|NdeLe)U1 z;JdlSao6BjTp_46AXE*63Jw_|R1Jg*a~UC24TK7F86i|n1EGS$8W5-kLWTN|9TyFR zs)0~7R;ZecP{GGS)zCX)g^F++sm1iQ1Lt?RA6|avYa4P zh(_NI>^2dqEYAv6phZHJp|DUn_&})OmdYShjV+CZD&QlbYPeQ#PA%LDRSb`XiZ(Ao zB^jRb-w~TpiU<|4PEiy+Tq{(X;WURN2am#Oj?~8_D^xiWJv${-ie!aqg5-pX^EjcJ z;KV|u!TW?N;HHJjVP^?d0M8RD-s*@@aV#ZNn3LQlLS=DYs2G1MGG9EFC9YnMGht?fXpE1` zd<77)y$2L5A1Cs<{E<^Ycp9!QZ7QDv zy>EkPEKp5tcc;42`&>i4)S|sFapPAi;}A4 z!0}cDg%@*Kb7OISpK8p=7Z~z~miywOP#HeCe1Hg?ndGsa?hVzPC`u+3hYWQ)c`(OKC&tK6i zR7C#Yy?^O$=XhkB_CAM@Vs3*x*6lP~>Z$p;18HOCcx>}(|EU9W8kS;g3)6);9!r*C z4k5+V{M>=GF>^e%|EdFX8kS<}`FktQpL&kftR1Kws2!*ss2!*ss2!*ss2!*sxVsLV PJ$&>TqQCdaKgRqA{2bi% literal 0 HcmV?d00001 diff --git a/Engine/lib/sdl/src/hidapi/testgui/copy_to_bundle.sh b/Engine/lib/sdl/src/hidapi/testgui/copy_to_bundle.sh new file mode 100644 index 000000000..f0fc767a9 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/copy_to_bundle.sh @@ -0,0 +1,97 @@ +#!/bin/bash + +#### Configuration: +# The name of the executable. It is assumed +# that it is in the current working directory. +EXE_NAME=hidapi-testgui +# Path to the executable directory inside the bundle. +# This must be an absolute path, so use $PWD. +EXEPATH=$PWD/TestGUI.app/Contents/MacOS +# Libraries to explicitly bundle, even though they +# may not be in /opt/local. One per line. These +# are used with grep, so only a portion of the name +# is required. eg: libFOX, libz, etc. +LIBS_TO_BUNDLE=libFOX + + +function copydeps { + local file=$1 + # echo "Copying deps for $file...." + local BASE_OF_EXE=`basename $file` + + # A will contain the dependencies of this library + local A=`otool -LX $file |cut -f 1 -d " "` + local i + for i in $A; do + local BASE=`basename $i` + + # See if it's a lib we specifically want to bundle + local bundle_this_lib=0 + local j + for j in $LIBS_TO_BUNDLE; do + echo $i |grep -q $j + if [ $? -eq 0 ]; then + bundle_this_lib=1 + echo "bundling $i because it's in the list." + break; + fi + done + + # See if it's in /opt/local. Bundle all in /opt/local + local isOptLocal=0 + echo $i |grep -q /opt/local + if [ $? -eq 0 ]; then + isOptLocal=1 + echo "bundling $i because it's in /opt/local." + fi + + # Bundle the library + if [ $isOptLocal -ne 0 ] || [ $bundle_this_lib -ne 0 ]; then + + # Copy the file into the bundle if it exists. + if [ -f $EXEPATH/$BASE ]; then + z=0 + else + cp $i $EXEPATH + chmod 755 $EXEPATH/$BASE + fi + + + # echo "$BASE_OF_EXE depends on $BASE" + + # Fix the paths using install_name_tool and then + # call this function recursively for each dependency + # of this library. + if [ $BASE_OF_EXE != $BASE ]; then + + # Fix the paths + install_name_tool -id @executable_path/$BASE $EXEPATH/$BASE + install_name_tool -change $i @executable_path/$BASE $EXEPATH/$BASE_OF_EXE + + # Call this function (recursive) on + # on each dependency of this library. + copydeps $EXEPATH/$BASE + fi + fi + done +} + +rm -f $EXEPATH/* + +# Copy the binary into the bundle. Use ../libtool to do this if it's +# available beacuse if $EXE_NAME was built with autotools, it will be +# necessary. If ../libtool not available, just use cp to do the copy, but +# only if $EXE_NAME is a binary. +if [ -x ../libtool ]; then + ../libtool --mode=install cp $EXE_NAME $EXEPATH +else + file -bI $EXE_NAME |grep binary + if [ $? -ne 0 ]; then + echo "There is no ../libtool and $EXE_NAME is not a binary." + echo "I'm not sure what to do." + exit 1 + else + cp $EXE_NAME $EXEPATH + fi +fi +copydeps $EXEPATH/$EXE_NAME diff --git a/Engine/lib/sdl/src/hidapi/testgui/mac_support.cpp b/Engine/lib/sdl/src/hidapi/testgui/mac_support.cpp new file mode 100644 index 000000000..e1e387442 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/mac_support.cpp @@ -0,0 +1,134 @@ +/******************************* + Mac support for HID Test GUI + + Alan Ott + Signal 11 Software + + Some of this code is from Apple Documentation, most notably + http://developer.apple.com/legacy/mac/library/documentation/AppleScript/Conceptual/AppleEvents/AppleEvents.pdf +*******************************/ + +#include +#include + + +extern FXMainWindow *g_main_window; + +static pascal OSErr HandleQuitMessage(const AppleEvent *theAppleEvent, AppleEvent + *reply, long handlerRefcon) +{ + puts("Quitting\n"); + FXApp::instance()->exit(); + return 0; +} + +static pascal OSErr HandleReopenMessage(const AppleEvent *theAppleEvent, AppleEvent + *reply, long handlerRefcon) +{ + puts("Showing"); + g_main_window->show(); + return 0; +} + +static pascal OSErr HandleWildCardMessage(const AppleEvent *theAppleEvent, AppleEvent + *reply, long handlerRefcon) +{ + puts("WildCard\n"); + return 0; +} + +OSStatus AEHandler(EventHandlerCallRef inCaller, EventRef inEvent, void* inRefcon) +{ + Boolean release = false; + EventRecord eventRecord; + OSErr ignoreErrForThisSample; + + // Events of type kEventAppleEvent must be removed from the queue + // before being passed to AEProcessAppleEvent. + if (IsEventInQueue(GetMainEventQueue(), inEvent)) + { + // RemoveEventFromQueue will release the event, which will + // destroy it if we don't retain it first. + RetainEvent(inEvent); + release = true; + RemoveEventFromQueue(GetMainEventQueue(), inEvent); + } + // Convert the event ref to the type AEProcessAppleEvent expects. + ConvertEventRefToEventRecord(inEvent, &eventRecord); + ignoreErrForThisSample = AEProcessAppleEvent(&eventRecord); + if (release) + ReleaseEvent(inEvent); + // This Carbon event has been handled, even if no AppleEvent handlers + // were installed for the Apple event. + return noErr; +} + +static void HandleEvent(EventRecord *event) +{ + //printf("What: %d message %x\n", event->what, event->message); + if (event->what == osEvt) { + if (((event->message >> 24) & 0xff) == suspendResumeMessage) { + if (event->message & resumeFlag) { + g_main_window->show(); + } + } + } + +#if 0 + switch (event->what) + { + case mouseDown: + //HandleMouseDown(event); + break; + case keyDown: + case autoKey: + //HandleKeyPress(event); + break; + case kHighLevelEvent: + puts("Calling ProcessAppleEvent\n"); + AEProcessAppleEvent(event); + break; + } +#endif +} + +void +init_apple_message_system() +{ + OSErr err; + static const EventTypeSpec appleEvents[] = + { + { kEventClassAppleEvent, kEventAppleEvent } + }; + + /* Install the handler for Apple Events */ + InstallApplicationEventHandler(NewEventHandlerUPP(AEHandler), + GetEventTypeCount(appleEvents), appleEvents, 0, NULL); + + /* Install handlers for the individual Apple Events that come + from the Dock icon: the Reopen (click), and the Quit messages. */ + err = AEInstallEventHandler(kCoreEventClass, kAEQuitApplication, + NewAEEventHandlerUPP(HandleQuitMessage), 0, false); + err = AEInstallEventHandler(kCoreEventClass, kAEReopenApplication, + NewAEEventHandlerUPP(HandleReopenMessage), 0, false); +#if 0 + // Left as an example of a wild card match. + err = AEInstallEventHandler(kCoreEventClass, typeWildCard, + NewAEEventHandlerUPP(HandleWildMessage), 0, false); +#endif +} + +void +check_apple_events() +{ + RgnHandle cursorRgn = NULL; + Boolean gotEvent=TRUE; + EventRecord event; + + while (gotEvent) { + gotEvent = WaitNextEvent(everyEvent, &event, 0L/*timeout*/, cursorRgn); + if (gotEvent) { + HandleEvent(&event); + } + } +} diff --git a/Engine/lib/sdl/src/hidapi/testgui/mac_support.h b/Engine/lib/sdl/src/hidapi/testgui/mac_support.h new file mode 100644 index 000000000..7d9ab493b --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/mac_support.h @@ -0,0 +1,17 @@ +/******************************* + Mac support for HID Test GUI + + Alan Ott + Signal 11 Software + +*******************************/ + +#ifndef MAC_SUPPORT_H__ +#define MAC_SUPPORT_H__ + +extern "C" { + void init_apple_message_system(); + void check_apple_events(); +} + +#endif diff --git a/Engine/lib/sdl/src/hidapi/testgui/mac_support_cocoa.m b/Engine/lib/sdl/src/hidapi/testgui/mac_support_cocoa.m new file mode 100644 index 000000000..75de7e9c4 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/mac_support_cocoa.m @@ -0,0 +1,94 @@ +/******************************* + Mac support for HID Test GUI + + Alan Ott + Signal 11 Software +*******************************/ + +#include +#import + +extern FXMainWindow *g_main_window; + + +@interface MyAppDelegate : NSObject +{ +} +@end + +@implementation MyAppDelegate +- (void) applicationWillBecomeActive:(NSNotification*)notif +{ + printf("WillBecomeActive\n"); + g_main_window->show(); + +} + +- (void) applicationWillTerminate:(NSNotification*)notif +{ + /* Doesn't get called. Not sure why */ + printf("WillTerminate\n"); + FXApp::instance()->exit(); +} + +- (NSApplicationTerminateReply) applicationShouldTerminate:(NSApplication*)sender +{ + /* Doesn't get called. Not sure why */ + printf("ShouldTerminate\n"); + return YES; +} + +- (void) applicationWillHide:(NSNotification*)notif +{ + printf("WillHide\n"); + g_main_window->hide(); +} + +- (void) handleQuitEvent:(NSAppleEventDescriptor*)event withReplyEvent:(NSAppleEventDescriptor*)replyEvent +{ + printf("QuitEvent\n"); + FXApp::instance()->exit(); +} + +@end + +extern "C" { + +void +init_apple_message_system() +{ + static MyAppDelegate *d = [MyAppDelegate new]; + + [[NSApplication sharedApplication] setDelegate:d]; + + /* Register for Apple Events. */ + /* This is from + http://stackoverflow.com/questions/1768497/application-exit-event */ + NSAppleEventManager *aem = [NSAppleEventManager sharedAppleEventManager]; + [aem setEventHandler:d + andSelector:@selector(handleQuitEvent:withReplyEvent:) + forEventClass:kCoreEventClass andEventID:kAEQuitApplication]; +} + +void +check_apple_events() +{ + NSApplication *app = [NSApplication sharedApplication]; + + NSAutoreleasePool *pool = [NSAutoreleasePool new]; + while (1) { + NSEvent* event = [NSApp nextEventMatchingMask:NSAnyEventMask + untilDate:nil + inMode:NSDefaultRunLoopMode + dequeue:YES]; + if (event == NULL) + break; + else { + //printf("Event happened: Type: %d\n", event->_type); + [app sendEvent: event]; + } + } + [pool release]; +} + +} /* extern "C" */ diff --git a/Engine/lib/sdl/src/hidapi/testgui/start.sh b/Engine/lib/sdl/src/hidapi/testgui/start.sh new file mode 100644 index 000000000..980635d95 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/start.sh @@ -0,0 +1,2 @@ +#!/bin/bash +xterm -e /Users/alan/work/hidapi/testgui/TestGUI.app/Contents/MacOS/tg diff --git a/Engine/lib/sdl/src/hidapi/testgui/test.cpp b/Engine/lib/sdl/src/hidapi/testgui/test.cpp new file mode 100644 index 000000000..538db7917 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/testgui/test.cpp @@ -0,0 +1,532 @@ +/******************************************************* + Demo Program for HIDAPI + + Alan Ott + Signal 11 Software + + 2010-07-20 + + Copyright 2010, All Rights Reserved + + This contents of this file may be used by anyone + for any reason without any conditions and may be + used as a starting point for your own applications + which use HIDAPI. +********************************************************/ + + +#include + +#include "hidapi.h" +#include "mac_support.h" +#include +#include +#include + +#ifdef _WIN32 + // Thanks Microsoft, but I know how to use strncpy(). + #pragma warning(disable:4996) +#endif + +class MainWindow : public FXMainWindow { + FXDECLARE(MainWindow) + +public: + enum { + ID_FIRST = FXMainWindow::ID_LAST, + ID_CONNECT, + ID_DISCONNECT, + ID_RESCAN, + ID_SEND_OUTPUT_REPORT, + ID_SEND_FEATURE_REPORT, + ID_GET_FEATURE_REPORT, + ID_CLEAR, + ID_TIMER, + ID_MAC_TIMER, + ID_LAST, + }; + +private: + FXList *device_list; + FXButton *connect_button; + FXButton *disconnect_button; + FXButton *rescan_button; + FXButton *output_button; + FXLabel *connected_label; + FXTextField *output_text; + FXTextField *output_len; + FXButton *feature_button; + FXButton *get_feature_button; + FXTextField *feature_text; + FXTextField *feature_len; + FXTextField *get_feature_text; + FXText *input_text; + FXFont *title_font; + + struct hid_device_info *devices; + hid_device *connected_device; + size_t getDataFromTextField(FXTextField *tf, char *buf, size_t len); + int getLengthFromTextField(FXTextField *tf); + + +protected: + MainWindow() {}; +public: + MainWindow(FXApp *a); + ~MainWindow(); + virtual void create(); + + long onConnect(FXObject *sender, FXSelector sel, void *ptr); + long onDisconnect(FXObject *sender, FXSelector sel, void *ptr); + long onRescan(FXObject *sender, FXSelector sel, void *ptr); + long onSendOutputReport(FXObject *sender, FXSelector sel, void *ptr); + long onSendFeatureReport(FXObject *sender, FXSelector sel, void *ptr); + long onGetFeatureReport(FXObject *sender, FXSelector sel, void *ptr); + long onClear(FXObject *sender, FXSelector sel, void *ptr); + long onTimeout(FXObject *sender, FXSelector sel, void *ptr); + long onMacTimeout(FXObject *sender, FXSelector sel, void *ptr); +}; + +// FOX 1.7 changes the timeouts to all be nanoseconds. +// Fox 1.6 had all timeouts as milliseconds. +#if (FOX_MINOR >= 7) + const int timeout_scalar = 1000*1000; +#else + const int timeout_scalar = 1; +#endif + +FXMainWindow *g_main_window; + + +FXDEFMAP(MainWindow) MainWindowMap [] = { + FXMAPFUNC(SEL_COMMAND, MainWindow::ID_CONNECT, MainWindow::onConnect ), + FXMAPFUNC(SEL_COMMAND, MainWindow::ID_DISCONNECT, MainWindow::onDisconnect ), + FXMAPFUNC(SEL_COMMAND, MainWindow::ID_RESCAN, MainWindow::onRescan ), + FXMAPFUNC(SEL_COMMAND, MainWindow::ID_SEND_OUTPUT_REPORT, MainWindow::onSendOutputReport ), + FXMAPFUNC(SEL_COMMAND, MainWindow::ID_SEND_FEATURE_REPORT, MainWindow::onSendFeatureReport ), + FXMAPFUNC(SEL_COMMAND, MainWindow::ID_GET_FEATURE_REPORT, MainWindow::onGetFeatureReport ), + FXMAPFUNC(SEL_COMMAND, MainWindow::ID_CLEAR, MainWindow::onClear ), + FXMAPFUNC(SEL_TIMEOUT, MainWindow::ID_TIMER, MainWindow::onTimeout ), + FXMAPFUNC(SEL_TIMEOUT, MainWindow::ID_MAC_TIMER, MainWindow::onMacTimeout ), +}; + +FXIMPLEMENT(MainWindow, FXMainWindow, MainWindowMap, ARRAYNUMBER(MainWindowMap)); + +MainWindow::MainWindow(FXApp *app) + : FXMainWindow(app, "HIDAPI Test Application", NULL, NULL, DECOR_ALL, 200,100, 425,700) +{ + devices = NULL; + connected_device = NULL; + + FXVerticalFrame *vf = new FXVerticalFrame(this, LAYOUT_FILL_Y|LAYOUT_FILL_X); + + FXLabel *label = new FXLabel(vf, "HIDAPI Test Tool"); + title_font = new FXFont(getApp(), "Arial", 14, FXFont::Bold); + label->setFont(title_font); + + new FXLabel(vf, + "Select a device and press Connect.", NULL, JUSTIFY_LEFT); + new FXLabel(vf, + "Output data bytes can be entered in the Output section, \n" + "separated by space, comma or brackets. Data starting with 0x\n" + "is treated as hex. Data beginning with a 0 is treated as \n" + "octal. All other data is treated as decimal.", NULL, JUSTIFY_LEFT); + new FXLabel(vf, + "Data received from the device appears in the Input section.", + NULL, JUSTIFY_LEFT); + new FXLabel(vf, + "Optionally, a report length may be specified. Extra bytes are\n" + "padded with zeros. If no length is specified, the length is \n" + "inferred from the data.", + NULL, JUSTIFY_LEFT); + new FXLabel(vf, ""); + + // Device List and Connect/Disconnect buttons + FXHorizontalFrame *hf = new FXHorizontalFrame(vf, LAYOUT_FILL_X); + //device_list = new FXList(new FXHorizontalFrame(hf,FRAME_SUNKEN|FRAME_THICK, 0,0,0,0, 0,0,0,0), NULL, 0, LISTBOX_NORMAL|LAYOUT_FILL_X|LAYOUT_FILL_Y|LAYOUT_FIX_WIDTH|LAYOUT_FIX_HEIGHT, 0,0,300,200); + device_list = new FXList(new FXHorizontalFrame(hf,FRAME_SUNKEN|FRAME_THICK|LAYOUT_FILL_X|LAYOUT_FILL_Y, 0,0,0,0, 0,0,0,0), NULL, 0, LISTBOX_NORMAL|LAYOUT_FILL_X|LAYOUT_FILL_Y, 0,0,300,200); + FXVerticalFrame *buttonVF = new FXVerticalFrame(hf); + connect_button = new FXButton(buttonVF, "Connect", NULL, this, ID_CONNECT, BUTTON_NORMAL|LAYOUT_FILL_X); + disconnect_button = new FXButton(buttonVF, "Disconnect", NULL, this, ID_DISCONNECT, BUTTON_NORMAL|LAYOUT_FILL_X); + disconnect_button->disable(); + rescan_button = new FXButton(buttonVF, "Re-Scan devices", NULL, this, ID_RESCAN, BUTTON_NORMAL|LAYOUT_FILL_X); + new FXHorizontalFrame(buttonVF, 0, 0,0,0,0, 0,0,50,0); + + connected_label = new FXLabel(vf, "Disconnected"); + + new FXHorizontalFrame(vf); + + // Output Group Box + FXGroupBox *gb = new FXGroupBox(vf, "Output", FRAME_GROOVE|LAYOUT_FILL_X); + FXMatrix *matrix = new FXMatrix(gb, 3, MATRIX_BY_COLUMNS|LAYOUT_FILL_X); + new FXLabel(matrix, "Data"); + new FXLabel(matrix, "Length"); + new FXLabel(matrix, ""); + + //hf = new FXHorizontalFrame(gb, LAYOUT_FILL_X); + output_text = new FXTextField(matrix, 30, NULL, 0, TEXTFIELD_NORMAL|LAYOUT_FILL_X|LAYOUT_FILL_COLUMN); + output_text->setText("1 0x81 0"); + output_len = new FXTextField(matrix, 5, NULL, 0, TEXTFIELD_NORMAL|LAYOUT_FILL_X|LAYOUT_FILL_COLUMN); + output_button = new FXButton(matrix, "Send Output Report", NULL, this, ID_SEND_OUTPUT_REPORT, BUTTON_NORMAL|LAYOUT_FILL_X); + output_button->disable(); + //new FXHorizontalFrame(matrix, LAYOUT_FILL_X); + + //hf = new FXHorizontalFrame(gb, LAYOUT_FILL_X); + feature_text = new FXTextField(matrix, 30, NULL, 0, TEXTFIELD_NORMAL|LAYOUT_FILL_X|LAYOUT_FILL_COLUMN); + feature_len = new FXTextField(matrix, 5, NULL, 0, TEXTFIELD_NORMAL|LAYOUT_FILL_X|LAYOUT_FILL_COLUMN); + feature_button = new FXButton(matrix, "Send Feature Report", NULL, this, ID_SEND_FEATURE_REPORT, BUTTON_NORMAL|LAYOUT_FILL_X); + feature_button->disable(); + + get_feature_text = new FXTextField(matrix, 30, NULL, 0, TEXTFIELD_NORMAL|LAYOUT_FILL_X|LAYOUT_FILL_COLUMN); + new FXWindow(matrix); + get_feature_button = new FXButton(matrix, "Get Feature Report", NULL, this, ID_GET_FEATURE_REPORT, BUTTON_NORMAL|LAYOUT_FILL_X); + get_feature_button->disable(); + + + // Input Group Box + gb = new FXGroupBox(vf, "Input", FRAME_GROOVE|LAYOUT_FILL_X|LAYOUT_FILL_Y); + FXVerticalFrame *innerVF = new FXVerticalFrame(gb, LAYOUT_FILL_X|LAYOUT_FILL_Y); + input_text = new FXText(new FXHorizontalFrame(innerVF,LAYOUT_FILL_X|LAYOUT_FILL_Y|FRAME_SUNKEN|FRAME_THICK, 0,0,0,0, 0,0,0,0), NULL, 0, LAYOUT_FILL_X|LAYOUT_FILL_Y); + input_text->setEditable(false); + new FXButton(innerVF, "Clear", NULL, this, ID_CLEAR, BUTTON_NORMAL|LAYOUT_RIGHT); + + +} + +MainWindow::~MainWindow() +{ + if (connected_device) + hid_close(connected_device); + hid_exit(); + delete title_font; +} + +void +MainWindow::create() +{ + FXMainWindow::create(); + show(); + + onRescan(NULL, 0, NULL); + + +#ifdef __APPLE__ + init_apple_message_system(); +#endif + + getApp()->addTimeout(this, ID_MAC_TIMER, + 50 * timeout_scalar /*50ms*/); +} + +long +MainWindow::onConnect(FXObject *sender, FXSelector sel, void *ptr) +{ + if (connected_device != NULL) + return 1; + + FXint cur_item = device_list->getCurrentItem(); + if (cur_item < 0) + return -1; + FXListItem *item = device_list->getItem(cur_item); + if (!item) + return -1; + struct hid_device_info *device_info = (struct hid_device_info*) item->getData(); + if (!device_info) + return -1; + + connected_device = hid_open_path(device_info->path); + + if (!connected_device) { + FXMessageBox::error(this, MBOX_OK, "Device Error", "Unable To Connect to Device"); + return -1; + } + + hid_set_nonblocking(connected_device, 1); + + getApp()->addTimeout(this, ID_TIMER, + 5 * timeout_scalar /*5ms*/); + + FXString s; + s.format("Connected to: %04hx:%04hx -", device_info->vendor_id, device_info->product_id); + s += FXString(" ") + device_info->manufacturer_string; + s += FXString(" ") + device_info->product_string; + connected_label->setText(s); + output_button->enable(); + feature_button->enable(); + get_feature_button->enable(); + connect_button->disable(); + disconnect_button->enable(); + input_text->setText(""); + + + return 1; +} + +long +MainWindow::onDisconnect(FXObject *sender, FXSelector sel, void *ptr) +{ + hid_close(connected_device); + connected_device = NULL; + connected_label->setText("Disconnected"); + output_button->disable(); + feature_button->disable(); + get_feature_button->disable(); + connect_button->enable(); + disconnect_button->disable(); + + getApp()->removeTimeout(this, ID_TIMER); + + return 1; +} + +long +MainWindow::onRescan(FXObject *sender, FXSelector sel, void *ptr) +{ + struct hid_device_info *cur_dev; + + device_list->clearItems(); + + // List the Devices + hid_free_enumeration(devices); + devices = hid_enumerate(0x0, 0x0); + cur_dev = devices; + while (cur_dev) { + // Add it to the List Box. + FXString s; + FXString usage_str; + s.format("%04hx:%04hx -", cur_dev->vendor_id, cur_dev->product_id); + s += FXString(" ") + cur_dev->manufacturer_string; + s += FXString(" ") + cur_dev->product_string; + usage_str.format(" (usage: %04hx:%04hx) ", cur_dev->usage_page, cur_dev->usage); + s += usage_str; + FXListItem *li = new FXListItem(s, NULL, cur_dev); + device_list->appendItem(li); + + cur_dev = cur_dev->next; + } + + if (device_list->getNumItems() == 0) + device_list->appendItem("*** No Devices Connected ***"); + else { + device_list->selectItem(0); + } + + return 1; +} + +size_t +MainWindow::getDataFromTextField(FXTextField *tf, char *buf, size_t len) +{ + const char *delim = " ,{}\t\r\n"; + FXString data = tf->getText(); + const FXchar *d = data.text(); + size_t i = 0; + + // Copy the string from the GUI. + size_t sz = strlen(d); + char *str = (char*) malloc(sz+1); + strcpy(str, d); + + // For each token in the string, parse and store in buf[]. + char *token = strtok(str, delim); + while (token) { + char *endptr; + long int val = strtol(token, &endptr, 0); + buf[i++] = val; + token = strtok(NULL, delim); + } + + free(str); + return i; +} + +/* getLengthFromTextField() + Returns length: + 0: empty text field + >0: valid length + -1: invalid length */ +int +MainWindow::getLengthFromTextField(FXTextField *tf) +{ + long int len; + FXString str = tf->getText(); + size_t sz = str.length(); + + if (sz > 0) { + char *endptr; + len = strtol(str.text(), &endptr, 0); + if (endptr != str.text() && *endptr == '\0') { + if (len <= 0) { + FXMessageBox::error(this, MBOX_OK, "Invalid length", "Enter a length greater than zero."); + return -1; + } + return len; + } + else + return -1; + } + + return 0; +} + +long +MainWindow::onSendOutputReport(FXObject *sender, FXSelector sel, void *ptr) +{ + char buf[256]; + size_t data_len, len; + int textfield_len; + + memset(buf, 0x0, sizeof(buf)); + textfield_len = getLengthFromTextField(output_len); + data_len = getDataFromTextField(output_text, buf, sizeof(buf)); + + if (textfield_len < 0) { + FXMessageBox::error(this, MBOX_OK, "Invalid length", "Length field is invalid. Please enter a number in hex, octal, or decimal."); + return 1; + } + + if (textfield_len > sizeof(buf)) { + FXMessageBox::error(this, MBOX_OK, "Invalid length", "Length field is too long."); + return 1; + } + + len = (textfield_len)? textfield_len: data_len; + + int res = hid_write(connected_device, (const unsigned char*)buf, len); + if (res < 0) { + FXMessageBox::error(this, MBOX_OK, "Error Writing", "Could not write to device. Error reported was: %ls", hid_error(connected_device)); + } + + return 1; +} + +long +MainWindow::onSendFeatureReport(FXObject *sender, FXSelector sel, void *ptr) +{ + char buf[256]; + size_t data_len, len; + int textfield_len; + + memset(buf, 0x0, sizeof(buf)); + textfield_len = getLengthFromTextField(feature_len); + data_len = getDataFromTextField(feature_text, buf, sizeof(buf)); + + if (textfield_len < 0) { + FXMessageBox::error(this, MBOX_OK, "Invalid length", "Length field is invalid. Please enter a number in hex, octal, or decimal."); + return 1; + } + + if (textfield_len > sizeof(buf)) { + FXMessageBox::error(this, MBOX_OK, "Invalid length", "Length field is too long."); + return 1; + } + + len = (textfield_len)? textfield_len: data_len; + + int res = hid_send_feature_report(connected_device, (const unsigned char*)buf, len); + if (res < 0) { + FXMessageBox::error(this, MBOX_OK, "Error Writing", "Could not send feature report to device. Error reported was: %ls", hid_error(connected_device)); + } + + return 1; +} + +long +MainWindow::onGetFeatureReport(FXObject *sender, FXSelector sel, void *ptr) +{ + char buf[256]; + size_t len; + + memset(buf, 0x0, sizeof(buf)); + len = getDataFromTextField(get_feature_text, buf, sizeof(buf)); + + if (len != 1) { + FXMessageBox::error(this, MBOX_OK, "Too many numbers", "Enter only a single report number in the text field"); + } + + int res = hid_get_feature_report(connected_device, (unsigned char*)buf, sizeof(buf)); + if (res < 0) { + FXMessageBox::error(this, MBOX_OK, "Error Getting Report", "Could not get feature report from device. Error reported was: %ls", hid_error(connected_device)); + } + + if (res > 0) { + FXString s; + s.format("Returned Feature Report. %d bytes:\n", res); + for (int i = 0; i < res; i++) { + FXString t; + t.format("%02hhx ", buf[i]); + s += t; + if ((i+1) % 4 == 0) + s += " "; + if ((i+1) % 16 == 0) + s += "\n"; + } + s += "\n"; + input_text->appendText(s); + input_text->setBottomLine(INT_MAX); + } + + return 1; +} + +long +MainWindow::onClear(FXObject *sender, FXSelector sel, void *ptr) +{ + input_text->setText(""); + return 1; +} + +long +MainWindow::onTimeout(FXObject *sender, FXSelector sel, void *ptr) +{ + unsigned char buf[256]; + int res = hid_read(connected_device, buf, sizeof(buf)); + + if (res > 0) { + FXString s; + s.format("Received %d bytes:\n", res); + for (int i = 0; i < res; i++) { + FXString t; + t.format("%02hhx ", buf[i]); + s += t; + if ((i+1) % 4 == 0) + s += " "; + if ((i+1) % 16 == 0) + s += "\n"; + } + s += "\n"; + input_text->appendText(s); + input_text->setBottomLine(INT_MAX); + } + if (res < 0) { + input_text->appendText("hid_read() returned error\n"); + input_text->setBottomLine(INT_MAX); + } + + getApp()->addTimeout(this, ID_TIMER, + 5 * timeout_scalar /*5ms*/); + return 1; +} + +long +MainWindow::onMacTimeout(FXObject *sender, FXSelector sel, void *ptr) +{ +#ifdef __APPLE__ + check_apple_events(); + + getApp()->addTimeout(this, ID_MAC_TIMER, + 50 * timeout_scalar /*50ms*/); +#endif + + return 1; +} + +int main(int argc, char **argv) +{ + FXApp app("HIDAPI Test Application", "Signal 11 Software"); + app.init(argc, argv); + g_main_window = new MainWindow(&app); + app.create(); + app.run(); + return 0; +} diff --git a/Engine/lib/sdl/src/hidapi/udev/99-hid.rules b/Engine/lib/sdl/src/hidapi/udev/99-hid.rules new file mode 100644 index 000000000..0385f50b0 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/udev/99-hid.rules @@ -0,0 +1,33 @@ +# This is a sample udev file for HIDAPI devices which changes the permissions +# to 0666 (world readable/writable) for a specified device on Linux systems. + + +# If you are using the libusb implementation of hidapi (libusb/hid.c), then +# use something like the following line, substituting the VID and PID with +# those of your device. Note that for kernels before 2.6.24, you will need +# to substitute "usb" with "usb_device". It shouldn't hurt to use two lines +# (one each way) for compatibility with older systems. + +# HIDAPI/libusb +SUBSYSTEM=="usb", ATTRS{idVendor}=="04d8", ATTRS{idProduct}=="003f", MODE="0666" + + +# If you are using the hidraw implementation (linux/hid.c), then do something +# like the following, substituting the VID and PID with your device. Busnum 1 +# is USB. + +# HIDAPI/hidraw +KERNEL=="hidraw*", ATTRS{busnum}=="1", ATTRS{idVendor}=="04d8", ATTRS{idProduct}=="003f", MODE="0666" + +# Once done, optionally rename this file for your device, and drop it into +# /etc/udev/rules.d and unplug and re-plug your device. This is all that is +# necessary to see the new permissions. Udev does not have to be restarted. + +# Note that the hexadecimal values for VID and PID are case sensitive and +# must be lower case. + +# If you think permissions of 0666 are too loose, then see: +# http://reactivated.net/writing_udev_rules.html for more information on finer +# grained permission setting. For example, it might be sufficient to just +# set the group or user owner for specific devices (for example the plugdev +# group on some systems). diff --git a/Engine/lib/sdl/src/hidapi/windows/Makefile-manual b/Engine/lib/sdl/src/hidapi/windows/Makefile-manual new file mode 100644 index 000000000..ac471d66d --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/windows/Makefile-manual @@ -0,0 +1,14 @@ + + +OS=$(shell uname) + +ifneq (,$(findstring MINGW,$(OS))) + FILE=Makefile.mingw +endif + +ifeq ($(FILE), ) +all: + $(error Your platform ${OS} is not supported at this time.) +endif + +include $(FILE) diff --git a/Engine/lib/sdl/src/hidapi/windows/Makefile.am b/Engine/lib/sdl/src/hidapi/windows/Makefile.am new file mode 100644 index 000000000..97e261ac9 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/windows/Makefile.am @@ -0,0 +1,16 @@ +lib_LTLIBRARIES = libhidapi.la +libhidapi_la_SOURCES = hid.c +libhidapi_la_LDFLAGS = $(LTLDFLAGS) +AM_CPPFLAGS = -I$(top_srcdir)/hidapi/ +libhidapi_la_LIBADD = $(LIBS) + +hdrdir = $(includedir)/hidapi +hdr_HEADERS = $(top_srcdir)/hidapi/hidapi.h + +EXTRA_DIST = \ + ddk_build \ + hidapi.vcproj \ + hidtest.vcproj \ + Makefile-manual \ + Makefile.mingw \ + hidapi.sln diff --git a/Engine/lib/sdl/src/hidapi/windows/Makefile.mingw b/Engine/lib/sdl/src/hidapi/windows/Makefile.mingw new file mode 100644 index 000000000..b8000041d --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/windows/Makefile.mingw @@ -0,0 +1,35 @@ +########################################### +# Simple Makefile for HIDAPI test program +# +# Alan Ott +# Signal 11 Software +# 2010-06-01 +########################################### + +all: hidtest libhidapi.dll + +CC=gcc +CXX=g++ +COBJS=hid.o +CPPOBJS=../hidtest/hidtest.o +OBJS=$(COBJS) $(CPPOBJS) +CFLAGS=-I../hidapi -g -c +LIBS= -lsetupapi +DLL_LDFLAGS = -mwindows -lsetupapi + +hidtest: $(OBJS) + g++ -g $^ $(LIBS) -o hidtest + +libhidapi.dll: $(OBJS) + $(CC) -g $^ $(DLL_LDFLAGS) -o libhidapi.dll + +$(COBJS): %.o: %.c + $(CC) $(CFLAGS) $< -o $@ + +$(CPPOBJS): %.o: %.cpp + $(CXX) $(CFLAGS) $< -o $@ + +clean: + rm *.o ../hidtest/*.o hidtest.exe + +.PHONY: clean diff --git a/Engine/lib/sdl/src/hidapi/windows/ddk_build/hidapi.def b/Engine/lib/sdl/src/hidapi/windows/ddk_build/hidapi.def new file mode 100644 index 000000000..05e35afd6 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/windows/ddk_build/hidapi.def @@ -0,0 +1,17 @@ +LIBRARY hidapi +EXPORTS + hid_open @1 + hid_write @2 + hid_read @3 + hid_close @4 + hid_get_product_string @5 + hid_get_manufacturer_string @6 + hid_get_serial_number_string @7 + hid_get_indexed_string @8 + hid_error @9 + hid_set_nonblocking @10 + hid_enumerate @11 + hid_open_path @12 + hid_send_feature_report @13 + hid_get_feature_report @14 + \ No newline at end of file diff --git a/Engine/lib/sdl/src/hidapi/windows/ddk_build/makefile b/Engine/lib/sdl/src/hidapi/windows/ddk_build/makefile new file mode 100644 index 000000000..637f712a4 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/windows/ddk_build/makefile @@ -0,0 +1,49 @@ +############################################################################# +# +# Copyright (C) Microsoft Corporation 1995, 1996 +# All Rights Reserved. +# +# MAKEFILE for HID directory +# +############################################################################# + +!IFDEF WIN95_BUILD + +ROOT=..\..\..\.. + +VERSIONLIST = debug retail +IS_32 = TRUE +IS_SDK = TRUE +IS_PRIVATE = TRUE +IS_SDK = TRUE +IS_DDK = TRUE +WIN32 = TRUE +COMMONMKFILE = hidapi.mk + +!include $(ROOT)\dev\master.mk + + +!ELSE + +# +# DO NOT EDIT THIS FILE!!! Edit .\sources. if you want to add a new source +# file to this component. This file merely indirects to the real make file +# that is shared by all the driver components of the Windows NT DDK +# + +!IF DEFINED(_NT_TARGET_VERSION) +! IF $(_NT_TARGET_VERSION)>=0x501 +! INCLUDE $(NTMAKEENV)\makefile.def +! ELSE +# Only warn once per directory +! INCLUDE $(NTMAKEENV)\makefile.plt +! IF "$(BUILD_PASS)"=="PASS1" +! message BUILDMSG: Warning : The sample "$(MAKEDIR)" is not valid for the current OS target. +! ENDIF +! ENDIF +!ELSE +! INCLUDE $(NTMAKEENV)\makefile.def +!ENDIF + +!ENDIF + diff --git a/Engine/lib/sdl/src/hidapi/windows/ddk_build/sources b/Engine/lib/sdl/src/hidapi/windows/ddk_build/sources new file mode 100644 index 000000000..7f06a0963 --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/windows/ddk_build/sources @@ -0,0 +1,23 @@ +TARGETNAME=hidapi +TARGETTYPE=DYNLINK +UMTYPE=console +UMENTRY=main + +MSC_WARNING_LEVEL=/W3 /WX + +TARGETLIBS=$(SDK_LIB_PATH)\hid.lib \ + $(SDK_LIB_PATH)\setupapi.lib \ + $(SDK_LIB_PATH)\kernel32.lib \ + $(SDK_LIB_PATH)\comdlg32.lib + +USE_MSVCRT=1 + +INCLUDES= ..\..\hidapi +SOURCES= ..\hid.c \ + + +TARGET_DESTINATION=retail + +MUI=0 +MUI_COMMENT="HID Interface DLL" + diff --git a/Engine/lib/sdl/src/hidapi/windows/hid.c b/Engine/lib/sdl/src/hidapi/windows/hid.c new file mode 100644 index 000000000..3795e18ad --- /dev/null +++ b/Engine/lib/sdl/src/hidapi/windows/hid.c @@ -0,0 +1,988 @@ +/******************************************************* + HIDAPI - Multi-Platform library for + communication with HID devices. + + Alan Ott + Signal 11 Software + + 8/22/2009 + + Copyright 2009, All Rights Reserved. + + At the discretion of the user of this library, + this software may be licensed under the terms of the + GNU General Public License v3, a BSD-Style license, or the + original HIDAPI license as outlined in the LICENSE.txt, + LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt + files located at the root of the source distribution. + These files may also be found in the public source + code repository located at: + http://github.com/signal11/hidapi . +********************************************************/ +#include "../../SDL_internal.h" + +#ifdef SDL_JOYSTICK_HIDAPI + +#include + +#if 0 /* can cause redefinition errors on some toolchains */ +#ifdef __MINGW32__ +#include +#include +#endif + +#ifdef __CYGWIN__ +#include +#define _wcsdup wcsdup +#endif +#endif /* */ + +#ifndef _NTDEF_ +typedef LONG NTSTATUS; +#endif + +/* SDL C runtime functions */ +#include "SDL_stdinc.h" + +#define calloc SDL_calloc +#define free SDL_free +#define malloc SDL_malloc +#define memcpy SDL_memcpy +#define memset SDL_memset +#define strcmp SDL_strcmp +#define strlen SDL_strlen +#define strncpy SDL_strlcpy +#define strstr SDL_strstr +#define strtol SDL_strtol +#define wcscmp SDL_wcscmp +#define _wcsdup SDL_wcsdup + +/* The maximum number of characters that can be passed into the + HidD_Get*String() functions without it failing.*/ +#define MAX_STRING_WCHARS 0xFFF + +/*#define HIDAPI_USE_DDK*/ + +#ifdef __cplusplus +extern "C" { +#endif + #include + #include + #ifdef HIDAPI_USE_DDK + #include + #endif + + /* Copied from inc/ddk/hidclass.h, part of the Windows DDK. */ + #define HID_OUT_CTL_CODE(id) \ + CTL_CODE(FILE_DEVICE_KEYBOARD, (id), METHOD_OUT_DIRECT, FILE_ANY_ACCESS) + #define IOCTL_HID_GET_FEATURE HID_OUT_CTL_CODE(100) + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#include +#include + + +#include "../hidapi/hidapi.h" + +#undef MIN +#define MIN(x,y) ((x) < (y)? (x): (y)) + +#ifdef _MSC_VER + /* Thanks Microsoft, but I know how to use strncpy(). */ + #pragma warning(disable:4996) +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef HIDAPI_USE_DDK + /* Since we're not building with the DDK, and the HID header + files aren't part of the SDK, we have to define all this + stuff here. In lookup_functions(), the function pointers + defined below are set. */ + typedef struct _HIDD_ATTRIBUTES{ + ULONG Size; + USHORT VendorID; + USHORT ProductID; + USHORT VersionNumber; + } HIDD_ATTRIBUTES, *PHIDD_ATTRIBUTES; + + typedef USHORT USAGE; + typedef struct _HIDP_CAPS { + USAGE Usage; + USAGE UsagePage; + USHORT InputReportByteLength; + USHORT OutputReportByteLength; + USHORT FeatureReportByteLength; + USHORT Reserved[17]; + USHORT fields_not_used_by_hidapi[10]; + } HIDP_CAPS, *PHIDP_CAPS; + typedef void* PHIDP_PREPARSED_DATA; + #define HIDP_STATUS_SUCCESS 0x110000 + + typedef BOOLEAN (__stdcall *HidD_GetAttributes_)(HANDLE device, PHIDD_ATTRIBUTES attrib); + typedef BOOLEAN (__stdcall *HidD_GetSerialNumberString_)(HANDLE device, PVOID buffer, ULONG buffer_len); + typedef BOOLEAN (__stdcall *HidD_GetManufacturerString_)(HANDLE handle, PVOID buffer, ULONG buffer_len); + typedef BOOLEAN (__stdcall *HidD_GetProductString_)(HANDLE handle, PVOID buffer, ULONG buffer_len); + typedef BOOLEAN (__stdcall *HidD_SetFeature_)(HANDLE handle, PVOID data, ULONG length); + typedef BOOLEAN (__stdcall *HidD_GetFeature_)(HANDLE handle, PVOID data, ULONG length); + typedef BOOLEAN (__stdcall *HidD_GetIndexedString_)(HANDLE handle, ULONG string_index, PVOID buffer, ULONG buffer_len); + typedef BOOLEAN (__stdcall *HidD_GetPreparsedData_)(HANDLE handle, PHIDP_PREPARSED_DATA *preparsed_data); + typedef BOOLEAN (__stdcall *HidD_FreePreparsedData_)(PHIDP_PREPARSED_DATA preparsed_data); + typedef NTSTATUS (__stdcall *HidP_GetCaps_)(PHIDP_PREPARSED_DATA preparsed_data, HIDP_CAPS *caps); + typedef BOOLEAN (__stdcall *HidD_SetNumInputBuffers_)(HANDLE handle, ULONG number_buffers); + typedef BOOLEAN(__stdcall *HidD_SetOutputReport_ )(HANDLE handle, PVOID buffer, ULONG buffer_len); + static HidD_GetAttributes_ HidD_GetAttributes; + static HidD_GetSerialNumberString_ HidD_GetSerialNumberString; + static HidD_GetManufacturerString_ HidD_GetManufacturerString; + static HidD_GetProductString_ HidD_GetProductString; + static HidD_SetFeature_ HidD_SetFeature; + static HidD_GetFeature_ HidD_GetFeature; + static HidD_GetIndexedString_ HidD_GetIndexedString; + static HidD_GetPreparsedData_ HidD_GetPreparsedData; + static HidD_FreePreparsedData_ HidD_FreePreparsedData; + static HidP_GetCaps_ HidP_GetCaps; + static HidD_SetNumInputBuffers_ HidD_SetNumInputBuffers; + static HidD_SetOutputReport_ HidD_SetOutputReport; + + static HMODULE lib_handle = NULL; + static BOOLEAN initialized = FALSE; +#endif /* HIDAPI_USE_DDK */ + +struct hid_device_ { + HANDLE device_handle; + BOOL blocking; + USHORT output_report_length; + size_t input_report_length; + void *last_error_str; + DWORD last_error_num; + BOOL read_pending; + char *read_buf; + OVERLAPPED ol; +}; + +static hid_device *new_hid_device() +{ + hid_device *dev = (hid_device*) calloc(1, sizeof(hid_device)); + dev->device_handle = INVALID_HANDLE_VALUE; + dev->blocking = TRUE; + dev->output_report_length = 0; + dev->input_report_length = 0; + dev->last_error_str = NULL; + dev->last_error_num = 0; + dev->read_pending = FALSE; + dev->read_buf = NULL; + memset(&dev->ol, 0, sizeof(dev->ol)); + dev->ol.hEvent = CreateEvent(NULL, FALSE, FALSE /*initial state f=nonsignaled*/, NULL); + + return dev; +} + +static void free_hid_device(hid_device *dev) +{ + CloseHandle(dev->ol.hEvent); + CloseHandle(dev->device_handle); + LocalFree(dev->last_error_str); + free(dev->read_buf); + free(dev); +} + +static void register_error(hid_device *device, const char *op) +{ + WCHAR *ptr, *msg; + + DWORD count = FormatMessageW(FORMAT_MESSAGE_ALLOCATE_BUFFER | + FORMAT_MESSAGE_FROM_SYSTEM | + FORMAT_MESSAGE_IGNORE_INSERTS, + NULL, + GetLastError(), + MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), + (LPWSTR)&msg, 0/*sz*/, + NULL); + if (!count) + return; + + /* Get rid of the CR and LF that FormatMessage() sticks at the + end of the message. Thanks Microsoft! */ + ptr = msg; + while (*ptr) { + if (*ptr == '\r') { + *ptr = 0x0000; + break; + } + ptr++; + } + + /* Store the message off in the Device entry so that + the hid_error() function can pick it up. */ + LocalFree(device->last_error_str); + device->last_error_str = msg; +} + +#ifndef HIDAPI_USE_DDK +static int lookup_functions() +{ + lib_handle = LoadLibraryA("hid.dll"); + if (lib_handle) { +#define RESOLVE(x) x = (x##_)GetProcAddress(lib_handle, #x); if (!x) return -1; + RESOLVE(HidD_GetAttributes); + RESOLVE(HidD_GetSerialNumberString); + RESOLVE(HidD_GetManufacturerString); + RESOLVE(HidD_GetProductString); + RESOLVE(HidD_SetFeature); + RESOLVE(HidD_GetFeature); + RESOLVE(HidD_GetIndexedString); + RESOLVE(HidD_GetPreparsedData); + RESOLVE(HidD_FreePreparsedData); + RESOLVE(HidP_GetCaps); + RESOLVE(HidD_SetNumInputBuffers); + RESOLVE(HidD_SetOutputReport); +#undef RESOLVE + } + else + return -1; + + return 0; +} +#endif + +static HANDLE open_device(const char *path, BOOL enumerate, BOOL bExclusive ) +{ + HANDLE handle; + // Opening with access 0 causes keyboards to stop responding in some system configurations + // http://steamcommunity.com/discussions/forum/1/1843493219428923893 + // Thanks to co-wie (Ka-wei Low ) for help narrowing down the problem on his system + //DWORD desired_access = (enumerate)? 0: (GENERIC_WRITE | GENERIC_READ); + DWORD desired_access = ( GENERIC_WRITE | GENERIC_READ ); + DWORD share_mode = bExclusive ? 0 : ( FILE_SHARE_READ | FILE_SHARE_WRITE ); + + handle = CreateFileA(path, + desired_access, + share_mode, + NULL, + OPEN_EXISTING, + FILE_FLAG_OVERLAPPED,/*FILE_ATTRIBUTE_NORMAL,*/ + 0); + + return handle; +} + +int HID_API_EXPORT hid_init(void) +{ +#ifndef HIDAPI_USE_DDK + if (!initialized) { + if (lookup_functions() < 0) { + hid_exit(); + return -1; + } + initialized = TRUE; + } +#endif + return 0; +} + +int HID_API_EXPORT hid_exit(void) +{ +#ifndef HIDAPI_USE_DDK + if (lib_handle) + FreeLibrary(lib_handle); + lib_handle = NULL; + initialized = FALSE; +#endif + return 0; +} + +struct hid_device_info HID_API_EXPORT * HID_API_CALL hid_enumerate(unsigned short vendor_id, unsigned short product_id) +{ + BOOL res; + struct hid_device_info *root = NULL; /* return object */ + struct hid_device_info *cur_dev = NULL; + + /* Windows objects for interacting with the driver. */ + GUID InterfaceClassGuid = {0x4d1e55b2, 0xf16f, 0x11cf, {0x88, 0xcb, 0x00, 0x11, 0x11, 0x00, 0x00, 0x30} }; + SP_DEVINFO_DATA devinfo_data; + SP_DEVICE_INTERFACE_DATA device_interface_data; + SP_DEVICE_INTERFACE_DETAIL_DATA_A *device_interface_detail_data = NULL; + HDEVINFO device_info_set = INVALID_HANDLE_VALUE; + int device_index = 0; + int i; + + if (hid_init() < 0) + return NULL; + + /* Initialize the Windows objects. */ + memset(&devinfo_data, 0x0, sizeof(devinfo_data)); + devinfo_data.cbSize = sizeof(SP_DEVINFO_DATA); + device_interface_data.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); + + /* Get information for all the devices belonging to the HID class. */ + device_info_set = SetupDiGetClassDevsA(&InterfaceClassGuid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); + + /* Iterate over each device in the HID class, looking for the right one. */ + + for (;;) { + HANDLE write_handle = INVALID_HANDLE_VALUE; + DWORD required_size = 0; + HIDD_ATTRIBUTES attrib; + + res = SetupDiEnumDeviceInterfaces(device_info_set, + NULL, + &InterfaceClassGuid, + device_index, + &device_interface_data); + + if (!res) { + /* A return of FALSE from this function means that + there are no more devices. */ + break; + } + + /* Call with 0-sized detail size, and let the function + tell us how long the detail struct needs to be. The + size is put in &required_size. */ + res = SetupDiGetDeviceInterfaceDetailA(device_info_set, + &device_interface_data, + NULL, + 0, + &required_size, + NULL); + + /* Allocate a long enough structure for device_interface_detail_data. */ + device_interface_detail_data = (SP_DEVICE_INTERFACE_DETAIL_DATA_A*) malloc(required_size); + device_interface_detail_data->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A); + + /* Get the detailed data for this device. The detail data gives us + the device path for this device, which is then passed into + CreateFile() to get a handle to the device. */ + res = SetupDiGetDeviceInterfaceDetailA(device_info_set, + &device_interface_data, + device_interface_detail_data, + required_size, + NULL, + NULL); + + if (!res) { + /* register_error(dev, "Unable to call SetupDiGetDeviceInterfaceDetail"); + Continue to the next device. */ + goto cont; + } + + /* Make sure this device is of Setup Class "HIDClass" and has a + driver bound to it. */ + for (i = 0; ; i++) { + char driver_name[256]; + + /* Populate devinfo_data. This function will return failure + when there are no more interfaces left. */ + res = SetupDiEnumDeviceInfo(device_info_set, i, &devinfo_data); + if (!res) + goto cont; + + res = SetupDiGetDeviceRegistryPropertyA(device_info_set, &devinfo_data, + SPDRP_CLASS, NULL, (PBYTE)driver_name, sizeof(driver_name), NULL); + if (!res) + goto cont; + + if (strcmp(driver_name, "HIDClass") == 0) { + /* See if there's a driver bound. */ + res = SetupDiGetDeviceRegistryPropertyA(device_info_set, &devinfo_data, + SPDRP_DRIVER, NULL, (PBYTE)driver_name, sizeof(driver_name), NULL); + if (res) + break; + } + } + + //wprintf(L"HandleName: %s\n", device_interface_detail_data->DevicePath); + + /* Open a handle to the device */ + write_handle = open_device(device_interface_detail_data->DevicePath, TRUE, FALSE); + + /* Check validity of write_handle. */ + if (write_handle == INVALID_HANDLE_VALUE) { + /* Unable to open the device. */ + //register_error(dev, "CreateFile"); + goto cont_close; + } + + + /* Get the Vendor ID and Product ID for this device. */ + attrib.Size = sizeof(HIDD_ATTRIBUTES); + HidD_GetAttributes(write_handle, &attrib); + //wprintf(L"Product/Vendor: %x %x\n", attrib.ProductID, attrib.VendorID); + + /* Check the VID/PID to see if we should add this + device to the enumeration list. */ + if ((vendor_id == 0x0 || attrib.VendorID == vendor_id) && + (product_id == 0x0 || attrib.ProductID == product_id)) { + + #define WSTR_LEN 512 + const char *str; + struct hid_device_info *tmp; + PHIDP_PREPARSED_DATA pp_data = NULL; + HIDP_CAPS caps; + BOOLEAN hidp_res; + NTSTATUS nt_res; + wchar_t wstr[WSTR_LEN]; /* TODO: Determine Size */ + size_t len; + + /* VID/PID match. Create the record. */ + tmp = (struct hid_device_info*) calloc(1, sizeof(struct hid_device_info)); + if (cur_dev) { + cur_dev->next = tmp; + } + else { + root = tmp; + } + cur_dev = tmp; + + /* Get the Usage Page and Usage for this device. */ + hidp_res = HidD_GetPreparsedData(write_handle, &pp_data); + if (hidp_res) { + nt_res = HidP_GetCaps(pp_data, &caps); + if (nt_res == HIDP_STATUS_SUCCESS) { + cur_dev->usage_page = caps.UsagePage; + cur_dev->usage = caps.Usage; + } + + HidD_FreePreparsedData(pp_data); + } + + /* Fill out the record */ + cur_dev->next = NULL; + str = device_interface_detail_data->DevicePath; + if (str) { + len = strlen(str); + cur_dev->path = (char*) calloc(len+1, sizeof(char)); + strncpy(cur_dev->path, str, len+1); + cur_dev->path[len] = '\0'; + } + else + cur_dev->path = NULL; + + /* Serial Number */ + hidp_res = HidD_GetSerialNumberString(write_handle, wstr, sizeof(wstr)); + wstr[WSTR_LEN-1] = 0x0000; + if (hidp_res) { + cur_dev->serial_number = _wcsdup(wstr); + } + + /* Manufacturer String */ + hidp_res = HidD_GetManufacturerString(write_handle, wstr, sizeof(wstr)); + wstr[WSTR_LEN-1] = 0x0000; + if (hidp_res) { + cur_dev->manufacturer_string = _wcsdup(wstr); + } + + /* Product String */ + hidp_res = HidD_GetProductString(write_handle, wstr, sizeof(wstr)); + wstr[WSTR_LEN-1] = 0x0000; + if (hidp_res) { + cur_dev->product_string = _wcsdup(wstr); + } + + /* VID/PID */ + cur_dev->vendor_id = attrib.VendorID; + cur_dev->product_id = attrib.ProductID; + + /* Release Number */ + cur_dev->release_number = attrib.VersionNumber; + + /* Interface Number. It can sometimes be parsed out of the path + on Windows if a device has multiple interfaces. See + http://msdn.microsoft.com/en-us/windows/hardware/gg487473 or + search for "Hardware IDs for HID Devices" at MSDN. If it's not + in the path, it's set to -1. */ + cur_dev->interface_number = -1; + if (cur_dev->path) { + char *interface_component = strstr(cur_dev->path, "&mi_"); + if (interface_component) { + char *hex_str = interface_component + 4; + char *endptr = NULL; + cur_dev->interface_number = strtol(hex_str, &endptr, 16); + if (endptr == hex_str) { + /* The parsing failed. Set interface_number to -1. */ + cur_dev->interface_number = -1; + } + } + } + } + +cont_close: + CloseHandle(write_handle); +cont: + /* We no longer need the detail data. It can be freed */ + free(device_interface_detail_data); + + device_index++; + + } + + /* Close the device information handle. */ + SetupDiDestroyDeviceInfoList(device_info_set); + + return root; + +} + +void HID_API_EXPORT HID_API_CALL hid_free_enumeration(struct hid_device_info *devs) +{ + /* TODO: Merge this with the Linux version. This function is platform-independent. */ + struct hid_device_info *d = devs; + while (d) { + struct hid_device_info *next = d->next; + free(d->path); + free(d->serial_number); + free(d->manufacturer_string); + free(d->product_string); + free(d); + d = next; + } +} + + +HID_API_EXPORT hid_device * HID_API_CALL hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number) +{ + /* TODO: Merge this functions with the Linux version. This function should be platform independent. */ + struct hid_device_info *devs, *cur_dev; + const char *path_to_open = NULL; + hid_device *handle = NULL; + + devs = hid_enumerate(vendor_id, product_id); + cur_dev = devs; + while (cur_dev) { + if (cur_dev->vendor_id == vendor_id && + cur_dev->product_id == product_id) { + if (serial_number) { + if (wcscmp(serial_number, cur_dev->serial_number) == 0) { + path_to_open = cur_dev->path; + break; + } + } + else { + path_to_open = cur_dev->path; + break; + } + } + cur_dev = cur_dev->next; + } + + if (path_to_open) { + /* Open the device */ + handle = hid_open_path(path_to_open, 0); + } + + hid_free_enumeration(devs); + + return handle; +} + +HID_API_EXPORT hid_device * HID_API_CALL hid_open_path(const char *path, int bExclusive) +{ + hid_device *dev; + HIDP_CAPS caps; + PHIDP_PREPARSED_DATA pp_data = NULL; + BOOLEAN res; + NTSTATUS nt_res; + + if (hid_init() < 0) { + return NULL; + } + + dev = new_hid_device(); + + /* Open a handle to the device */ + dev->device_handle = open_device(path, FALSE, bExclusive); + + /* Check validity of write_handle. */ + if (dev->device_handle == INVALID_HANDLE_VALUE) { + /* Unable to open the device. */ + register_error(dev, "CreateFile"); + goto err; + } + + /* Set the Input Report buffer size to 64 reports. */ + res = HidD_SetNumInputBuffers(dev->device_handle, 64); + if (!res) { + register_error(dev, "HidD_SetNumInputBuffers"); + goto err; + } + + /* Get the Input Report length for the device. */ + res = HidD_GetPreparsedData(dev->device_handle, &pp_data); + if (!res) { + register_error(dev, "HidD_GetPreparsedData"); + goto err; + } + nt_res = HidP_GetCaps(pp_data, &caps); + if (nt_res != HIDP_STATUS_SUCCESS) { + register_error(dev, "HidP_GetCaps"); + goto err_pp_data; + } + dev->output_report_length = caps.OutputReportByteLength; + dev->input_report_length = caps.InputReportByteLength; + HidD_FreePreparsedData(pp_data); + + dev->read_buf = (char*) malloc(dev->input_report_length); + + return dev; + +err_pp_data: + HidD_FreePreparsedData(pp_data); +err: + free_hid_device(dev); + return NULL; +} + +int HID_API_EXPORT HID_API_CALL hid_write_output_report(hid_device *dev, const unsigned char *data, size_t length) +{ + BOOL res; + res = HidD_SetOutputReport(dev->device_handle, (void *)data, (ULONG)length); + if (res) + return (int)length; + else + return -1; +} + +int HID_API_EXPORT HID_API_CALL hid_write(hid_device *dev, const unsigned char *data, size_t length) +{ + DWORD bytes_written; + BOOL res; + size_t stashed_length = length; + OVERLAPPED ol; + unsigned char *buf; + memset(&ol, 0, sizeof(ol)); + + /* Make sure the right number of bytes are passed to WriteFile. Windows + expects the number of bytes which are in the _longest_ report (plus + one for the report number) bytes even if the data is a report + which is shorter than that. Windows gives us this value in + caps.OutputReportByteLength. If a user passes in fewer bytes than this, + create a temporary buffer which is the proper size. */ + if (length >= dev->output_report_length) { + /* The user passed the right number of bytes. Use the buffer as-is. */ + buf = (unsigned char *) data; + } else { + /* Create a temporary buffer and copy the user's data + into it, padding the rest with zeros. */ + buf = (unsigned char *) malloc(dev->output_report_length); + memcpy(buf, data, length); + memset(buf + length, 0, dev->output_report_length - length); + length = dev->output_report_length; + } + if (length > 512) + { + return hid_write_output_report( dev, data, stashed_length ); + } + else + { + res = WriteFile( dev->device_handle, buf, ( DWORD ) length, NULL, &ol ); + if (!res) { + if (GetLastError() != ERROR_IO_PENDING) { + /* WriteFile() failed. Return error. */ + register_error(dev, "WriteFile"); + bytes_written = (DWORD) -1; + goto end_of_function; + } + } + + /* Wait here until the write is done. This makes + hid_write() synchronous. */ + res = GetOverlappedResult(dev->device_handle, &ol, &bytes_written, TRUE/*wait*/); + if (!res) { + /* The Write operation failed. */ + register_error(dev, "WriteFile"); + bytes_written = (DWORD) -1; + goto end_of_function; + } + } +end_of_function: + if (buf != data) + free(buf); + + return bytes_written; +} + + +int HID_API_EXPORT HID_API_CALL hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds) +{ + DWORD bytes_read = 0; + size_t copy_len = 0; + BOOL res; + + /* Copy the handle for convenience. */ + HANDLE ev = dev->ol.hEvent; + + if (!dev->read_pending) { + /* Start an Overlapped I/O read. */ + dev->read_pending = TRUE; + memset(dev->read_buf, 0, dev->input_report_length); + ResetEvent(ev); + res = ReadFile(dev->device_handle, dev->read_buf, (DWORD)dev->input_report_length, &bytes_read, &dev->ol); + + if (!res) { + if (GetLastError() != ERROR_IO_PENDING) { + /* ReadFile() has failed. + Clean up and return error. */ + CancelIo(dev->device_handle); + dev->read_pending = FALSE; + goto end_of_function; + } + } + } + + if (milliseconds >= 0) { + /* See if there is any data yet. */ + res = WaitForSingleObject(ev, milliseconds); + if (res != WAIT_OBJECT_0) { + /* There was no data this time. Return zero bytes available, + but leave the Overlapped I/O running. */ + return 0; + } + } + + /* Either WaitForSingleObject() told us that ReadFile has completed, or + we are in non-blocking mode. Get the number of bytes read. The actual + data has been copied to the data[] array which was passed to ReadFile(). */ + res = GetOverlappedResult(dev->device_handle, &dev->ol, &bytes_read, TRUE/*wait*/); + + /* Set pending back to false, even if GetOverlappedResult() returned error. */ + dev->read_pending = FALSE; + + if (res && bytes_read > 0) { + if (dev->read_buf[0] == 0x0) { + /* If report numbers aren't being used, but Windows sticks a report + number (0x0) on the beginning of the report anyway. To make this + work like the other platforms, and to make it work more like the + HID spec, we'll skip over this byte. */ + bytes_read--; + copy_len = length > bytes_read ? bytes_read : length; + memcpy(data, dev->read_buf+1, copy_len); + } + else { + /* Copy the whole buffer, report number and all. */ + copy_len = length > bytes_read ? bytes_read : length; + memcpy(data, dev->read_buf, copy_len); + } + } + +end_of_function: + if (!res) { + register_error(dev, "GetOverlappedResult"); + return -1; + } + + return (int)copy_len; +} + +int HID_API_EXPORT HID_API_CALL hid_read(hid_device *dev, unsigned char *data, size_t length) +{ + return hid_read_timeout(dev, data, length, (dev->blocking)? -1: 0); +} + +int HID_API_EXPORT HID_API_CALL hid_set_nonblocking(hid_device *dev, int nonblock) +{ + dev->blocking = !nonblock; + return 0; /* Success */ +} + +int HID_API_EXPORT HID_API_CALL hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length) +{ + BOOL res = HidD_SetFeature(dev->device_handle, (PVOID)data, (ULONG)length); + if (!res) { + register_error(dev, "HidD_SetFeature"); + return -1; + } + + return (int)length; +} + + +int HID_API_EXPORT HID_API_CALL hid_get_feature_report(hid_device *dev, unsigned char *data, size_t length) +{ + BOOL res; +#if 0 + res = HidD_GetFeature(dev->device_handle, (PVOID)data, (ULONG)length); + if (!res) { + register_error(dev, "HidD_GetFeature"); + return -1; + } + return 0; /* HidD_GetFeature() doesn't give us an actual length, unfortunately */ +#else + DWORD bytes_returned; + + OVERLAPPED ol; + memset(&ol, 0, sizeof(ol)); + + res = DeviceIoControl(dev->device_handle, + IOCTL_HID_GET_FEATURE, + data, (DWORD)length, + data, (DWORD)length, + &bytes_returned, &ol); + + if (!res) { + if (GetLastError() != ERROR_IO_PENDING) { + /* DeviceIoControl() failed. Return error. */ + register_error(dev, "Send Feature Report DeviceIoControl"); + return -1; + } + } + + /* Wait here until the write is done. This makes + hid_get_feature_report() synchronous. */ + res = GetOverlappedResult(dev->device_handle, &ol, &bytes_returned, TRUE/*wait*/); + if (!res) { + /* The operation failed. */ + register_error(dev, "Send Feature Report GetOverLappedResult"); + return -1; + } + + /* bytes_returned does not include the first byte which contains the + report ID. The data buffer actually contains one more byte than + bytes_returned. */ + bytes_returned++; + + + return bytes_returned; +#endif +} + +void HID_API_EXPORT HID_API_CALL hid_close(hid_device *dev) +{ + if (!dev) + return; + CancelIo(dev->device_handle); + free_hid_device(dev); +} + +int HID_API_EXPORT_CALL HID_API_CALL hid_get_manufacturer_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + BOOL res; + + res = HidD_GetManufacturerString(dev->device_handle, string, (ULONG)(sizeof(wchar_t) * MIN(maxlen, MAX_STRING_WCHARS))); + if (!res) { + register_error(dev, "HidD_GetManufacturerString"); + return -1; + } + + return 0; +} + +int HID_API_EXPORT_CALL HID_API_CALL hid_get_product_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + BOOL res; + + res = HidD_GetProductString(dev->device_handle, string, (ULONG)(sizeof(wchar_t) * MIN(maxlen, MAX_STRING_WCHARS))); + if (!res) { + register_error(dev, "HidD_GetProductString"); + return -1; + } + + return 0; +} + +int HID_API_EXPORT_CALL HID_API_CALL hid_get_serial_number_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + BOOL res; + + res = HidD_GetSerialNumberString(dev->device_handle, string, (ULONG)(sizeof(wchar_t) * MIN(maxlen, MAX_STRING_WCHARS))); + if (!res) { + register_error(dev, "HidD_GetSerialNumberString"); + return -1; + } + + return 0; +} + +int HID_API_EXPORT_CALL HID_API_CALL hid_get_indexed_string(hid_device *dev, int string_index, wchar_t *string, size_t maxlen) +{ + BOOL res; + + res = HidD_GetIndexedString(dev->device_handle, string_index, string, (ULONG)(sizeof(wchar_t) * MIN(maxlen, MAX_STRING_WCHARS))); + if (!res) { + register_error(dev, "HidD_GetIndexedString"); + return -1; + } + + return 0; +} + +HID_API_EXPORT const wchar_t * HID_API_CALL hid_error(hid_device *dev) +{ + return (wchar_t*)dev->last_error_str; +} + + +#if 0 + +/*#define PICPGM*/ +/*#define S11*/ +#define P32 +#ifdef S11 + unsigned short VendorID = 0xa0a0; + unsigned short ProductID = 0x0001; +#endif + +#ifdef P32 + unsigned short VendorID = 0x04d8; + unsigned short ProductID = 0x3f; +#endif + +#ifdef PICPGM + unsigned short VendorID = 0x04d8; + unsigned short ProductID = 0x0033; +#endif + +int __cdecl main(int argc, char* argv[]) +{ + int res; + unsigned char buf[65]; + + UNREFERENCED_PARAMETER(argc); + UNREFERENCED_PARAMETER(argv); + + /* Set up the command buffer. */ + memset(buf,0x00,sizeof(buf)); + buf[0] = 0; + buf[1] = 0x81; + + + /* Open the device. */ + int handle = open(VendorID, ProductID, L"12345"); + if (handle < 0) + printf("unable to open device\n"); + + + /* Toggle LED (cmd 0x80) */ + buf[1] = 0x80; + res = write(handle, buf, 65); + if (res < 0) + printf("Unable to write()\n"); + + /* Request state (cmd 0x81) */ + buf[1] = 0x81; + write(handle, buf, 65); + if (res < 0) + printf("Unable to write() (2)\n"); + + /* Read requested state */ + read(handle, buf, 65); + if (res < 0) + printf("Unable to read()\n"); + + /* Print out the returned buffer. */ + for (int i = 0; i < 4; i++) + printf("buf[%d]: %d\n", i, buf[i]); + + return 0; +} +#endif + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /* SDL_JOYSTICK_HIDAPI */ diff --git a/Engine/lib/sdl/src/joystick/SDL_gamecontroller.c b/Engine/lib/sdl/src/joystick/SDL_gamecontroller.c index b0f833a33..eb1ef0654 100644 --- a/Engine/lib/sdl/src/joystick/SDL_gamecontroller.c +++ b/Engine/lib/sdl/src/joystick/SDL_gamecontroller.c @@ -25,6 +25,7 @@ #include "SDL_events.h" #include "SDL_assert.h" #include "SDL_hints.h" +#include "SDL_timer.h" #include "SDL_sysjoystick.h" #include "SDL_joystick_c.h" #include "SDL_gamecontrollerdb.h" @@ -38,6 +39,9 @@ #endif +/* Many controllers turn the center button into an instantaneous button press */ +#define SDL_MINIMUM_GUIDE_BUTTON_DELAY_MS 250 + #define SDL_CONTROLLER_PLATFORM_FIELD "platform:" /* a list of currently opened game controllers */ @@ -97,8 +101,9 @@ typedef struct _ControllerMapping_t static SDL_JoystickGUID s_zeroGUID; static ControllerMapping_t *s_pSupportedControllers = NULL; +static ControllerMapping_t *s_pDefaultMapping = NULL; +static ControllerMapping_t *s_pHIDAPIMapping = NULL; static ControllerMapping_t *s_pXInputMapping = NULL; -static ControllerMapping_t *s_pEmscriptenMapping = NULL; /* The SDL game controller structure */ struct _SDL_GameController @@ -106,12 +111,12 @@ struct _SDL_GameController SDL_Joystick *joystick; /* underlying joystick device */ int ref_count; - SDL_JoystickGUID guid; const char *name; int num_bindings; SDL_ExtendedGameControllerBind *bindings; SDL_ExtendedGameControllerBind **last_match_axis; Uint8 *last_hat_mask; + Uint32 guide_button_down; struct _SDL_GameController *next; /* pointer to next game controller we have allocated */ }; @@ -416,7 +421,7 @@ static int SDLCALL SDL_GameControllerEventWatcher(void *userdata, SDL_Event * ev /* * Helper function to scan the mappings database for a controller with the specified GUID */ -static ControllerMapping_t *SDL_PrivateGetControllerMappingForGUID(SDL_JoystickGUID *guid) +static ControllerMapping_t *SDL_PrivateGetControllerMappingForGUID(SDL_JoystickGUID *guid, SDL_bool exact_match) { ControllerMapping_t *pSupportedController = s_pSupportedControllers; while (pSupportedController) { @@ -425,6 +430,18 @@ static ControllerMapping_t *SDL_PrivateGetControllerMappingForGUID(SDL_JoystickG } pSupportedController = pSupportedController->next; } + if (!exact_match) { + if (SDL_IsJoystickHIDAPI(*guid)) { + /* This is a HIDAPI device */ + return s_pHIDAPIMapping; + } +#if SDL_JOYSTICK_XINPUT + if (SDL_IsJoystickXInput(*guid)) { + /* This is an XInput device */ + return s_pXInputMapping; + } +#endif + } return NULL; } @@ -665,11 +682,10 @@ SDL_PrivateGameControllerParseControllerConfigString(SDL_GameController *gamecon /* * Make a new button mapping struct */ -static void SDL_PrivateLoadButtonMapping(SDL_GameController *gamecontroller, SDL_JoystickGUID guid, const char *pchName, const char *pchMapping) +static void SDL_PrivateLoadButtonMapping(SDL_GameController *gamecontroller, const char *pchName, const char *pchMapping) { int i; - gamecontroller->guid = guid; gamecontroller->name = pchName; gamecontroller->num_bindings = 0; SDL_memset(gamecontroller->last_match_axis, 0, gamecontroller->joystick->naxes * sizeof(*gamecontroller->last_match_axis)); @@ -783,14 +799,16 @@ static void SDL_PrivateGameControllerRefreshMapping(ControllerMapping_t *pContro { SDL_GameController *gamecontrollerlist = SDL_gamecontrollers; while (gamecontrollerlist) { - if (!SDL_memcmp(&gamecontrollerlist->guid, &pControllerMapping->guid, sizeof(pControllerMapping->guid))) { - SDL_Event event; - event.type = SDL_CONTROLLERDEVICEREMAPPED; - event.cdevice.which = gamecontrollerlist->joystick->instance_id; - SDL_PushEvent(&event); - + if (!SDL_memcmp(&gamecontrollerlist->joystick->guid, &pControllerMapping->guid, sizeof(pControllerMapping->guid))) { /* Not really threadsafe. Should this lock access within SDL_GameControllerEventWatcher? */ - SDL_PrivateLoadButtonMapping(gamecontrollerlist, pControllerMapping->guid, pControllerMapping->name, pControllerMapping->mapping); + SDL_PrivateLoadButtonMapping(gamecontrollerlist, pControllerMapping->name, pControllerMapping->mapping); + + { + SDL_Event event; + event.type = SDL_CONTROLLERDEVICEREMAPPED; + event.cdevice.which = gamecontrollerlist->joystick->instance_id; + SDL_PushEvent(&event); + } } gamecontrollerlist = gamecontrollerlist->next; @@ -820,7 +838,7 @@ SDL_PrivateAddMappingForGUID(SDL_JoystickGUID jGUID, const char *mappingString, return NULL; } - pControllerMapping = SDL_PrivateGetControllerMappingForGUID(&jGUID); + pControllerMapping = SDL_PrivateGetControllerMappingForGUID(&jGUID, SDL_TRUE); if (pControllerMapping) { /* Only overwrite the mapping if the priority is the same or higher. */ if (pControllerMapping->priority <= priority) { @@ -869,6 +887,119 @@ SDL_PrivateAddMappingForGUID(SDL_JoystickGUID jGUID, const char *mappingString, return pControllerMapping; } +#ifdef __ANDROID__ +/* + * Helper function to guess at a mapping based on the elements reported for this controller + */ +static ControllerMapping_t *SDL_CreateMappingForAndroidController(const char *name, SDL_JoystickGUID guid) +{ + SDL_bool existing; + char name_string[128]; + char mapping_string[1024]; + int button_mask; + int axis_mask; + + button_mask = SDL_SwapLE16(*(Uint16*)(&guid.data[sizeof(guid.data)-4])); + axis_mask = SDL_SwapLE16(*(Uint16*)(&guid.data[sizeof(guid.data)-2])); + if (!button_mask && !axis_mask) { + /* Accelerometer, shouldn't have a game controller mapping */ + return NULL; + } + + /* Remove any commas in the name */ + SDL_strlcpy(name_string, name, sizeof(name_string)); + { + char *spot; + for (spot = name_string; *spot; ++spot) { + if (*spot == ',') { + *spot = ' '; + } + } + } + SDL_snprintf(mapping_string, sizeof(mapping_string), "none,%s,", name_string); + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_A)) { + SDL_strlcat(mapping_string, "a:b0,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_B)) { + SDL_strlcat(mapping_string, "b:b1,", sizeof(mapping_string)); + } else if (button_mask & (1 << SDL_CONTROLLER_BUTTON_BACK)) { + /* Use the back button as "B" for easy UI navigation with TV remotes */ + SDL_strlcat(mapping_string, "b:b4,", sizeof(mapping_string)); + button_mask &= ~(1 << SDL_CONTROLLER_BUTTON_BACK); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_X)) { + SDL_strlcat(mapping_string, "x:b2,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_Y)) { + SDL_strlcat(mapping_string, "y:b3,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_BACK)) { + SDL_strlcat(mapping_string, "back:b4,", sizeof(mapping_string)); + } +#if 0 /* The guide button generally isn't functional (or acts as a home button) on most Android controllers */ + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_GUIDE)) { + SDL_strlcat(mapping_string, "guide:b5,", sizeof(mapping_string)); +#if 0 /* Actually this will be done in Steam */ + } else if (button_mask & (1 << SDL_CONTROLLER_BUTTON_START)) { + /* The guide button doesn't exist, use the start button instead, + so you can do Steam guide button chords and open the Steam overlay. + */ + SDL_strlcat(mapping_string, "guide:b6,", sizeof(mapping_string)); + button_mask &= ~(1 << SDL_CONTROLLER_BUTTON_START); +#endif + } +#endif + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_START)) { + SDL_strlcat(mapping_string, "start:b6,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_LEFTSTICK)) { + SDL_strlcat(mapping_string, "leftstick:b7,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_RIGHTSTICK)) { + SDL_strlcat(mapping_string, "rightstick:b8,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_LEFTSHOULDER)) { + SDL_strlcat(mapping_string, "leftshoulder:b9,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_RIGHTSHOULDER)) { + SDL_strlcat(mapping_string, "rightshoulder:b10,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_DPAD_UP)) { + SDL_strlcat(mapping_string, "dpup:b11,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_DPAD_DOWN)) { + SDL_strlcat(mapping_string, "dpdown:b12,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_DPAD_LEFT)) { + SDL_strlcat(mapping_string, "dpleft:b13,", sizeof(mapping_string)); + } + if (button_mask & (1 << SDL_CONTROLLER_BUTTON_DPAD_RIGHT)) { + SDL_strlcat(mapping_string, "dpright:b14,", sizeof(mapping_string)); + } + if (axis_mask & (1 << SDL_CONTROLLER_AXIS_LEFTX)) { + SDL_strlcat(mapping_string, "leftx:a0,", sizeof(mapping_string)); + } + if (axis_mask & (1 << SDL_CONTROLLER_AXIS_LEFTY)) { + SDL_strlcat(mapping_string, "lefty:a1,", sizeof(mapping_string)); + } + if (axis_mask & (1 << SDL_CONTROLLER_AXIS_RIGHTX)) { + SDL_strlcat(mapping_string, "rightx:a2,", sizeof(mapping_string)); + } + if (axis_mask & (1 << SDL_CONTROLLER_AXIS_RIGHTY)) { + SDL_strlcat(mapping_string, "righty:a3,", sizeof(mapping_string)); + } + if (axis_mask & (1 << SDL_CONTROLLER_AXIS_TRIGGERLEFT)) { + SDL_strlcat(mapping_string, "lefttrigger:a4,", sizeof(mapping_string)); + } + if (axis_mask & (1 << SDL_CONTROLLER_AXIS_TRIGGERRIGHT)) { + SDL_strlcat(mapping_string, "righttrigger:a5,", sizeof(mapping_string)); + } + return SDL_PrivateAddMappingForGUID(guid, mapping_string, + &existing, SDL_CONTROLLER_MAPPING_PRIORITY_DEFAULT); +} +#endif /* __ANDROID__ */ + + /* * Helper function to determine pre-calculated offset to certain joystick mappings */ @@ -876,14 +1007,7 @@ static ControllerMapping_t *SDL_PrivateGetControllerMappingForNameAndGUID(const { ControllerMapping_t *mapping; - mapping = SDL_PrivateGetControllerMappingForGUID(&guid); -#if defined(SDL_JOYSTICK_EMSCRIPTEN) - if (!mapping && s_pEmscriptenMapping) { - mapping = s_pEmscriptenMapping; - } -#else - (void) s_pEmscriptenMapping; /* pacify ARMCC */ -#endif + mapping = SDL_PrivateGetControllerMappingForGUID(&guid, SDL_FALSE); #ifdef __LINUX__ if (!mapping && name) { if (SDL_strstr(name, "Xbox 360 Wireless Receiver")) { @@ -901,6 +1025,14 @@ static ControllerMapping_t *SDL_PrivateGetControllerMappingForNameAndGUID(const mapping = s_pXInputMapping; } } +#ifdef __ANDROID__ + if (!mapping && name && !SDL_IsJoystickHIDAPI(guid)) { + mapping = SDL_CreateMappingForAndroidController(name, guid); + } +#endif + if (!mapping) { + mapping = s_pDefaultMapping; + } return mapping; } @@ -921,20 +1053,6 @@ static ControllerMapping_t *SDL_PrivateGetControllerMapping(int device_index) name = SDL_JoystickNameForIndex(device_index); guid = SDL_JoystickGetDeviceGUID(device_index); mapping = SDL_PrivateGetControllerMappingForNameAndGUID(name, guid); -#if SDL_JOYSTICK_XINPUT - if (!mapping && SDL_SYS_IsXInputGamepad_DeviceIndex(device_index)) { - mapping = s_pXInputMapping; - } -#endif -#if defined(__ANDROID__) - if (!mapping && SDL_SYS_IsDPAD_DeviceIndex(device_index)) { - SDL_bool existing; - char mapping_string[1024]; - SDL_snprintf(mapping_string, sizeof(mapping_string), "none,%s,a:b0,b:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,", name); - mapping = SDL_PrivateAddMappingForGUID(guid, mapping_string, - &existing, SDL_CONTROLLER_MAPPING_PRIORITY_DEFAULT); - } -#endif /* __ANDROID__ */ SDL_UnlockJoysticks(); return mapping; } @@ -1018,8 +1136,9 @@ SDL_PrivateGameControllerAddMapping(const char *mappingString, SDL_ControllerMap { char *pchGUID; SDL_JoystickGUID jGUID; + SDL_bool is_default_mapping = SDL_FALSE; + SDL_bool is_hidapi_mapping = SDL_FALSE; SDL_bool is_xinput_mapping = SDL_FALSE; - SDL_bool is_emscripten_mapping = SDL_FALSE; SDL_bool existing = SDL_FALSE; ControllerMapping_t *pControllerMapping; @@ -1031,12 +1150,13 @@ SDL_PrivateGameControllerAddMapping(const char *mappingString, SDL_ControllerMap if (!pchGUID) { return SDL_SetError("Couldn't parse GUID from %s", mappingString); } - if (!SDL_strcasecmp(pchGUID, "xinput")) { + if (!SDL_strcasecmp(pchGUID, "default")) { + is_default_mapping = SDL_TRUE; + } else if (!SDL_strcasecmp(pchGUID, "hidapi")) { + is_hidapi_mapping = SDL_TRUE; + } else if (!SDL_strcasecmp(pchGUID, "xinput")) { is_xinput_mapping = SDL_TRUE; } - if (!SDL_strcasecmp(pchGUID, "emscripten")) { - is_emscripten_mapping = SDL_TRUE; - } jGUID = SDL_JoystickGetGUIDFromString(pchGUID); SDL_free(pchGUID); @@ -1048,12 +1168,13 @@ SDL_PrivateGameControllerAddMapping(const char *mappingString, SDL_ControllerMap if (existing) { return 0; } else { - if (is_xinput_mapping) { + if (is_default_mapping) { + s_pDefaultMapping = pControllerMapping; + } else if (is_hidapi_mapping) { + s_pHIDAPIMapping = pControllerMapping; + } else if (is_xinput_mapping) { s_pXInputMapping = pControllerMapping; } - if (is_emscripten_mapping) { - s_pEmscriptenMapping = pControllerMapping; - } return 1; } } @@ -1125,7 +1246,7 @@ char * SDL_GameControllerMappingForGUID(SDL_JoystickGUID guid) { char *pMappingString = NULL; - ControllerMapping_t *mapping = SDL_PrivateGetControllerMappingForGUID(&guid); + ControllerMapping_t *mapping = SDL_PrivateGetControllerMappingForGUID(&guid, SDL_FALSE); if (mapping) { char pchGUID[33]; size_t needed; @@ -1152,7 +1273,7 @@ SDL_GameControllerMapping(SDL_GameController * gamecontroller) return NULL; } - return SDL_GameControllerMappingForGUID(gamecontroller->guid); + return SDL_GameControllerMappingForGUID(gamecontroller->joystick->guid); } static void @@ -1263,12 +1384,50 @@ SDL_GameControllerNameForIndex(int device_index) { ControllerMapping_t *pSupportedController = SDL_PrivateGetControllerMapping(device_index); if (pSupportedController) { - return pSupportedController->name; + if (SDL_strcmp(pSupportedController->name, "*") == 0) { + return SDL_JoystickNameForIndex(device_index); + } else { + return pSupportedController->name; + } } return NULL; } +/** + * Get the mapping of a game controller. + * This can be called before any controllers are opened. + * If no mapping can be found, this function returns NULL. + */ +char * +SDL_GameControllerMappingForDeviceIndex(int joystick_index) +{ + char *pMappingString = NULL; + ControllerMapping_t *mapping; + + SDL_LockJoysticks(); + mapping = SDL_PrivateGetControllerMapping(joystick_index); + if (mapping) { + SDL_JoystickGUID guid; + char pchGUID[33]; + size_t needed; + guid = SDL_JoystickGetDeviceGUID(joystick_index); + SDL_JoystickGetGUIDString(guid, pchGUID, sizeof(pchGUID)); + /* allocate enough memory for GUID + ',' + name + ',' + mapping + \0 */ + needed = SDL_strlen(pchGUID) + 1 + SDL_strlen(mapping->name) + 1 + SDL_strlen(mapping->mapping) + 1; + pMappingString = SDL_malloc(needed); + if (!pMappingString) { + SDL_OutOfMemory(); + SDL_UnlockJoysticks(); + return NULL; + } + SDL_snprintf(pMappingString, needed, "%s,%s,%s", pchGUID, mapping->name, mapping->mapping); + } + SDL_UnlockJoysticks(); + return pMappingString; +} + + /* * Return 1 if the joystick with this name and GUID is a supported controller */ @@ -1303,6 +1462,7 @@ SDL_bool SDL_ShouldIgnoreGameController(const char *name, SDL_JoystickGUID guid) int i; Uint16 vendor; Uint16 product; + Uint16 version; Uint32 vidpid; if (SDL_allowed_controllers.num_entries == 0 && @@ -1310,8 +1470,7 @@ SDL_bool SDL_ShouldIgnoreGameController(const char *name, SDL_JoystickGUID guid) return SDL_FALSE; } - SDL_GetJoystickGUIDInfo(guid, &vendor, &product, NULL); - vidpid = MAKE_VIDPID(vendor, product); + SDL_GetJoystickGUIDInfo(guid, &vendor, &product, &version); if (SDL_GetHintBoolean("SDL_GAMECONTROLLER_ALLOW_STEAM_VIRTUAL_GAMEPAD", SDL_FALSE)) { /* We shouldn't ignore Steam's virtual gamepad since it's using the hints to filter out the real controllers so it can remap input for the virtual controller */ @@ -1319,7 +1478,7 @@ SDL_bool SDL_ShouldIgnoreGameController(const char *name, SDL_JoystickGUID guid) #if defined(__LINUX__) bSteamVirtualGamepad = (vendor == 0x28DE && product == 0x11FF); #elif defined(__MACOSX__) - bSteamVirtualGamepad = (SDL_strncmp(name, "GamePad-", 8) == 0); + bSteamVirtualGamepad = (vendor == 0x045E && product == 0x028E && version == 1); #elif defined(__WIN32__) /* We can't tell on Windows, but Steam will block others in input hooks */ bSteamVirtualGamepad = SDL_TRUE; @@ -1329,6 +1488,8 @@ SDL_bool SDL_ShouldIgnoreGameController(const char *name, SDL_JoystickGUID guid) } } + vidpid = MAKE_VIDPID(vendor, product); + if (SDL_allowed_controllers.num_entries > 0) { for (i = 0; i < SDL_allowed_controllers.num_entries; ++i) { if (vidpid == SDL_allowed_controllers.entries[i]) { @@ -1356,22 +1517,18 @@ SDL_bool SDL_ShouldIgnoreGameController(const char *name, SDL_JoystickGUID guid) SDL_GameController * SDL_GameControllerOpen(int device_index) { + SDL_JoystickID instance_id; SDL_GameController *gamecontroller; SDL_GameController *gamecontrollerlist; ControllerMapping_t *pSupportedController = NULL; SDL_LockJoysticks(); - if ((device_index < 0) || (device_index >= SDL_NumJoysticks())) { - SDL_SetError("There are %d joysticks available", SDL_NumJoysticks()); - SDL_UnlockJoysticks(); - return (NULL); - } - gamecontrollerlist = SDL_gamecontrollers; /* If the controller is already open, return it */ + instance_id = SDL_JoystickGetDeviceInstanceID(device_index); while (gamecontrollerlist) { - if (SDL_SYS_GetInstanceIdOfDeviceIndex(device_index) == gamecontrollerlist->joystick->instance_id) { + if (instance_id == gamecontrollerlist->joystick->instance_id) { gamecontroller = gamecontrollerlist; ++gamecontroller->ref_count; SDL_UnlockJoysticks(); @@ -1425,7 +1582,7 @@ SDL_GameControllerOpen(int device_index) } } - SDL_PrivateLoadButtonMapping(gamecontroller, pSupportedController->guid, pSupportedController->name, pSupportedController->mapping); + SDL_PrivateLoadButtonMapping(gamecontroller, pSupportedController->name, pSupportedController->mapping); /* Add the controller to list */ ++gamecontroller->ref_count; @@ -1552,7 +1709,17 @@ SDL_GameControllerName(SDL_GameController * gamecontroller) if (!gamecontroller) return NULL; - return gamecontroller->name; + if (SDL_strcmp(gamecontroller->name, "*") == 0) { + return SDL_JoystickName(SDL_GameControllerGetJoystick(gamecontroller)); + } else { + return gamecontroller->name; + } +} + +int +SDL_GameControllerGetPlayerIndex(SDL_GameController *gamecontroller) +{ + return SDL_JoystickGetPlayerIndex(SDL_GameControllerGetJoystick(gamecontroller)); } Uint16 @@ -1683,6 +1850,12 @@ SDL_GameControllerButtonBind SDL_GameControllerGetBindForButton(SDL_GameControll } +int +SDL_GameControllerRumble(SDL_GameController *gamecontroller, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + return SDL_JoystickRumble(SDL_GameControllerGetJoystick(gamecontroller), low_frequency_rumble, high_frequency_rumble, duration_ms); +} + void SDL_GameControllerClose(SDL_GameController * gamecontroller) { @@ -1820,6 +1993,24 @@ SDL_PrivateGameControllerButton(SDL_GameController * gamecontroller, SDL_GameCon } #endif /* !SDL_EVENTS_DISABLED */ + if (button == SDL_CONTROLLER_BUTTON_GUIDE) { + Uint32 now = SDL_GetTicks(); + if (state == SDL_PRESSED) { + gamecontroller->guide_button_down = now; + + if (gamecontroller->joystick->delayed_guide_button) { + /* Skip duplicate press */ + return (0); + } + } else { + if (!SDL_TICKS_PASSED(now, gamecontroller->guide_button_down+SDL_MINIMUM_GUIDE_BUTTON_DELAY_MS) && !gamecontroller->joystick->force_recentering) { + gamecontroller->joystick->delayed_guide_button = SDL_TRUE; + return (0); + } + gamecontroller->joystick->delayed_guide_button = SDL_FALSE; + } + } + /* translate the event, if desired */ posted = 0; #if !SDL_EVENTS_DISABLED @@ -1868,4 +2059,17 @@ SDL_GameControllerEventState(int state) #endif /* SDL_EVENTS_DISABLED */ } +void +SDL_GameControllerHandleDelayedGuideButton(SDL_Joystick *joystick) +{ + SDL_GameController *controllerlist = SDL_gamecontrollers; + while (controllerlist) { + if (controllerlist->joystick == joystick) { + SDL_PrivateGameControllerButton(controllerlist, SDL_CONTROLLER_BUTTON_GUIDE, SDL_RELEASED); + break; + } + controllerlist = controllerlist->next; + } +} + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/SDL_gamecontrollerdb.h b/Engine/lib/sdl/src/joystick/SDL_gamecontrollerdb.h index fdc0b592b..9d95c9078 100644 --- a/Engine/lib/sdl/src/joystick/SDL_gamecontrollerdb.h +++ b/Engine/lib/sdl/src/joystick/SDL_gamecontrollerdb.h @@ -35,197 +35,557 @@ static const char *s_ControllerMappings [] = "xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", #endif #if SDL_JOYSTICK_DINPUT + "03000000fa2d00000100000000000000,3DRUDDER,leftx:a0,lefty:a1,rightx:a5,righty:a2,", "03000000022000000090000000000000,8Bitdo NES30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b4,y:b3,", "03000000203800000900000000000000,8Bitdo NES30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b4,y:b3,", + "03000000c82d00000060000000000000,8Bitdo SF30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b4,y:b3,", + "03000000c82d00000061000000000000,8Bitdo SF30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b4,y:b3,", "03000000102800000900000000000000,8Bitdo SFC30 GamePad,a:b1,b:b0,back:b10,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b11,x:b4,y:b3,", "03000000a00500003232000000000000,8Bitdo Zero GamePad,a:b0,b:b1,back:b10,dpdown:+a2,dpleft:-a0,dpright:+a0,dpup:-a2,leftshoulder:b6,rightshoulder:b7,start:b11,x:b3,y:b4,", + "03000000c82d00002038000000000000,8bitdo,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b4,y:b3,", + "030000008f0e00001200000000000000,Acme GA-02,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b2,y:b3,", + "03000000fa190000f0ff000000000000,Acteck AGJ-3200,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", "03000000341a00003608000000000000,Afterglow PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006f0e00000263000000000000,Afterglow PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006f0e00001101000000000000,Afterglow PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006f0e00001401000000000000,Afterglow PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006f0e00001402000000000000,Afterglow PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006f0e00001901000000000000,Afterglow PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006f0e00001a01000000000000,Afterglow PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000d62000001d57000000000000,Airflo PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000d81d00000b00000000000000,BUFFALO BSGP1601 Series ,a:b5,b:b3,back:b12,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b8,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b9,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b13,x:b4,y:b2,", + "03000000d6200000e557000000000000,Batarang,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000c01100001352000000000000,Battalife Joystick,a:b6,b:b7,back:b2,leftshoulder:b0,leftx:a0,lefty:a1,rightshoulder:b1,start:b3,x:b4,y:b5,", + "030000006f0e00003201000000000000,Battlefield 4 PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000bc2000006012000000000000,Betop 2126F,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000bc2000000055000000000000,Betop BFM Gamepad,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,", + "03000000bc2000006312000000000000,Betop Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000bc2000006412000000000000,Betop Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000c01100000555000000000000,Betop Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000c01100000655000000000000,Betop Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000790000000700000000000000,Betop Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a4,start:b9,x:b3,y:b0,", + "03000000808300000300000000000000,Betop Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a4,start:b9,x:b3,y:b0,", + "030000006b1400000055000000000000,Bigben PS3 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "030000006b1400000103000000000000,Bigben PS3 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b2,", + "0300000066f700000500000000000000,BrutalLegendTest,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b0,y:b3,", "03000000e82000006058000000000000,Cideko AK08b,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", "03000000260900008888000000000000,Cyber Gadget GameCube Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:a4,rightx:a2,righty:a3~,start:b7,x:b2,y:b3,", "03000000a306000022f6000000000000,Cyborg V.3 Rumble Pad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:+a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:-a3,rightx:a2,righty:a4,start:b9,x:b0,y:b3,", - "03000000ffff00000000000000000000,GameStop Gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000451300000830000000000000,Defender Game Racer X7,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000791d00000103000000000000,Dual Box WII,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b6,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b5,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000bd12000002e0000000000000,Dual USB Vibration Joystick,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b9,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b10,righttrigger:b5,rightx:a3,righty:a2,start:b11,x:b3,y:b0,", + "030000006f0e00003001000000000000,EA SPORTS PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000341a00000108000000000000,EXEQ RF USB Gamepad 8206,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "030000008f0e00000f31000000000000,EXEQ,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b2,", + "03000000b80500000410000000000000,Elecom Gamepad,a:b2,b:b3,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b1,", + "03000000b80500000610000000000000,Elecom Gamepad,a:b2,b:b3,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b1,", + "03000000852100000201000000000000,FF-GP1,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000000d0f00002700000000000000,FIGHTING STICK V3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", + "030000000d0f00008500000000000000,Fighting Commander 2016 PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000000d0f00008400000000000000,Fighting Commander 5,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f00008700000000000000,Fighting Stick mini 4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", + "030000000d0f00008800000000000000,Fighting Stick mini 4,a:b1,b:b2,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b8,x:b0,y:b3,", + "78696e70757403000000000000000000,Fightstick TES,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,lefttrigger:a2,rightshoulder:b5,righttrigger:a5,start:b7,x:b2,y:b3,", + "03000000790000000600000000000000,G-Shark GS-GP702,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a4,start:b9,x:b3,y:b0,", + "030000008f0e00000d31000000000000,GAMEPAD 3 TURBO,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000300f00000b01000000000000,GGE909 Recoil Pad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b3,y:b0,", + "03000000790000002201000000000000,Game Controller for PC,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "0300000066f700000100000000000000,Game VIB Joystick,a:b2,b:b3,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a3,righty:a2,start:b11,x:b0,y:b1,", + "03000000280400000140000000000000,GamePad Pro USB,a:b1,b:b2,back:b8,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", + "03000000ac0500003d03000000000000,GameSir,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,", + "03000000ac0500004d04000000000000,GameSir,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,", + "03000000ffff00000000000000000000,GameStop Gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000260900002625000000000000,Gamecube Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b6,lefttrigger:a4,leftx:a0,lefty:a1,righttrigger:a5,rightx:a2,righty:a3,start:b7,x:b2,y:b3,", + "030000005c1a00003330000000000000,Genius MaxFire Grandias 12V,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b7,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b2,y:b3,", + "030000008305000031b0000000000000,Genius Maxfire Blaze 3,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000451300000010000000000000,Genius Maxfire Grandias 12,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "030000008305000009a0000000000000,Genius,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000f025000021c1000000000000,Gioteck PS3 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000f0250000c383000000000000,Gioteck VX2 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000f0250000c483000000000000,Gioteck VX2 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000f0250000c283000000000000,Gioteck,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000632500002605000000000000,HJD-X,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,", "030000000d0f00006e00000000000000,HORIPAD 4 (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000000d0f00006600000000000000,HORIPAD 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f0000ee00000000000000,HORIPAD mini4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000250900000017000000000000,HRAP2 on PS/SS/N64 Joypad to USB BOX,a:b2,b:b1,back:b9,leftshoulder:b5,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b6,start:b8,x:b3,y:b0,", + "03000000341a00000302000000000000,Hama Scorpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000000d0f00004900000000000000,Hatsune Miku Sho Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000d81400000862000000000000,HitBox Edition Cthulhu+,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b5,lefttrigger:b4,rightshoulder:b7,righttrigger:b6,start:b9,x:b0,y:b3,", "030000000d0f00005f00000000000000,Hori Fighting Commander 4 (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000000d0f00005e00000000000000,Hori Fighting Commander 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f00004000000000000000,Hori Fighting Stick Mini 3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b5,lefttrigger:b4,rightshoulder:b7,righttrigger:b6,start:b9,x:b0,y:b3,", + "030000000d0f00000900000000000000,Hori Pad 3 Turbo,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000000d0f00005400000000000000,Hori Pad 3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000000d0f00004d00000000000000,Hori Pad A,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000000d0f0000c100000000000000,Horipad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000008f0e00001330000000000000,HuiJia SNES Controller,a:b2,b:b1,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b9,x:b3,y:b0,", + "030000006f0e00002401000000000000,INJUSTICE FightStick PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", + "03000000ac0500002c02000000000000,IPEGA,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b8,leftstick:b13,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b9,rightstick:b14,righttrigger:b7,rightx:a3,righty:a4,start:b11,x:b3,y:b4,", + "03000000b50700001403000000000000,Impact Black,a:b2,b:b3,back:b8,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b0,y:b1,", + "03000000491900000204000000000000,Ipega PG-9023,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,", + "030000006e0500000520000000000000,JC-P301U,a:b2,b:b3,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a2,righty:a3,start:b11,x:b0,y:b1,", + "030000006e0500000320000000000000,JC-U3613M (DInput),a:b2,b:b3,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a2,righty:a3,start:b11,x:b0,y:b1,", + "030000006e0500000720000000000000,JC-W01U,a:b2,b:b3,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b1,", + "03000000790000000200000000000000,King PS3 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a4,start:b9,x:b3,y:b0,", + "030000006d040000d1ca000000000000,Logitech ChillStream,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006d040000d2ca000000000000,Logitech Cordless Precision,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006d04000011c2000000000000,Logitech Cordless Wingman,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b5,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b2,righttrigger:b7,rightx:a3,righty:a4,x:b4,", "030000006d04000016c2000000000000,Logitech Dual Action,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000006d04000018c2000000000000,Logitech F510 Gamepad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000006d04000019c2000000000000,Logitech F710 Gamepad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", /* Guide button doesn't seem to be sent in DInput mode. */ + "03000000380700008081000000000000,MADCATZ SFV Arcade FightStick Alpha PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000380700006382000000000000,MLG GamePad PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000250900006688000000000000,MP-8866 Super Dual Box,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,", + "03000000380700006652000000000000,Mad Catz C.T.R.L.R,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b0,y:b3,", "03000000380700005032000000000000,Mad Catz FightPad PRO (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "03000000380700005082000000000000,Mad Catz FightPad PRO (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000380700008433000000000000,Mad Catz FightStick TE S+ (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000380700008483000000000000,Mad Catz FightStick TE S+ (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000380700008134000000000000,Mad Catz FightStick TE2+ PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b7,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b4,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000380700008184000000000000,Mad Catz FightStick TE2+ PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b5,leftstick:b10,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000380700006252000000000000,Mad Catz Micro C.T.R.L.R,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b0,y:b3,", + "03000000380700008034000000000000,Mad Catz TE2 PS3 Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000380700008084000000000000,Mad Catz TE2 PS4 Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000380700001888000000000000,MadCatz SFIV FightStick PS3,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b5,lefttrigger:b7,leftx:a0,lefty:a1,rightshoulder:b4,righttrigger:b6,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000380700008532000000000000,Madcatz Arcade Fightstick TE S PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000380700003888000000000000,Madcatz Arcade Fightstick TE S+ PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000002a0600001024000000000000,Matricom,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b2,y:b3,", + "03000000250900000128000000000000,Mayflash Arcade Stick,a:b1,b:b2,back:b8,leftshoulder:b0,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b3,righttrigger:b7,start:b9,x:b5,y:b6,", + "03000000790000004318000000000000,Mayflash GameCube Controller Adapter,a:b1,b:b2,back:b0,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b0,leftshoulder:b4,leftstick:b0,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b0,righttrigger:a4,rightx:a5,righty:a2,start:b9,x:b0,y:b3,", "03000000790000004418000000000000,Mayflash GameCube Controller,a:b1,b:b2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:a4,rightx:a5,righty:a2,start:b9,x:b0,y:b3,", + "030000008f0e00001030000000000000,Mayflash USB Adapter for original Sega Saturn controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,lefttrigger:b5,rightshoulder:b2,righttrigger:b7,start:b9,x:b3,y:b4,", + "0300000025090000e803000000000000,Mayflash Wii Classic Controller,a:b1,b:b0,back:b8,dpdown:b13,dpleft:b12,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b2,", + "03000000790000000018000000000000,Mayflash WiiU Pro Game Controller Adapter (DInput),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000efbe0000edfe000000000000,Monect Virtual Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b0,", "030000001008000001e5000000000000,NEXT SNES Controller,a:b2,b:b1,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b6,start:b9,x:b3,y:b0,", + "03000000152000000182000000000000,NGDS,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b3,y:b0,", + "030000004b120000014d000000000000,NYKO AIRFLO,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:a3,leftstick:a0,lefttrigger:b6,rightshoulder:b5,rightstick:a2,righttrigger:b7,start:b9,x:b2,y:b3,", + "03000000bd12000015d0000000000000,Nintendo Retrolink USB Super SNES Classic Controller,a:b2,b:b1,back:b8,leftshoulder:b4,leftx:a0,lefty:a1,rightshoulder:b5,start:b9,x:b3,y:b0,", "030000007e0500000920000000000000,Nintendo Switch Pro Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "030000000d0500000308000000000000,Nostromo N45,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b9,leftshoulder:b4,leftstick:b12,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b10,x:b2,y:b3,", + "03000000d62000006d57000000000000,OPP PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "03000000362800000100000000000000,OUYA Game Controller,a:b0,b:b3,dpdown:b9,dpleft:b10,dpright:b11,dpup:b8,guide:b14,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:b13,rightx:a3,righty:a4,x:b1,y:b2,", + "03000000782300000a10000000000000,Onlive Wireless Controller,a:b15,b:b14,back:b7,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b11,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b13,y:b12,", + "030000006b14000001a1000000000000,Orange Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a4,rightx:a5,righty:a2,start:b9,x:b2,y:b3,", + "03000000120c0000f60e000000000000,P4 Wired Gamepad,a:b1,b:b2,back:b12,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b5,lefttrigger:b7,rightshoulder:b4,righttrigger:b6,start:b9,x:b0,y:b3,", + "03000000632500002306000000000000,PS Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b3,y:b4,", + "03000000e30500009605000000000000,PS to USB convert cable,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,", + "03000000100800000100000000000000,PS1 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b3,y:b0,", + "030000008f0e00007530000000000000,PS1 Controller,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b1,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000100800000300000000000000,PS2 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a4,righty:a2,start:b9,x:b3,y:b0,", + "03000000250900008888000000000000,PS2 Controller,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,", + "03000000666600006706000000000000,PS2 Controller,a:b2,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b13,dpup:b12,leftshoulder:b6,leftstick:b9,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b10,righttrigger:b5,rightx:a2,righty:a3,start:b11,x:b3,y:b0,", + "030000006b1400000303000000000000,PS2 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "030000009d0d00001330000000000000,PS2 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000250900000500000000000000,PS3 Controller,a:b2,b:b1,back:b9,dpdown:h0.8,dpleft:h0.4,dpright:h0.2,dpup:h0.1,guide:,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b0,y:b3,", + "030000004c0500006802000000000000,PS3 Controller,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b10,lefttrigger:a3~,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:a4~,rightx:a2,righty:a5,start:b8,x:b3,y:b0,", + "03000000632500007505000000000000,PS3 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", "03000000888800000803000000000000,PS3 Controller,a:b2,b:b1,back:b8,dpdown:h0.8,dpleft:h0.4,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b9,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:b7,rightx:a3,righty:a4,start:b11,x:b0,y:b3,", - "030000004c0500006802000000000000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", - "03000000250900000500000000000000,PS3 DualShock,a:b2,b:b1,back:b9,dpdown:h0.8,dpleft:h0.4,dpright:h0.2,dpup:h0.1,guide:,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b0,y:b3,", + "030000008f0e00001431000000000000,PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000003807000056a8000000000000,PS3 RF pad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000100000008200000000000000,PS360+ v1.66,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:h0.4,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", + "030000004c050000a00b000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "030000004c050000c405000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "030000004c050000cc09000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", - "030000004c050000a00b000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000008f0e00000300000000000000,Piranha xtreme,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b3,y:b0,", + "03000000d62000006dca000000000000,PowerA Pro Ex,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000d62000009557000000000000,Pro Elite PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000d62000009f31000000000000,Pro Ex mini PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000d6200000c757000000000000,Pro Ex mini PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000222c00000020000000000000,QANBA DRONE ARCADE JOYSTICK,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,rightshoulder:b5,righttrigger:a4,start:b9,x:b0,y:b3,", + "03000000300f00000011000000000000,QanBa Arcade JoyStick 1008,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,start:b10,x:b0,y:b3,", + "03000000300f00001611000000000000,QanBa Arcade JoyStick 4018,a:b1,b:b2,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b9,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b8,x:b0,y:b3,", + "03000000300f00001210000000000000,QanBa Joystick Plus,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,rightshoulder:b5,start:b9,x:b2,y:b3,", + "03000000341a00000104000000000000,QanBa Joystick Q4RAF,a:b5,b:b6,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b0,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b3,righttrigger:b7,start:b9,x:b1,y:b2,", + "03000000222c00000223000000000000,Qanba Obsidian Arcade Joystick PS3 Mode,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000222c00000023000000000000,Qanba Obsidian Arcade Joystick PS4 Mode,a:b1,b:b2,back:b13,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f00001100000000000000,REAL ARCADE PRO.3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,rightstick:b11,righttrigger:b7,start:b9,x:b0,y:b3,", + "030000000d0f00007000000000000000,REAL ARCADE PRO.4 VLX,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,rightstick:b11,righttrigger:b7,start:b9,x:b0,y:b3,", + "030000000d0f00002200000000000000,REAL ARCADE Pro.V3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000321500000003000000000000,Razer Hydra,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a2,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000321500000204000000000000,Razer Panthera (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000321500000104000000000000,Razer Panthera (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f00006a00000000000000,Real Arcade Pro.4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f00006b00000000000000,Real Arcade Pro.4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000000d0f00008a00000000000000,Real Arcade Pro.4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f00008b00000000000000,Real Arcade Pro.4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000000d0f00005b00000000000000,Real Arcade Pro.V4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f00005c00000000000000,Real Arcade Pro.V4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "0300000000f000000300000000000000,RetroUSB.com RetroPad,a:b1,b:b5,back:b2,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b3,x:b0,y:b4,", + "0300000000f00000f100000000000000,RetroUSB.com Super RetroPort,a:b1,b:b5,back:b2,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b3,x:b0,y:b4,", "03000000790000001100000000000000,Retrolink SNES Controller,a:b2,b:b1,back:b8,dpdown:+a4,dpleft:-a3,dpright:+a3,dpup:-a4,leftshoulder:b4,rightshoulder:b5,start:b9,x:b3,y:b0,", "030000006b140000010d000000000000,Revolution Pro Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000006f0e00001e01000000000000,Rock Candy PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006f0e00002801000000000000,Rock Candy PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006f0e00002f01000000000000,Rock Candy PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000341a00000208000000000000,SL-6555-SBK,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:-a4,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a4,rightx:a3,righty:a2,start:b7,x:b2,y:b3,", + "03000000341a00000908000000000000,SL-6566,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000790000001c18000000000000,STK-7024X,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,", + "03000000ff1100003133000000000000,SVEN X-PAD,a:b2,b:b3,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a2,righty:a4,start:b5,x:b0,y:b1,", + "03000000a306000023f6000000000000,Saitek Cyborg V.1 Game pad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a4,start:b9,x:b0,y:b3,", + "03000000a30600001af5000000000000,Saitek Cyborg,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b0,y:b3,", + "03000000300f00001201000000000000,Saitek Dual Analog Pad,a:b2,b:b3,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b0,y:b1,", "03000000a30600000cff000000000000,Saitek P2500 Force Rumble Pad,a:b2,b:b3,back:b11,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a2,righty:a3,x:b0,y:b1,", + "03000000a30600000c04000000000000,Saitek P2900,a:b1,b:b2,back:b12,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b0,y:b3,", + "03000000300f00001001000000000000,Saitek P480 Rumble Pad,a:b2,b:b3,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b0,y:b1,", "03000000a30600000b04000000010000,Saitek P990 Dual Analog Pad,a:b1,b:b2,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b8,x:b0,y:b3,", + "03000000a30600000b04000000000000,Saitek P990,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b0,y:b3,", + "03000000a30600002106000000000000,Saitek PS1000,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a4,start:b9,x:b0,y:b3,", + "03000000a306000020f6000000000000,Saitek PS2700,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a4,start:b9,x:b0,y:b3,", + "03000000300f00001101000000000000,Saitek Rumble Pad,a:b2,b:b3,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b0,y:b1,", + "0300000000050000289b000000000000,Saturn_Adapter_2.0,a:b1,b:b2,leftshoulder:b6,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b5,start:b9,x:b0,y:b3,", + "030000009b2800000500000000000000,Saturn_Adapter_2.0,a:b1,b:b2,leftshoulder:b6,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b5,start:b9,x:b0,y:b3,", + "030000008f0e00000800000000000000,SpeedLink Strike FX,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000c01100000591000000000000,Speedlink Torid,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000381000001814000000000000,SteelSeries Stratus XL,a:b0,b:b1,back:b18,dpdown:b13,dpleft:b14,dpright:b15,dpup:b12,guide:b19,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b2,y:b3,", + "03000000110100001914000000000000,SteelSeries,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:,leftstick:b13,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:,rightstick:b14,righttrigger:b7,rightx:a3,righty:a4,start:b11,x:b3,y:b4,", + "03000000d620000011a7000000000000,Switch,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000004f04000007d0000000000000,T Mini Wireless,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000fa1900000706000000000000,Team 5,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000b50700001203000000000000,Techmobility X6-38V,a:b2,b:b3,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b0,y:b1,", + "030000004f04000015b3000000000000,Thrustmaster Dual Analog 4,a:b0,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b1,y:b3,", + "030000004f04000023b3000000000000,Thrustmaster Dual Trigger 3-in-1,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000004f04000004b3000000000000,Thrustmaster Firestorm Dual Power 3,a:b0,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b1,y:b3,", + "030000004f04000000b3000000000000,Thrustmaster Firestorm Dual Power,a:b0,b:b2,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b11,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b12,righttrigger:b7,rightx:a2,righty:a3,start:b10,x:b1,y:b3,", + "03000000666600000488000000000000,TigerGame PS/PS2 Game Controller Adapter,a:b2,b:b1,back:b9,dpdown:b14,dpleft:b15,dpright:b13,dpup:b12,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,", + "03000000d62000006000000000000000,Tournament PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000005f140000c501000000000000,Trust Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000b80500000210000000000000,Trust Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000d90400000200000000000000,TwinShock PS2,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b3,y:b0,", + "03000000300f00000701000000000000,USB 4-Axis 12-Button Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b3,y:b0,", + "03000000632500002305000000000000,USB Vibration Joystick (BM),a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000341a00002308000000000000,USB gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "030000005509000000b4000000000000,USB gamepad,a:b10,b:b11,back:b5,dpdown:b1,dpleft:b2,dpright:b3,dpup:b0,guide:b14,leftshoulder:b8,leftstick:b6,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b9,rightstick:b7,righttrigger:a5,rightx:a2,righty:a3,start:b4,x:b12,y:b13,", + "030000006b1400000203000000000000,USB gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000790000000a00000000000000,USB gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a4,start:b9,x:b3,y:b0,", + "03000000f0250000c183000000000000,USB gamepad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000ff1100004133000000000000,USB gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a4,righty:a2,start:b9,x:b3,y:b0,", + "03000000790000001b18000000000000,Venom Arcade Joystick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", + "03000000450c00002043000000000000,XEOX Gamepad SL-6556-BK,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000341a00000608000000000000,Xeox,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", "03000000172700004431000000000000,XiaoMi Game Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b20,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a7,rightx:a2,righty:a5,start:b11,x:b3,y:b4,", + "03000000790000004f18000000000000,ZD-T Android,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,", + "03000000d81d00000f00000000000000,iBUFFALO BSGP1204 Series,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000d81d00001000000000000000,iBUFFALO BSGP1204P Series,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", "03000000830500006020000000000000,iBuffalo SNES Controller,a:b1,b:b0,back:b6,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b5,start:b7,x:b3,y:b2,", + "030000004f04000003d0000000000000,run'n'drive,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b7,leftshoulder:a3,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:a4,rightstick:b11,righttrigger:b5,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000101c0000171c000000000000,uRage Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", #endif #if defined(__MACOSX__) "03000000022000000090000001000000,8Bitdo NES30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,", "03000000203800000900000000010000,8Bitdo NES30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,", "03000000102800000900000000000000,8Bitdo SFC30 GamePad Joystick,a:b1,b:b0,back:b10,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b11,x:b4,y:b3,", "03000000a00500003232000008010000,8Bitdo Zero GamePad,a:b0,b:b1,back:b10,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b11,x:b3,y:b4,", + "03000000a00500003232000009010000,8Bitdo Zero GamePad,a:b0,b:b1,back:b10,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b11,x:b3,y:b4,", "030000008305000031b0000000000000,Cideko AK08b,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "03000000260900008888000088020000,Cyber Gadget GameCube Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:a5,rightx:a2,righty:a3~,start:b7,x:b2,y:b3,", "03000000a306000022f6000001030000,Cyborg V.3 Rumble Pad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:+a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:-a3,rightx:a2,righty:a4,start:b9,x:b0,y:b3,", - "0500000047532047616d657061640000,GameStop Gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000790000000600000000000000,G-Shark GP-702,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b3,y:b0,", + "0500000047532047616d657061640000,GameStop Gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000ad1b000001f9000000000000,Gamestop BB-070 X360 Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", + "030000000d0f00005f00000000000000,HORI Fighting Commander 4 PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000000d0f00005e00000000000000,HORI Fighting Commander 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000000d0f00004d00000000000000,HORI Gem Pad 3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000000d0f00006e00000000010000,HORIPAD 4 (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000000d0f00006600000000010000,HORIPAD 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f00006600000000000000,HORIPAD FPS PLUS 4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "030000000d0f00005f00000000010000,Hori Fighting Commander 4 (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000000d0f00005e00000000010000,Hori Fighting Commander 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "030000008f0e00001330000011010000,HuiJia SNES Controller,a:b4,b:b2,back:b16,dpdown:+a2,dpleft:-a0,dpright:+a0,dpup:-a2,leftshoulder:b12,rightshoulder:b14,start:b18,x:b6,y:b0,", - "030000006d04000016c2000014040000,Logitech Dual Action,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006d04000016c2000000020000,Logitech Dual Action,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000006d04000016c2000000030000,Logitech Dual Action,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006d04000016c2000014040000,Logitech Dual Action,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000006d04000016c2000000000000,Logitech F310 Gamepad (DInput),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", /* Guide button doesn't seem to be sent in DInput mode. */ "030000006d04000018c2000000000000,Logitech F510 Gamepad (DInput),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000006d0400001fc2000000000000,Logitech F710 Gamepad (XInput),a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", "030000006d04000019c2000000000000,Logitech Wireless Gamepad (DInput),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", /* This includes F710 in DInput mode and the "Logitech Cordless RumblePad 2", at the very least. */ + "03000000d8140000cecf000000000000,MC Cthulhu,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", "03000000380700005032000000010000,Mad Catz FightPad PRO (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "03000000380700005082000000010000,Mad Catz FightPad PRO (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000380700008433000000010000,Mad Catz FightStick TE S+ (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000380700008483000000010000,Mad Catz FightStick TE S+ (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "03000000790000004418000000010000,Mayflash GameCube Controller,a:b1,b:b2,dpdown:b14,dpleft:b15,dpright:b13,dpup:b12,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:a4,rightx:a5,righty:a2,start:b9,x:b0,y:b3,", + "0300000025090000e803000000000000,Mayflash Wii Classic Controller,a:b1,b:b0,back:b8,dpdown:b13,dpleft:b12,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b2,", + "03000000790000000018000000000000,Mayflash WiiU Pro Game Controller Adapter (DInput),a:b4,b:b8,back:b32,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b16,leftstick:b40,lefttrigger:b24,leftx:a0,lefty:a4,rightshoulder:b20,rightstick:b44,righttrigger:b28,rightx:a8,righty:a12,start:b36,x:b0,y:b12,", "030000001008000001e5000006010000,NEXT SNES Controller,a:b2,b:b1,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b6,start:b9,x:b3,y:b0,", "030000007e0500000920000000000000,Nintendo Switch Pro Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "030000004c0500006802000000000000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", "030000004c0500006802000000010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", + "030000004c050000a00b000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000004c050000c405000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "030000004c050000c405000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "030000004c050000cc09000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", - "030000004c050000a00b000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000008f0e00000300000000000000,Piranha xtreme,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b3,y:b0,", + "030000008916000000fd000000000000,Razer Onza TE,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", + "03000000321500000204000000010000,Razer Panthera (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000321500000104000000010000,Razer Panthera (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "03000000321500000010000000010000,Razer RAIJU,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "0300000032150000030a000000000000,Razer Wildcat,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", + "03000000790000001100000000000000,Retrolink Classic Controller,a:b2,b:b1,back:b8,leftshoulder:b4,leftx:a3,lefty:a4,rightshoulder:b5,start:b9,x:b3,y:b0,", "03000000790000001100000006010000,Retrolink SNES Controller,a:b2,b:b1,back:b8,dpdown:+a4,dpleft:-a3,dpright:+a3,dpup:-a4,leftshoulder:b4,rightshoulder:b5,start:b9,x:b3,y:b0,", "030000006b140000010d000000010000,Revolution Pro Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "030000003512000021ab000000000000,SFC30 Joystick,a:b1,b:b0,back:b10,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b11,x:b4,y:b3,", - "030000005e0400008e02000001000000,Steam Virtual GamePad,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", + "03000000b40400000a01000000000000,Sega Saturn USB Gamepad,a:b0,b:b1,back:b5,guide:b2,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b8,x:b3,y:b4,", + "03000000811700007e05000000000000,Sega Saturn,a:b2,b:b4,dpdown:b16,dpleft:b15,dpright:b14,dpup:b17,leftshoulder:b8,lefttrigger:a5,leftx:a0,lefty:a2,rightshoulder:b9,righttrigger:a4,start:b13,x:b0,y:b6,", + "030000004c050000cc09000000000000,Sony DualShock 4 V2,a:b1,b:b2,back:b13,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000004c050000a00b000000000000,Sony DualShock 4 Wireless Adaptor,a:b1,b:b2,back:b13,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000005e0400008e02000001000000,Steam Virtual Gamepad,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", + "03000000110100002014000000000000,SteelSeries Nimbus,a:b0,b:b1,dpdown:b9,dpleft:b11,dpright:b10,dpup:b8,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b12,x:b2,y:b3,", "03000000110100002014000001000000,SteelSeries Nimbus,a:b0,b:b1,dpdown:b9,dpleft:b11,dpright:b10,dpup:b8,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1~,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3~,x:b2,y:b3,", "03000000381000002014000001000000,SteelSeries Nimbus,a:b0,b:b1,dpdown:b9,dpleft:b11,dpright:b10,dpup:b8,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1~,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3~,x:b2,y:b3,", "03000000110100001714000000000000,SteelSeries Stratus XL,a:b0,b:b1,dpdown:b9,dpleft:b11,dpright:b10,dpup:b8,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1~,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3~,start:b12,x:b2,y:b3,", "03000000110100001714000020010000,SteelSeries Stratus XL,a:b0,b:b1,dpdown:b9,dpleft:b11,dpright:b10,dpup:b8,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1~,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3~,start:b12,x:b2,y:b3,", + "030000004f04000015b3000000000000,Thrustmaster Dual Analog 3.2,a:b0,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b1,y:b3,", + "030000004f04000000b3000000000000,Thrustmaster Firestorm Dual Power,a:b0,b:b2,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b11,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:b7,rightx:a2,righty:a3,start:b10,x:b1,y:b3,", + "03000000bd12000015d0000000000000,Tomee SNES USB Controller,a:b2,b:b1,back:b8,leftshoulder:b4,leftx:a0,lefty:a1,rightshoulder:b5,start:b9,x:b3,y:b0,", + "03000000bd12000015d0000000010000,Tomee SNES USB Controller,a:b2,b:b1,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b5,start:b9,x:b3,y:b0,", + "03000000100800000100000000000000,Twin USB Joystick,a:b4,b:b2,back:b16,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b12,leftstick:b20,lefttrigger:b8,leftx:a0,lefty:a2,rightshoulder:b14,rightstick:b22,righttrigger:b10,rightx:a6,righty:a4,start:b18,x:b6,y:b0,", + "050000005769696d6f74652028303000,Wii Remote,a:b4,b:b5,back:b7,dpdown:b3,dpleft:b0,dpright:b1,dpup:b2,guide:b8,leftshoulder:b11,lefttrigger:b12,leftx:a0,lefty:a1,start:b6,x:b10,y:b9,", + "050000005769696d6f74652028313800,Wii U Pro Controller,a:b16,b:b15,back:b7,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b8,leftshoulder:b19,leftstick:b23,lefttrigger:b21,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b24,righttrigger:b22,rightx:a2,righty:a3,start:b6,x:b18,y:b17,", "030000005e0400008e02000000000000,X360 Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", "03000000c6240000045d000000000000,Xbox 360 Wired Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", "030000005e040000d102000000000000,Xbox One Wired Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", - "030000005e040000e302000000000000,Xbox One Wired Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", "030000005e040000dd02000000000000,Xbox One Wired Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", + "030000005e040000e302000000000000,Xbox One Wired Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", + "030000005e040000e002000000000000,Xbox Wireless Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "030000005e040000e002000003090000,Xbox Wireless Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "030000005e040000ea02000000000000,Xbox Wireless Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,", "030000005e040000fd02000003090000,Xbox Wireless Controller,a:b0,b:b1,back:b16,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b15,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,", "03000000172700004431000029010000,XiaoMi Game Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b15,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a6,rightx:a2,righty:a5,start:b11,x:b3,y:b4,", "03000000120c0000100e000000010000,ZEROPLUS P4 Gamepad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "03000000830500006020000000010000,iBuffalo SNES Controller,a:b1,b:b0,back:b6,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b5,start:b7,x:b3,y:b2,", + "03000000830500006020000000000000,iBuffalo USB 2-axis 8-button Gamepad,a:b1,b:b0,back:b6,leftshoulder:b4,leftx:a0,lefty:a1,rightshoulder:b5,start:b7,x:b3,y:b2,", #endif #if defined(__LINUX__) + "03000000c82d00000190000011010000,8Bitdo NES30 Pro 8Bitdo NES30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a5,rightx:a2,righty:a3,start:b11,x:b4,y:b3,", "03000000022000000090000011010000,8Bitdo NES30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,", "05000000203800000900000000010000,8Bitdo NES30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,", + "05000000c82d00002038000000010000,8Bitdo NES30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b4,y:b3,", + "05000000c82d00000061000000010000,8Bitdo SF30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b4,y:b3,", "05000000102800000900000000010000,8Bitdo SFC30 GamePad,a:b1,b:b0,back:b10,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b11,x:b4,y:b3,", + "05000000c82d00003028000000010000,8Bitdo SFC30 GamePad,a:b1,b:b0,back:b10,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b11,x:b4,y:b3,", + "05000000a00500003232000001000000,8Bitdo Zero GamePad,a:b0,b:b1,back:b10,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b11,x:b3,y:b4,", "05000000a00500003232000008010000,8Bitdo Zero GamePad,a:b0,b:b1,back:b10,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b11,x:b3,y:b4,", + "05000000050b00000045000031000000,ASUS Gamepad,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b6,leftshoulder:b4,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b10,x:b2,y:b3,", + "030000006f0e00003901000020060000,Afterglow Controller for Xbox One,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000006f0e00003901000000430000,Afterglow Prismatic Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000006f0e00001302000000010000,Afterglow,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "03000000100000008200000011010000,Akishop Customs PS360+ v1.66,a:b1,b:b2,back:b12,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", + "03000000b40400000a01000000010000,CYPRESS USB Gamepad,a:b0,b:b1,back:b5,guide:b2,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b8,x:b3,y:b4,", "03000000e82000006058000001010000,Cideko AK08b,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", "03000000260900008888000000010000,Cyber Gadget GameCube Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:a5,rightx:a2,righty:a3~,start:b7,x:b2,y:b3,", "03000000a306000022f6000011010000,Cyborg V.3 Rumble Pad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:+a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:-a3,rightx:a2,righty:a4,start:b9,x:b0,y:b3,", "03000000790000000600000010010000,DragonRise Inc. Generic USB Joystick,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b3,y:b0,", - "0500000047532047616d657061640000,GameStop Gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "030000006f0e00003001000001010000,EA Sports PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "0300000079000000d418000000010000,GPD Win 2 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "0500000047532067616d657061640000,GS gamepad,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000341a000005f7000010010000,GameCube {HuiJia USB box},a:b1,b:b2,dpdown:b14,dpleft:b15,dpright:b13,dpup:b12,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:a4,rightx:a5,righty:a2,start:b9,x:b0,y:b3,", + "03000000bc2000000055000011010000,GameSir G3w,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,", + "0500000047532047616d657061640000,GameStop Gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", "030000006f0e00000104000000010000,Gamestop Logic3 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000006f0e00001304000000010000,Generic X-Box pad,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:a0,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:a3,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000f0250000c183000010010000,Goodbetterbest Ltd USB Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000280400000140000000010000,Gravis GamePad Pro USB ,a:b1,b:b2,back:b8,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", + "030000008f0e00000610000000010000,GreenAsia Electronics 4Axes 12Keys GamePad ,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b9,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b10,righttrigger:b5,rightx:a3,righty:a2,start:b11,x:b3,y:b0,", + "030000008f0e00001200000010010000,GreenAsia Inc. USB Joystick,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b2,y:b3,", + "03000000c9110000f055000011010000,HJC Game GAMEPAD,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "030000000d0f00001000000011010000,HORI CO. LTD. FIGHTING STICK 3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", + "030000000d0f00002200000011010000,HORI CO. LTD. REAL ARCADE Pro.V3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,", + "030000000d0f00006a00000011010000,HORI CO. LTD. Real Arcade Pro.4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f00006b00000011010000,HORI CO. LTD. Real Arcade Pro.4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000000d0f00006e00000011010000,HORIPAD 4 (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000000d0f00006600000011010000,HORIPAD 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000000d0f00006700000001010000,HORIPAD ONE,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "06000000adde0000efbe000002010000,Hidromancer Game Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000d81400000862000011010000,HitBox (PS3/PC) Analog Mode,a:b1,b:b2,back:b8,guide:b9,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,start:b12,x:b0,y:b3,", "030000000d0f00005f00000011010000,Hori Fighting Commander 4 (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000000d0f00005e00000011010000,Hori Fighting Commander 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000ad1b000001f5000033050000,Hori Pad EX Turbo 2,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "030000008f0e00001330000010010000,HuiJia SNES Controller,a:b2,b:b1,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b9,x:b3,y:b0,", - "03000000ba2200002010000001010000,Jess Technology USB Game Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b3,y:b0,", - "030000006d04000019c2000010010000,Logitech Cordless RumblePad 2,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", - "030000006d04000016c2000011010000,Logitech Dual Action,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000fd0500000030000000010000,InterAct GoPad I-73000 (Fighting Game Layout),a:b3,b:b4,back:b6,leftx:a0,lefty:a1,rightshoulder:b2,righttrigger:b5,start:b7,x:b0,y:b1,", + "030000006e0500000320000010010000,JC-U3613M - DirectInput Mode,a:b2,b:b3,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a2,righty:a3,start:b11,x:b0,y:b1,", + "03000000300f00001001000010010000,Jess Tech Dual Analog Rumble Pad,a:b2,b:b3,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b0,y:b1,", + "03000000ba2200002010000001010000,Jess Technology USB Game Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b3,y:b0,", + "030000006f0e00000103000000020000,Logic3 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000006d04000019c2000010010000,Logitech Cordless RumblePad 2,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000006d04000016c2000010010000,Logitech Dual Action,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006d04000016c2000011010000,Logitech Dual Action,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "030000006d0400001dc2000014400000,Logitech F310 Gamepad (XInput),a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "030000006d0400001ec2000020200000,Logitech F510 Gamepad (XInput),a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "030000006d04000019c2000011010000,Logitech F710 Gamepad (DInput),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", /* Guide button doesn't seem to be sent in DInput mode. */ "030000006d0400001fc2000005030000,Logitech F710 Gamepad (XInput),a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000006d04000015c2000010010000,Logitech Logitech Extreme 3D,a:b0,b:b4,back:b6,guide:b8,leftshoulder:b9,leftstick:h0.8,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:h0.2,start:b7,x:b2,y:b5,", "030000006d04000018c2000010010000,Logitech RumblePad 2,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006d04000011c2000010010000,Logitech WingMan Cordless RumblePad,a:b0,b:b1,back:b2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b6,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b10,rightx:a3,righty:a4,start:b8,x:b3,y:b4,", + "03000000250900006688000000010000,MP-8866 Super Dual Box,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,", + "05000000380700006652000025010000,Mad Catz C.T.R.L.R ,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "03000000380700005032000011010000,Mad Catz FightPad PRO (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "03000000380700005082000011010000,Mad Catz FightPad PRO (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", - "03000000380700008433000011010000,Mad Catz FightStick TE S+ PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", - "03000000380700008483000011010000,Mad Catz FightStick TE S+ PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000380700008433000011010000,Mad Catz FightStick TE S+ (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000380700008483000011010000,Mad Catz FightStick TE S+ (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000ad1b00002ef0000090040000,Mad Catz Fightpad SFxT,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,lefttrigger:a2,rightshoulder:b5,righttrigger:a5,start:b7,x:b2,y:b3,", "03000000380700003847000090040000,Mad Catz Wired Xbox 360 Controller (SFIV),a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000380700001647000010040000,Mad Catz Wired Xbox 360 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000ad1b000016f0000090040000,Mad Catz Xbox 360 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "03000000380700008034000011010000,Mad Catz fightstick (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "03000000380700008084000011010000,Mad Catz fightstick (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "03000000380700001888000010010000,MadCatz PC USB Wired Stick 8818,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000380700003888000010010000,MadCatz PC USB Wired Stick 8838,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:a0,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", "03000000790000004418000010010000,Mayflash GameCube Controller,a:b1,b:b2,dpdown:b14,dpleft:b15,dpright:b13,dpup:b12,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:a4,rightx:a5,righty:a2,start:b9,x:b0,y:b3,", + "03000000780000000600000010010000,Microntek USB Joystick,a:b2,b:b1,back:b8,leftshoulder:b6,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b5,start:b9,x:b3,y:b0,", + "030000005e0400000e00000000010000,Microsoft SideWinder,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,rightshoulder:b7,start:b8,x:b3,y:b4,", + "030000005e0400008e02000004010000,Microsoft X-Box 360 pad,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000005e0400008e02000062230000,Microsoft X-Box 360 pad,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000005e040000d102000003020000,Microsoft X-Box One pad v2,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000005e040000d102000001010000,Microsoft X-Box One pad,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000005e0400008502000000010000,Microsoft X-Box pad (Japan),a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b5,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b2,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b3,y:b4,", + "030000005e0400008902000021010000,Microsoft X-Box pad v2 (US),a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b5,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b2,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b3,y:b4,", + "05000000d6200000ad0d000001000000,Moga Pro,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,", "030000001008000001e5000010010000,NEXT SNES Controller,a:b2,b:b1,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b6,start:b9,x:b3,y:b0,", "03000000550900001072000011010000,NVIDIA Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b13,leftshoulder:b4,leftstick:b8,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a4,rightx:a2,righty:a3,start:b7,x:b2,y:b3,", + "03000000451300000830000010010000,NYKO CORE,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "050000007e0500000920000001000000,Nintendo Switch Pro Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", "050000007e0500003003000001000000,Nintendo Wii Remote Pro Controller,a:b0,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b2,", + "05000000010000000100000003000000,Nintendo Wiimote,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", "030000000d0500000308000010010000,Nostromo n45 Dual Analog Gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b9,leftshoulder:b4,leftstick:b12,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b10,x:b2,y:b3,", "05000000362800000100000002010000,OUYA Game Controller,a:b0,b:b3,dpdown:b9,dpleft:b10,dpright:b11,dpup:b8,guide:b14,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,x:b1,y:b2,", + "05000000362800000100000003010000,OUYA Game Controller,a:b0,b:b3,dpdown:b9,dpleft:b10,dpright:b11,dpup:b8,guide:b14,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,x:b1,y:b2,", + "030000005e0400000202000000010000,Old Xbox pad,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b5,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b2,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b3,y:b4,", + "03000000ff1100003133000010010000,PC Game Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", "030000006f0e00006401000001010000,PDP Battlefield One,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "03000000ff1100004133000010010000,PS2 Controller,a:b2,b:b1,back:b8,leftshoulder:b6,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b5,start:b9,x:b3,y:b0,", - "030000004c0500006802000010010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", - "050000004c0500006802000000010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:a12,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:a13,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", - "030000004c0500006802000011010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", - "030000004c0500006802000010810000,PS3 Controller,a:b0,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", - "050000004c0500006802000000810000,PS3 Controller,a:b0,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", - "030000004c0500006802000011810000,PS3 Controller,a:b0,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", "03000000341a00003608000011010000,PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", - "030000004c050000c405000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", - "050000004c050000c405000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", - "030000004c050000cc09000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", - "050000004c050000cc09000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000004c0500006802000010010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", + "030000004c0500006802000010810000,PS3 Controller,a:b0,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", + "030000004c0500006802000011010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", + "030000004c0500006802000011810000,PS3 Controller,a:b0,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", + "030000006f0e00001402000011010000,PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000008f0e00000300000010010000,PS3 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "050000004c0500006802000000010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:a12,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:a13,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", + "050000004c0500006802000000800000,PS3 Controller,a:b0,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", + "050000004c0500006802000000810000,PS3 Controller,a:b0,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", + "05000000504c415953544154494f4e00,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", + "060000004c0500006802000000010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,", "030000004c050000a00b000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", - "030000004c050000c405000011810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", - "050000004c050000c405000000810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", - "030000004c050000cc09000011810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", - "050000004c050000cc09000001800000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", - "050000004c050000cc09000000810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", "030000004c050000a00b000011810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", + "030000004c050000c405000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000004c050000c405000011810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", + "030000004c050000cc09000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000004c050000cc09000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000004c050000cc09000011810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", + "050000004c050000c405000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "050000004c050000c405000000810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", + "050000004c050000cc09000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "050000004c050000cc09000000810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", + "050000004c050000cc09000001800000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,", + "03000000c62400000053000000010000,PowerA,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "03000000300f00001211000011010000,QanBa Arcade JoyStick,a:b2,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b5,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b6,start:b9,x:b1,y:b3,", + "030000008916000001fd000024010000,Razer Onza Classic Edition,a:b0,b:b1,back:b6,dpdown:b14,dpleft:b11,dpright:b12,dpup:b13,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000008916000000fd000024010000,Razer Onza Tournament Edition,a:b0,b:b1,back:b6,dpdown:b14,dpleft:b11,dpright:b12,dpup:b13,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000321500000204000011010000,Razer Panthera (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000321500000104000011010000,Razer Panthera (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", "03000000321500000010000011010000,Razer RAIJU,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000008916000000fe000024010000,Razer Sabertooth,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000c6240000045d000024010000,Razer Sabertooth,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "03000000c6240000045d000025010000,Razer Sabertooth,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "03000000321500000009000011010000,Razer Serval,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a4,rightx:a2,righty:a3,start:b7,x:b2,y:b3,", "050000003215000000090000163a0000,Razer Serval,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a4,rightx:a2,righty:a3,start:b7,x:b2,y:b3,", "0300000032150000030a000001010000,Razer Wildcat,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "0300000000f000000300000000010000,RetroPad,a:b1,b:b5,back:b2,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b3,x:b0,y:b4,", "03000000790000001100000010010000,Retrolink SNES Controller,a:b2,b:b1,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b5,start:b9,x:b3,y:b0,", "030000006b140000010d000011010000,Revolution Pro Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000006f0e00001e01000011010000,Rock Candy PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000006f0e00004601000001010000,Rock Candy Xbox One Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000006f0e00001f01000000010000,Rock Candy,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000632500007505000010010000,SHANWAN PS3/PC Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000341a00000908000010010000,SL-6566,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "03000000a306000023f6000011010000,Saitek Cyborg V.1 Game Pad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a4,start:b9,x:b0,y:b3,", "03000000a30600000cff000010010000,Saitek P2500 Force Rumble Pad,a:b2,b:b3,back:b11,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a3,righty:a2,x:b0,y:b1,", + "03000000a30600000c04000011010000,Saitek P2900 Wireless Pad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b9,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b12,x:b0,y:b3,", + "03000000a30600000901000000010000,Saitek P880,a:b2,b:b3,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a3,righty:a2,x:b0,y:b1,", "03000000a30600000b04000000010000,Saitek P990 Dual Analog Pad,a:b1,b:b2,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b8,x:b0,y:b3,", - "03000000de2800000211000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", - "05000000de2800000511000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", + "03000000a306000018f5000010010000,Saitek PLC Saitek P3200 Rumble Pad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b0,y:b3,", + "03000000c01600008704000011010000,Serial/Keyboard/Mouse/Joystick,a:b12,b:b10,back:b4,dpdown:b2,dpleft:b3,dpright:b1,dpup:b0,leftshoulder:b9,leftstick:b14,lefttrigger:b6,leftx:a1,lefty:a0,rightshoulder:b8,rightstick:b15,righttrigger:b7,rightx:a2,righty:a3,start:b5,x:b13,y:b11,", + "03000000f025000021c1000010010000,ShanWan Gioteck PS3 Wired Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000632500002305000010010000,ShanWan USB Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,", + "03000000250900000500000000010000,Sony PS2 pad with SmartJoy adapter,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,", + "030000005e0400008e02000020200000,SpeedLink XEOX Pro Analog Gamepad pad,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000005e0400008e02000073050000,Speedlink TORID Wireless Gamepad,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "03000000de2800000112000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", - "05000000de2800000212000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", + "03000000de2800000211000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", "03000000de2800004211000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", - "03000000de280000fc11000001000000,Steam Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000de280000fc11000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "05000000de2800000212000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", + "05000000de2800000511000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", + "05000000de2800000611000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", "03000000de280000ff11000001000000,Steam Virtual Gamepad,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000ad1b000038f0000090040000,Street Fighter IV FightStick TE,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000666600000488000000010000,Super Joy Box 5 Pro,a:b2,b:b1,back:b9,dpdown:b14,dpleft:b15,dpright:b13,dpup:b12,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,", + "0300000000f00000f100000000010000,Super RetroPort,a:b1,b:b5,back:b2,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b3,x:b0,y:b4,", + "030000004f04000020b3000010010000,Thrustmaster 2 in 1 DT,a:b0,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b1,y:b3,", + "030000004f04000015b3000010010000,Thrustmaster Dual Analog 4,a:b0,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b1,y:b3,", + "030000004f04000023b3000000010000,Thrustmaster Dual Trigger 3-in-1,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "030000004f04000000b3000010010000,Thrustmaster Firestorm Dual Power,a:b0,b:b2,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b11,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b12,righttrigger:b7,rightx:a2,righty:a3,start:b10,x:b1,y:b3,", + "030000004f04000009d0000000010000,Thrustmaster Run N Drive Wireless PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "030000004f04000008d0000000010000,Thrustmaster Run N Drive Wireless,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000bd12000015d0000010010000,Tomee SNES USB Controller,a:b2,b:b1,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b5,start:b9,x:b3,y:b0,", + "03000000d814000007cd000011010000,Toodles 2008 Chimp PC/PS3,a:b0,b:b1,back:b8,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,start:b9,x:b3,y:b2,", + "03000000100800000100000010010000,Twin USB PS2 Adapter,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b3,y:b0,", + "03000000100800000300000010010000,USB Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b3,y:b0,", + "03000000790000001100000000010000,USB Gamepad1,a:b2,b:b1,back:b8,dpdown:a0,dpleft:a1,dpright:a2,dpup:a4,start:b9,", + "03000000790000000600000007010000,USB gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b3,y:b0,", + "05000000ac0500003232000001000000,VR-BOX,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b2,y:b3,", + "030000005e0400008e02000010010000,X360 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000005e0400008e02000014010000,X360 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000005e0400001907000000010000,X360 Wireless Controller,a:b0,b:b1,back:b6,dpdown:b14,dpleft:b11,dpright:b12,dpup:b13,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000005e0400009102000007010000,X360 Wireless Controller,a:b0,b:b1,back:b6,dpdown:b14,dpleft:b11,dpright:b12,dpup:b13,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000005e040000a102000000010000,X360 Wireless Controller,a:b0,b:b1,back:b6,dpdown:b14,dpleft:b11,dpright:b12,dpup:b13,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "030000005e040000a102000007010000,X360 Wireless Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "03000000450c00002043000010010000,XEOX Gamepad SL-6556-BK,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", "xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "0000000058626f782033363020576900,Xbox 360 Wireless Controller,a:b0,b:b1,back:b14,dpdown:b11,dpleft:b12,dpright:b13,dpup:b10,guide:b7,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,", + "030000005e040000a102000014010000,Xbox 360 Wireless Receiver (XBOX),a:b0,b:b1,back:b6,dpdown:b14,dpleft:b11,dpright:b12,dpup:b13,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", + "0000000058626f782047616d65706100,Xbox Gamepad (userspace driver),a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a4,rightx:a2,righty:a3,start:b7,x:b2,y:b3,", "050000005e040000e002000003090000,Xbox One Wireless Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,", "050000005e040000fd02000003090000,Xbox One Wireless Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b16,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,", "05000000172700004431000029010000,XiaoMi Game Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b20,leftshoulder:b6,leftstick:b13,lefttrigger:a7,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a6,rightx:a2,righty:a5,start:b11,x:b3,y:b4,", + "03000000c0160000e105000001010000,Xin-Mo Xin-Mo Dual Arcade,a:b4,b:b3,back:b6,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b9,leftshoulder:b2,leftx:a0,lefty:a1,rightshoulder:b5,start:b7,x:b1,y:b0,", "03000000120c0000100e000011010000,ZEROPLUS P4 Gamepad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,", + "03000000666600006706000000010000,boom PSX to PC Converter,a:b2,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b13,dpup:b12,leftshoulder:b6,leftstick:b9,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b10,righttrigger:b5,rightx:a2,righty:a3,start:b11,x:b3,y:b0,", + "030000000d0f00000d00000000010000,hori,a:b0,b:b6,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b3,leftx:b4,lefty:b5,rightshoulder:b7,start:b9,x:b1,y:b2,", "03000000830500006020000010010000,iBuffalo SNES Controller,a:b1,b:b0,back:b6,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b5,start:b7,x:b3,y:b2,", + "050000006964726f69643a636f6e0000,idroid:con,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,", + "03000000b50700001503000010010000,impact,a:b2,b:b3,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,rightstick:b11,righttrigger:b7,rightx:a3,righty:a2,start:b9,x:b0,y:b1,", + "030000009b2800000300000001010000,raphnet.net 4nes4snes v1.5,a:b0,b:b4,back:b2,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b3,x:b1,y:b5,", #endif #if defined(__ANDROID__) - "34323662653333636330306631326233,ASUS Gamepad,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b6,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,x:b2,y:b3,", - "64633436313965656664373634323364,Microsoft X-Box 360 pad,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,x:b2,y:b3,", - "4e564944494120436f72706f72617469,NVIDIA Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,", - "61363931656135336130663561616264,NVIDIA Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,", - "37336435666338653565313731303834,NVIDIA Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,", - "35643031303033326130316330353564,PS4 Controller,a:b1,b:b17,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b3,leftstick:b4,lefttrigger:+a3,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:+a4,rightx:a2,righty:a5,start:b16,x:b0,y:b2,", + "05000000d6020000e5890000dfff3f00,GPD XD Plus,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a5,start:b6,x:b2,y:b3,", + "05000000bc20000000550000ffff3f00,GameSir G3w,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,", + "050000005509000003720000cf7f3f00,NVIDIA Controller v01.01,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,", + "050000005509000010720000ffff3f00,NVIDIA Controller v01.03,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,", + "050000007e05000009200000ffff0f00,Nintendo Switch Pro Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,leftstick:b4,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b16,x:b17,y:b2,", /* Extremely slow in Bluetooth mode on Android */ + "050000004c05000068020000dfff3f00,PS3 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,", + "050000004c050000c4050000fffe3f00,PS4 Controller,a:b1,b:b17,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,leftstick:b4,lefttrigger:+a3,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:+a4,rightx:a2,righty:a5,start:b16,x:b0,y:b2,", + "050000004c050000cc090000fffe3f00,PS4 Controller,a:b1,b:b17,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,leftstick:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:a4,rightx:a2,righty:a5,start:b16,x:b0,y:b2,", + "050000003215000000090000bf7f3f00,Razer Serval,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,x:b2,y:b3,", "05000000de2800000511000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", - "34356136633366613530316338376136,Xbox Wireless Controller,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b3,leftstick:b15,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b16,righttrigger:a5,rightx:a3,righty:a4,x:b17,y:b2,", + "05000000de2800000611000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", + "050000005e040000e00200000ffe3f00,Xbox One Wireless Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b16,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b17,y:b2,", + "050000005e040000fd020000ffff3f00,Xbox One Wireless Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,", + "050000005e04000091020000ff073f00,Xbox Wireless Controller,a:b0,b:b1,back:b4,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,", /* The DPAD doesn't seem to work on this controller on Android TV? */ #endif #if defined(SDL_JOYSTICK_MFI) - "4d466947616d65706164010000000000,MFi Extended Gamepad,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,", - "4d466947616d65706164020000000000,MFi Gamepad,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,rightshoulder:b5,start:b6,x:b2,y:b3,", - "4d466947616d65706164030000000000,Remote,a:b0,b:b2,leftx:a0,lefty:a1,", + "05000000ac0500000100000000006d01,*,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b6,leftshoulder:b4,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a5,rightx:a3,righty:a4,x:b2,y:b3,", + "05000000ac0500000200000000006d02,*,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b6,leftshoulder:b4,rightshoulder:b5,x:b2,y:b3,", + "05000000ac0500000300000000006d03,Remote,a:b0,b:b2,leftx:a0,lefty:a1,", "05000000de2800000511000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", + "05000000de2800000611000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,", #endif #if defined(SDL_JOYSTICK_EMSCRIPTEN) - "emscripten,Standard Gamepad,a:b0,b:b1,back:b8,dpdown:b13,dpleft:b14,dpright:b15,dpup:b12,guide:b16,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", + "default,Standard Gamepad,a:b0,b:b1,back:b8,dpdown:b13,dpleft:b14,dpright:b15,dpup:b12,guide:b16,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,", #endif + "hidapi,*,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,", NULL }; diff --git a/Engine/lib/sdl/src/joystick/SDL_joystick.c b/Engine/lib/sdl/src/joystick/SDL_joystick.c index b93c03d37..02903f5f9 100644 --- a/Engine/lib/sdl/src/joystick/SDL_joystick.c +++ b/Engine/lib/sdl/src/joystick/SDL_joystick.c @@ -23,6 +23,7 @@ /* This is the joystick API for Simple DirectMedia Layer */ #include "SDL.h" +#include "SDL_atomic.h" #include "SDL_events.h" #include "SDL_sysjoystick.h" #include "SDL_assert.h" @@ -33,11 +34,54 @@ #endif #include "../video/SDL_sysvideo.h" +/* This is included in only one place because it has a large static list of controllers */ +#include "controller_type.h" +#ifdef __WIN32__ +/* Needed for checking for input remapping programs */ +#include "../core/windows/SDL_windows.h" + +#undef UNICODE /* We want ASCII functions */ +#include +#endif + +static SDL_JoystickDriver *SDL_joystick_drivers[] = { +#if defined(SDL_JOYSTICK_DINPUT) || defined(SDL_JOYSTICK_XINPUT) + &SDL_WINDOWS_JoystickDriver, +#endif +#ifdef SDL_JOYSTICK_LINUX + &SDL_LINUX_JoystickDriver, +#endif +#ifdef SDL_JOYSTICK_IOKIT + &SDL_DARWIN_JoystickDriver, +#endif +#if defined(__IPHONEOS__) || defined(__TVOS__) + &SDL_IOS_JoystickDriver, +#endif +#ifdef SDL_JOYSTICK_ANDROID + &SDL_ANDROID_JoystickDriver, +#endif +#ifdef SDL_JOYSTICK_EMSCRIPTEN + &SDL_EMSCRIPTEN_JoystickDriver, +#endif +#ifdef SDL_JOYSTICK_HAIKU + &SDL_HAIKU_JoystickDriver, +#endif +#ifdef SDL_JOYSTICK_USBHID /* !!! FIXME: "USBHID" is a generic name, and doubly-confusing with HIDAPI next to it. This is the *BSD interface, rename this. */ + &SDL_BSD_JoystickDriver, +#endif +#ifdef SDL_JOYSTICK_HIDAPI + &SDL_HIDAPI_JoystickDriver, +#endif +#if defined(SDL_JOYSTICK_DUMMY) || defined(SDL_JOYSTICK_DISABLED) + &SDL_DUMMY_JoystickDriver +#endif +}; static SDL_bool SDL_joystick_allows_background_events = SDL_FALSE; static SDL_Joystick *SDL_joysticks = NULL; static SDL_bool SDL_updating_joystick = SDL_FALSE; static SDL_mutex *SDL_joystick_lock = NULL; /* This needs to support recursive locks */ +static SDL_atomic_t SDL_next_joystick_instance_id; void SDL_LockJoysticks(void) @@ -69,7 +113,7 @@ SDL_JoystickAllowBackgroundEventsChanged(void *userdata, const char *name, const int SDL_JoystickInit(void) { - int status; + int i, status; SDL_GameControllerInitMappings(); @@ -88,11 +132,13 @@ SDL_JoystickInit(void) } #endif /* !SDL_EVENTS_DISABLED */ - status = SDL_SYS_JoystickInit(); - if (status >= 0) { - status = 0; + status = -1; + for (i = 0; i < SDL_arraysize(SDL_joystick_drivers); ++i) { + if (SDL_joystick_drivers[i]->Init() >= 0) { + status = 0; + } } - return (status); + return status; } /* @@ -101,20 +147,99 @@ SDL_JoystickInit(void) int SDL_NumJoysticks(void) { - return SDL_SYS_NumJoysticks(); + int i, total_joysticks = 0; + SDL_LockJoysticks(); + for (i = 0; i < SDL_arraysize(SDL_joystick_drivers); ++i) { + total_joysticks += SDL_joystick_drivers[i]->GetCount(); + } + SDL_UnlockJoysticks(); + return total_joysticks; } +/* + * Return the next available joystick instance ID + * This may be called by drivers from multiple threads, unprotected by any locks + */ +SDL_JoystickID SDL_GetNextJoystickInstanceID() +{ + return SDL_AtomicIncRef(&SDL_next_joystick_instance_id); +} + +/* + * Get the driver and device index for an API device index + * This should be called while the joystick lock is held, to prevent another thread from updating the list + */ +SDL_bool +SDL_GetDriverAndJoystickIndex(int device_index, SDL_JoystickDriver **driver, int *driver_index) +{ + int i, num_joysticks, total_joysticks = 0; + + if (device_index >= 0) { + for (i = 0; i < SDL_arraysize(SDL_joystick_drivers); ++i) { + num_joysticks = SDL_joystick_drivers[i]->GetCount(); + if (device_index < num_joysticks) { + *driver = SDL_joystick_drivers[i]; + *driver_index = device_index; + return SDL_TRUE; + } + device_index -= num_joysticks; + total_joysticks += num_joysticks; + } + } + + SDL_SetError("There are %d joysticks available", total_joysticks); + return SDL_FALSE; +} + +/* + * Perform any needed fixups for joystick names + */ +static const char * +SDL_FixupJoystickName(const char *name) +{ + if (name) { + const char *skip_prefix = "NVIDIA Corporation "; + + if (SDL_strncmp(name, skip_prefix, SDL_strlen(skip_prefix)) == 0) { + name += SDL_strlen(skip_prefix); + } + } + return name; +} + + /* * Get the implementation dependent name of a joystick */ const char * SDL_JoystickNameForIndex(int device_index) { - if (device_index < 0 || device_index >= SDL_NumJoysticks()) { - SDL_SetError("There are %d joysticks available", SDL_NumJoysticks()); - return (NULL); + SDL_JoystickDriver *driver; + const char *name = NULL; + + SDL_LockJoysticks(); + if (SDL_GetDriverAndJoystickIndex(device_index, &driver, &device_index)) { + name = SDL_FixupJoystickName(driver->GetDeviceName(device_index)); } - return (SDL_SYS_JoystickNameForDeviceIndex(device_index)); + SDL_UnlockJoysticks(); + + /* FIXME: Really we should reference count this name so it doesn't go away after unlock */ + return name; +} + +int +SDL_JoystickGetDevicePlayerIndex(int device_index) +{ + SDL_JoystickDriver *driver; + int player_index = -1; + + SDL_LockJoysticks(); + if (SDL_GetDriverAndJoystickIndex(device_index, &driver, &device_index)) { + player_index = driver->GetDevicePlayerIndex(device_index); + } + SDL_UnlockJoysticks(); + + return player_index; } /* @@ -159,27 +284,30 @@ SDL_JoystickAxesCenteredAtZero(SDL_Joystick *joystick) SDL_Joystick * SDL_JoystickOpen(int device_index) { + SDL_JoystickDriver *driver; + SDL_JoystickID instance_id; SDL_Joystick *joystick; SDL_Joystick *joysticklist; const char *joystickname = NULL; - if ((device_index < 0) || (device_index >= SDL_NumJoysticks())) { - SDL_SetError("There are %d joysticks available", SDL_NumJoysticks()); - return (NULL); - } - SDL_LockJoysticks(); + if (!SDL_GetDriverAndJoystickIndex(device_index, &driver, &device_index)) { + SDL_UnlockJoysticks(); + return NULL; + } + joysticklist = SDL_joysticks; /* If the joystick is already open, return it * it is important that we have a single joystick * for each instance id */ + instance_id = driver->GetDeviceInstanceID(device_index); while (joysticklist) { - if (SDL_JoystickGetDeviceInstanceID(device_index) == joysticklist->instance_id) { + if (instance_id == joysticklist->instance_id) { joystick = joysticklist; ++joystick->ref_count; SDL_UnlockJoysticks(); - return (joystick); + return joystick; } joysticklist = joysticklist->next; } @@ -191,18 +319,25 @@ SDL_JoystickOpen(int device_index) SDL_UnlockJoysticks(); return NULL; } + joystick->driver = driver; + joystick->instance_id = instance_id; + joystick->attached = SDL_TRUE; + joystick->player_index = -1; - if (SDL_SYS_JoystickOpen(joystick, device_index) < 0) { + if (driver->Open(joystick, device_index) < 0) { SDL_free(joystick); SDL_UnlockJoysticks(); return NULL; } - joystickname = SDL_SYS_JoystickNameForDeviceIndex(device_index); - if (joystickname) + joystickname = driver->GetDeviceName(device_index); + if (joystickname) { joystick->name = SDL_strdup(joystickname); - else + } else { joystick->name = NULL; + } + + joystick->guid = driver->GetDeviceGUID(device_index); if (joystick->naxes > 0) { joystick->axes = (SDL_JoystickAxisInfo *) SDL_calloc(joystick->naxes, sizeof(SDL_JoystickAxisInfo)); @@ -246,9 +381,9 @@ SDL_JoystickOpen(int device_index) SDL_UnlockJoysticks(); - SDL_SYS_JoystickUpdate(joystick); + driver->Update(joystick); - return (joystick); + return joystick; } @@ -277,9 +412,9 @@ int SDL_JoystickNumAxes(SDL_Joystick * joystick) { if (!SDL_PrivateJoystickValid(joystick)) { - return (-1); + return -1; } - return (joystick->naxes); + return joystick->naxes; } /* @@ -289,9 +424,9 @@ int SDL_JoystickNumHats(SDL_Joystick * joystick) { if (!SDL_PrivateJoystickValid(joystick)) { - return (-1); + return -1; } - return (joystick->nhats); + return joystick->nhats; } /* @@ -301,9 +436,9 @@ int SDL_JoystickNumBalls(SDL_Joystick * joystick) { if (!SDL_PrivateJoystickValid(joystick)) { - return (-1); + return -1; } - return (joystick->nballs); + return joystick->nballs; } /* @@ -313,9 +448,9 @@ int SDL_JoystickNumButtons(SDL_Joystick * joystick) { if (!SDL_PrivateJoystickValid(joystick)) { - return (-1); + return -1; } - return (joystick->nbuttons); + return joystick->nbuttons; } /* @@ -327,7 +462,7 @@ SDL_JoystickGetAxis(SDL_Joystick * joystick, int axis) Sint16 state; if (!SDL_PrivateJoystickValid(joystick)) { - return (0); + return 0; } if (axis < joystick->naxes) { state = joystick->axes[axis].value; @@ -335,7 +470,7 @@ SDL_JoystickGetAxis(SDL_Joystick * joystick, int axis) SDL_SetError("Joystick only has %d axes", joystick->naxes); state = 0; } - return (state); + return state; } /* @@ -366,7 +501,7 @@ SDL_JoystickGetHat(SDL_Joystick * joystick, int hat) Uint8 state; if (!SDL_PrivateJoystickValid(joystick)) { - return (0); + return 0; } if (hat < joystick->nhats) { state = joystick->hats[hat]; @@ -374,7 +509,7 @@ SDL_JoystickGetHat(SDL_Joystick * joystick, int hat) SDL_SetError("Joystick only has %d hats", joystick->nhats); state = 0; } - return (state); + return state; } /* @@ -386,7 +521,7 @@ SDL_JoystickGetBall(SDL_Joystick * joystick, int ball, int *dx, int *dy) int retval; if (!SDL_PrivateJoystickValid(joystick)) { - return (-1); + return -1; } retval = 0; @@ -402,7 +537,7 @@ SDL_JoystickGetBall(SDL_Joystick * joystick, int ball, int *dx, int *dy) } else { return SDL_SetError("Joystick only has %d balls", joystick->nballs); } - return (retval); + return retval; } /* @@ -414,7 +549,7 @@ SDL_JoystickGetButton(SDL_Joystick * joystick, int button) Uint8 state; if (!SDL_PrivateJoystickValid(joystick)) { - return (0); + return 0; } if (button < joystick->nbuttons) { state = joystick->buttons[button]; @@ -422,7 +557,7 @@ SDL_JoystickGetButton(SDL_Joystick * joystick, int button) SDL_SetError("Joystick only has %d buttons", joystick->nbuttons); state = 0; } - return (state); + return state; } /* @@ -436,7 +571,7 @@ SDL_JoystickGetAttached(SDL_Joystick * joystick) return SDL_FALSE; } - return SDL_SYS_JoystickAttached(joystick); + return joystick->attached; } /* @@ -446,10 +581,10 @@ SDL_JoystickID SDL_JoystickInstanceID(SDL_Joystick * joystick) { if (!SDL_PrivateJoystickValid(joystick)) { - return (-1); + return -1; } - return (joystick->instance_id); + return joystick->instance_id; } /* @@ -463,12 +598,11 @@ SDL_JoystickFromInstanceID(SDL_JoystickID joyid) SDL_LockJoysticks(); for (joystick = SDL_joysticks; joystick; joystick = joystick->next) { if (joystick->instance_id == joyid) { - SDL_UnlockJoysticks(); - return joystick; + break; } } SDL_UnlockJoysticks(); - return NULL; + return joystick; } /* @@ -478,10 +612,28 @@ const char * SDL_JoystickName(SDL_Joystick * joystick) { if (!SDL_PrivateJoystickValid(joystick)) { - return (NULL); + return NULL; } - return (joystick->name); + return SDL_FixupJoystickName(joystick->name); +} + +int +SDL_JoystickGetPlayerIndex(SDL_Joystick * joystick) +{ + if (!SDL_PrivateJoystickValid(joystick)) { + return -1; + } + return joystick->player_index; +} + +int +SDL_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + if (!SDL_PrivateJoystickValid(joystick)) { + return -1; + } + return joystick->driver->Rumble(joystick, low_frequency_rumble, high_frequency_rumble, duration_ms); } /* @@ -493,7 +645,7 @@ SDL_JoystickClose(SDL_Joystick * joystick) SDL_Joystick *joysticklist; SDL_Joystick *joysticklistprev; - if (!joystick) { + if (!SDL_PrivateJoystickValid(joystick)) { return; } @@ -510,7 +662,7 @@ SDL_JoystickClose(SDL_Joystick * joystick) return; } - SDL_SYS_JoystickClose(joystick); + joystick->driver->Close(joystick); joystick->hwdata = NULL; joysticklist = SDL_joysticks; @@ -544,6 +696,8 @@ SDL_JoystickClose(SDL_Joystick * joystick) void SDL_JoystickQuit(void) { + int i; + /* Make sure we're not getting called in the middle of updating joysticks */ SDL_assert(!SDL_updating_joystick); @@ -556,7 +710,9 @@ SDL_JoystickQuit(void) } /* Quit the joystick setup */ - SDL_SYS_JoystickQuit(); + for (i = 0; i < SDL_arraysize(SDL_joystick_drivers); ++i) { + SDL_joystick_drivers[i]->Quit(); + } SDL_UnlockJoysticks(); @@ -592,10 +748,16 @@ SDL_PrivateJoystickShouldIgnoreEvent() /* These are global for SDL_sysjoystick.c and SDL_events.c */ -void SDL_PrivateJoystickAdded(int device_index) +void SDL_PrivateJoystickAdded(SDL_JoystickID device_instance) { #if !SDL_EVENTS_DISABLED SDL_Event event; + int device_index; + + device_index = SDL_JoystickGetDeviceIndexFromInstanceID(device_instance); + if (device_index < 0) { + return; + } event.type = SDL_JOYDEVICEADDED; @@ -637,6 +799,8 @@ static void UpdateEventsForDeviceRemoval() void SDL_PrivateJoystickRemoved(SDL_JoystickID device_instance) { + SDL_Joystick *joystick; + #if !SDL_EVENTS_DISABLED SDL_Event event; @@ -649,6 +813,15 @@ void SDL_PrivateJoystickRemoved(SDL_JoystickID device_instance) UpdateEventsForDeviceRemoval(); #endif /* !SDL_EVENTS_DISABLED */ + + /* Mark this joystick as no longer attached */ + for (joystick = SDL_joysticks; joystick; joystick = joystick->next) { + if (joystick->instance_id == device_instance) { + joystick->attached = SDL_FALSE; + joystick->force_recentering = SDL_TRUE; + break; + } + } } int @@ -705,7 +878,7 @@ SDL_PrivateJoystickAxis(SDL_Joystick * joystick, Uint8 axis, Sint16 value) posted = SDL_PushEvent(&event) == 1; } #endif /* !SDL_EVENTS_DISABLED */ - return (posted); + return posted; } int @@ -745,7 +918,7 @@ SDL_PrivateJoystickHat(SDL_Joystick * joystick, Uint8 hat, Uint8 value) posted = SDL_PushEvent(&event) == 1; } #endif /* !SDL_EVENTS_DISABLED */ - return (posted); + return posted; } int @@ -781,7 +954,7 @@ SDL_PrivateJoystickBall(SDL_Joystick * joystick, Uint8 ball, posted = SDL_PushEvent(&event) == 1; } #endif /* !SDL_EVENTS_DISABLED */ - return (posted); + return posted; } int @@ -800,7 +973,7 @@ SDL_PrivateJoystickButton(SDL_Joystick * joystick, Uint8 button, Uint8 state) break; default: /* Invalid state -- bail */ - return (0); + return 0; } #endif /* !SDL_EVENTS_DISABLED */ @@ -833,12 +1006,13 @@ SDL_PrivateJoystickButton(SDL_Joystick * joystick, Uint8 button, Uint8 state) posted = SDL_PushEvent(&event) == 1; } #endif /* !SDL_EVENTS_DISABLED */ - return (posted); + return posted; } void SDL_JoystickUpdate(void) { + int i; SDL_Joystick *joystick; SDL_LockJoysticks(); @@ -855,11 +1029,15 @@ SDL_JoystickUpdate(void) SDL_UnlockJoysticks(); for (joystick = SDL_joysticks; joystick; joystick = joystick->next) { - SDL_SYS_JoystickUpdate(joystick); + if (joystick->attached) { + joystick->driver->Update(joystick); + + if (joystick->delayed_guide_button) { + SDL_GameControllerHandleDelayedGuideButton(joystick); + } + } if (joystick->force_recentering) { - int i; - /* Tell the app that everything is centered/unpressed... */ for (i = 0; i < joystick->naxes; i++) { if (joystick->axes[i].has_initial_value) { @@ -893,7 +1071,9 @@ SDL_JoystickUpdate(void) /* this needs to happen AFTER walking the joystick list above, so that any dangling hardware data from removed devices can be free'd */ - SDL_SYS_JoystickDetect(); + for (i = 0; i < SDL_arraysize(SDL_joystick_drivers); ++i) { + SDL_joystick_drivers[i]->Detect(); + } SDL_UnlockJoysticks(); } @@ -926,7 +1106,7 @@ SDL_JoystickEventState(int state) } break; } - return (state); + return state; #endif /* SDL_EVENTS_DISABLED */ } @@ -942,7 +1122,7 @@ void SDL_GetJoystickGUIDInfo(SDL_JoystickGUID guid, Uint16 *vendor, Uint16 *prod /* guid16[4] is product ID */ guid16[5] == 0x0000 /* guid16[6] is product version */ - ) { + ) { if (vendor) { *vendor = guid16[2]; } @@ -965,6 +1145,55 @@ void SDL_GetJoystickGUIDInfo(SDL_JoystickGUID guid, Uint16 *vendor, Uint16 *prod } } +SDL_bool +SDL_IsJoystickPS4(Uint16 vendor, Uint16 product) +{ + return (GuessControllerType(vendor, product) == k_eControllerType_PS4Controller); +} + +SDL_bool +SDL_IsJoystickNintendoSwitchPro(Uint16 vendor, Uint16 product) +{ + return (GuessControllerType(vendor, product) == k_eControllerType_SwitchProController); +} + +SDL_bool +SDL_IsJoystickSteamController(Uint16 vendor, Uint16 product) +{ + return BIsSteamController(GuessControllerType(vendor, product)); +} + +SDL_bool +SDL_IsJoystickXbox360(Uint16 vendor, Uint16 product) +{ + /* Filter out some bogus values here */ + if (vendor == 0x0000 && product == 0x0000) { + return SDL_FALSE; + } + if (vendor == 0x0001 && product == 0x0001) { + return SDL_FALSE; + } + return (GuessControllerType(vendor, product) == k_eControllerType_XBox360Controller); +} + +SDL_bool +SDL_IsJoystickXboxOne(Uint16 vendor, Uint16 product) +{ + return (GuessControllerType(vendor, product) == k_eControllerType_XBoxOneController); +} + +SDL_bool +SDL_IsJoystickXInput(SDL_JoystickGUID guid) +{ + return (guid.data[14] == 'x') ? SDL_TRUE : SDL_FALSE; +} + +SDL_bool +SDL_IsJoystickHIDAPI(SDL_JoystickGUID guid) +{ + return (guid.data[14] == 'h') ? SDL_TRUE : SDL_FALSE; +} + static SDL_bool SDL_IsJoystickProductWheel(Uint32 vidpid) { static Uint32 wheel_joysticks[] = { @@ -1030,7 +1259,7 @@ static SDL_JoystickType SDL_GetJoystickGUIDType(SDL_JoystickGUID guid) Uint16 product; Uint32 vidpid; - if (guid.data[14] == 'x') { + if (SDL_IsJoystickXInput(guid)) { /* XInput GUID, get the type based on the XInput device subtype */ switch (guid.data[15]) { case 0x01: /* XINPUT_DEVSUBTYPE_GAMEPAD */ @@ -1071,19 +1300,80 @@ static SDL_JoystickType SDL_GetJoystickGUIDType(SDL_JoystickGUID guid) return SDL_JOYSTICK_TYPE_THROTTLE; } + if (GuessControllerType(vendor, product) != k_eControllerType_UnknownNonSteamController) { + return SDL_JOYSTICK_TYPE_GAMECONTROLLER; + } + return SDL_JOYSTICK_TYPE_UNKNOWN; } +static SDL_bool SDL_IsPS4RemapperRunning(void) +{ +#ifdef __WIN32__ + const char *mapper_processes[] = { + "DS4Windows.exe", + "InputMapper.exe", + }; + int i; + PROCESSENTRY32 pe32; + SDL_bool found = SDL_FALSE; + + /* Take a snapshot of all processes in the system */ + HANDLE hProcessSnap = CreateToolhelp32Snapshot(TH32CS_SNAPPROCESS, 0); + if (hProcessSnap != INVALID_HANDLE_VALUE) { + pe32.dwSize = sizeof(PROCESSENTRY32); + if (Process32First(hProcessSnap, &pe32)) { + do + { + for (i = 0; i < SDL_arraysize(mapper_processes); ++i) { + if (SDL_strcasecmp(pe32.szExeFile, mapper_processes[i]) == 0) { + found = SDL_TRUE; + } + } + } while (Process32Next(hProcessSnap, &pe32) && !found); + } + CloseHandle(hProcessSnap); + } + return found; +#else + return SDL_FALSE; +#endif +} + +SDL_bool SDL_ShouldIgnoreJoystick(const char *name, SDL_JoystickGUID guid) +{ + Uint16 vendor; + Uint16 product; + + SDL_GetJoystickGUIDInfo(guid, &vendor, &product, NULL); + + if (SDL_IsJoystickPS4(vendor, product) && SDL_IsPS4RemapperRunning()) { + return SDL_TRUE; + } + + if (SDL_IsGameControllerNameAndGUID(name, guid) && + SDL_ShouldIgnoreGameController(name, guid)) { + return SDL_TRUE; + } + + return SDL_FALSE; +} + /* return the guid for this index */ SDL_JoystickGUID SDL_JoystickGetDeviceGUID(int device_index) { - if (device_index < 0 || device_index >= SDL_NumJoysticks()) { - SDL_JoystickGUID emptyGUID; - SDL_SetError("There are %d joysticks available", SDL_NumJoysticks()); - SDL_zero(emptyGUID); - return emptyGUID; + SDL_JoystickDriver *driver; + SDL_JoystickGUID guid; + + SDL_LockJoysticks(); + if (SDL_GetDriverAndJoystickIndex(device_index, &driver, &device_index)) { + guid = driver->GetDeviceGUID(device_index); + } else { + SDL_zero(guid); } - return SDL_SYS_JoystickGetDeviceGUID(device_index); + SDL_UnlockJoysticks(); + + return guid; } Uint16 SDL_JoystickGetDeviceVendor(int device_index) @@ -1129,11 +1419,33 @@ SDL_JoystickType SDL_JoystickGetDeviceType(int device_index) SDL_JoystickID SDL_JoystickGetDeviceInstanceID(int device_index) { - if (device_index < 0 || device_index >= SDL_NumJoysticks()) { - SDL_SetError("There are %d joysticks available", SDL_NumJoysticks()); - return -1; + SDL_JoystickDriver *driver; + SDL_JoystickID instance_id = -1; + + SDL_LockJoysticks(); + if (SDL_GetDriverAndJoystickIndex(device_index, &driver, &device_index)) { + instance_id = driver->GetDeviceInstanceID(device_index); } - return SDL_SYS_GetInstanceIdOfDeviceIndex(device_index); + SDL_UnlockJoysticks(); + + return instance_id; +} + +int SDL_JoystickGetDeviceIndexFromInstanceID(SDL_JoystickID instance_id) +{ + int i, num_joysticks, device_index = -1; + + SDL_LockJoysticks(); + num_joysticks = SDL_NumJoysticks(); + for (i = 0; i < num_joysticks; ++i) { + if (SDL_JoystickGetDeviceInstanceID(i) == instance_id) { + device_index = i; + break; + } + } + SDL_UnlockJoysticks(); + + return device_index; } SDL_JoystickGUID SDL_JoystickGetGUID(SDL_Joystick * joystick) @@ -1143,7 +1455,7 @@ SDL_JoystickGUID SDL_JoystickGetGUID(SDL_Joystick * joystick) SDL_zero(emptyGUID); return emptyGUID; } - return SDL_SYS_JoystickGetGUID(joystick); + return joystick->guid; } Uint16 SDL_JoystickGetVendor(SDL_Joystick * joystick) @@ -1208,7 +1520,6 @@ void SDL_JoystickGetGUIDString(SDL_JoystickGUID guid, char *pszGUID, int cbGUID) *pszGUID = '\0'; } - /*----------------------------------------------------------------------------- * Purpose: Returns the 4 bit nibble for a hex character * Input : c - @@ -1233,7 +1544,6 @@ static unsigned char nibble(char c) return 0; } - /* convert the string version of a joystick guid to the struct */ SDL_JoystickGUID SDL_JoystickGetGUIDFromString(const char *pchGUID) { @@ -1256,19 +1566,17 @@ SDL_JoystickGUID SDL_JoystickGetGUIDFromString(const char *pchGUID) return guid; } - /* update the power level for this joystick */ void SDL_PrivateJoystickBatteryLevel(SDL_Joystick * joystick, SDL_JoystickPowerLevel ePowerLevel) { joystick->epowerlevel = ePowerLevel; } - /* return its power level */ SDL_JoystickPowerLevel SDL_JoystickCurrentPowerLevel(SDL_Joystick * joystick) { if (!SDL_PrivateJoystickValid(joystick)) { - return (SDL_JOYSTICK_POWER_UNKNOWN); + return SDL_JOYSTICK_POWER_UNKNOWN; } return joystick->epowerlevel; } diff --git a/Engine/lib/sdl/src/joystick/SDL_joystick_c.h b/Engine/lib/sdl/src/joystick/SDL_joystick_c.h index 0a8fdb455..900d5904c 100644 --- a/Engine/lib/sdl/src/joystick/SDL_joystick_c.h +++ b/Engine/lib/sdl/src/joystick/SDL_joystick_c.h @@ -18,32 +18,74 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_joystick_c_h_ +#define SDL_joystick_c_h_ + #include "../SDL_internal.h" /* Useful functions and variables from SDL_joystick.c */ #include "SDL_joystick.h" +struct _SDL_JoystickDriver; + /* Initialization and shutdown functions */ extern int SDL_JoystickInit(void); extern void SDL_JoystickQuit(void); +/* Function to get the next available joystick instance ID */ +extern SDL_JoystickID SDL_GetNextJoystickInstanceID(void); + /* Initialization and shutdown functions */ extern int SDL_GameControllerInitMappings(void); extern void SDL_GameControllerQuitMappings(void); extern int SDL_GameControllerInit(void); extern void SDL_GameControllerQuit(void); +/* Function to get the joystick driver and device index for an API device index */ +extern SDL_bool SDL_GetDriverAndJoystickIndex(int device_index, struct _SDL_JoystickDriver **driver, int *driver_index); + +/* Function to return the device index for a joystick ID, or -1 if not found */ +extern int SDL_JoystickGetDeviceIndexFromInstanceID(SDL_JoystickID instance_id); + /* Function to extract information from an SDL joystick GUID */ extern void SDL_GetJoystickGUIDInfo(SDL_JoystickGUID guid, Uint16 *vendor, Uint16 *product, Uint16 *version); +/* Function to return whether a joystick is a PS4 controller */ +extern SDL_bool SDL_IsJoystickPS4(Uint16 vendor_id, Uint16 product_id); + +/* Function to return whether a joystick is a Nintendo Switch Pro controller */ +extern SDL_bool SDL_IsJoystickNintendoSwitchPro(Uint16 vendor_id, Uint16 product_id); + +/* Function to return whether a joystick is a Steam Controller */ +extern SDL_bool SDL_IsJoystickSteamController(Uint16 vendor_id, Uint16 product_id); + +/* Function to return whether a joystick is an Xbox 360 controller */ +extern SDL_bool SDL_IsJoystickXbox360(Uint16 vendor_id, Uint16 product_id); + +/* Function to return whether a joystick is an Xbox One controller */ +extern SDL_bool SDL_IsJoystickXboxOne(Uint16 vendor_id, Uint16 product_id); + +/* Function to return whether a joystick guid comes from the XInput driver */ +extern SDL_bool SDL_IsJoystickXInput(SDL_JoystickGUID guid); + +/* Function to return whether a joystick guid comes from the HIDAPI driver */ +extern SDL_bool SDL_IsJoystickHIDAPI(SDL_JoystickGUID guid); + +/* Function to return whether a joystick should be ignored */ +extern SDL_bool SDL_ShouldIgnoreJoystick(const char *name, SDL_JoystickGUID guid); + /* Function to return whether a joystick name and GUID is a game controller */ extern SDL_bool SDL_IsGameControllerNameAndGUID(const char *name, SDL_JoystickGUID guid); /* Function to return whether a game controller should be ignored */ extern SDL_bool SDL_ShouldIgnoreGameController(const char *name, SDL_JoystickGUID guid); +/* Handle delayed guide button on a game controller */ +extern void SDL_GameControllerHandleDelayedGuideButton(SDL_Joystick *joystick); + /* Internal event queueing functions */ -extern void SDL_PrivateJoystickAdded(int device_index); +extern void SDL_PrivateJoystickAdded(SDL_JoystickID device_instance); extern void SDL_PrivateJoystickRemoved(SDL_JoystickID device_instance); extern int SDL_PrivateJoystickAxis(SDL_Joystick * joystick, Uint8 axis, Sint16 value); @@ -59,4 +101,6 @@ extern void SDL_PrivateJoystickBatteryLevel(SDL_Joystick * joystick, /* Internal sanity checking functions */ extern int SDL_PrivateJoystickValid(SDL_Joystick * joystick); +#endif /* SDL_joystick_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/SDL_sysjoystick.h b/Engine/lib/sdl/src/joystick/SDL_sysjoystick.h index 7de5d83d2..341669361 100644 --- a/Engine/lib/sdl/src/joystick/SDL_sysjoystick.h +++ b/Engine/lib/sdl/src/joystick/SDL_sysjoystick.h @@ -42,6 +42,8 @@ struct _SDL_Joystick { SDL_JoystickID instance_id; /* Device instance, monotonically increasing from 0 */ char *name; /* Joystick name - system dependent */ + int player_index; /* Joystick player index, or -1 if unavailable */ + SDL_JoystickGUID guid; /* Joystick guid */ int naxes; /* Number of axis controls on the joystick */ SDL_JoystickAxisInfo *axes; @@ -58,78 +60,97 @@ struct _SDL_Joystick int nbuttons; /* Number of buttons on the joystick */ Uint8 *buttons; /* Current button states */ + SDL_bool attached; + SDL_bool is_game_controller; + SDL_bool delayed_guide_button; /* SDL_TRUE if this device has the guide button event delayed */ + SDL_bool force_recentering; /* SDL_TRUE if this device needs to have its state reset to 0 */ + SDL_JoystickPowerLevel epowerlevel; /* power level of this joystick, SDL_JOYSTICK_POWER_UNKNOWN if not supported */ + struct _SDL_JoystickDriver *driver; + struct joystick_hwdata *hwdata; /* Driver dependent information */ int ref_count; /* Reference count for multiple opens */ - SDL_bool is_game_controller; - SDL_bool force_recentering; /* SDL_TRUE if this device needs to have its state reset to 0 */ - SDL_JoystickPowerLevel epowerlevel; /* power level of this joystick, SDL_JOYSTICK_POWER_UNKNOWN if not supported */ struct _SDL_Joystick *next; /* pointer to next joystick we have allocated */ }; +#if defined(__IPHONEOS__) || defined(__ANDROID__) +#define HAVE_STEAMCONTROLLERS +#define USE_STEAMCONTROLLER_HIDAPI +#elif defined(__LINUX__) +#define HAVE_STEAMCONTROLLERS +#define USE_STEAMCONTROLLER_LINUX +#endif + +/* Device bus definitions */ +#define SDL_HARDWARE_BUS_USB 0x03 +#define SDL_HARDWARE_BUS_BLUETOOTH 0x05 + /* Macro to combine a USB vendor ID and product ID into a single Uint32 value */ #define MAKE_VIDPID(VID, PID) (((Uint32)(VID))<<16|(PID)) -/* Function to scan the system for joysticks. - * Joystick 0 should be the system default joystick. - * This function should return the number of available joysticks, or -1 - * on an unrecoverable fatal error. - */ -extern int SDL_SYS_JoystickInit(void); +typedef struct _SDL_JoystickDriver +{ + /* Function to scan the system for joysticks. + * Joystick 0 should be the system default joystick. + * This function should return 0, or -1 on an unrecoverable error. + */ + int (*Init)(void); -/* Function to return the number of joystick devices plugged in right now */ -extern int SDL_SYS_NumJoysticks(void); + /* Function to return the number of joystick devices plugged in right now */ + int (*GetCount)(void); -/* Function to cause any queued joystick insertions to be processed */ -extern void SDL_SYS_JoystickDetect(void); + /* Function to cause any queued joystick insertions to be processed */ + void (*Detect)(void); -/* Function to get the device-dependent name of a joystick */ -extern const char *SDL_SYS_JoystickNameForDeviceIndex(int device_index); + /* Function to get the device-dependent name of a joystick */ + const char *(*GetDeviceName)(int device_index); -/* Function to get the current instance id of the joystick located at device_index */ -extern SDL_JoystickID SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index); + /* Function to get the player index of a joystick */ + int (*GetDevicePlayerIndex)(int device_index); -/* Function to open a joystick for use. - The joystick to open is specified by the device index. - This should fill the nbuttons and naxes fields of the joystick structure. - It returns 0, or -1 if there is an error. - */ -extern int SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index); + /* Function to return the stable GUID for a plugged in device */ + SDL_JoystickGUID (*GetDeviceGUID)(int device_index); -/* Function to query if the joystick is currently attached - * It returns SDL_TRUE if attached, SDL_FALSE otherwise. - */ -extern SDL_bool SDL_SYS_JoystickAttached(SDL_Joystick * joystick); + /* Function to get the current instance id of the joystick located at device_index */ + SDL_JoystickID (*GetDeviceInstanceID)(int device_index); -/* Function to update the state of a joystick - called as a device poll. - * This function shouldn't update the joystick structure directly, - * but instead should call SDL_PrivateJoystick*() to deliver events - * and update joystick device state. - */ -extern void SDL_SYS_JoystickUpdate(SDL_Joystick * joystick); + /* Function to open a joystick for use. + The joystick to open is specified by the device index. + This should fill the nbuttons and naxes fields of the joystick structure. + It returns 0, or -1 if there is an error. + */ + int (*Open)(SDL_Joystick * joystick, int device_index); -/* Function to close a joystick after use */ -extern void SDL_SYS_JoystickClose(SDL_Joystick * joystick); + /* Rumble functionality */ + int (*Rumble)(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms); -/* Function to perform any system-specific joystick related cleanup */ -extern void SDL_SYS_JoystickQuit(void); + /* Function to update the state of a joystick - called as a device poll. + * This function shouldn't update the joystick structure directly, + * but instead should call SDL_PrivateJoystick*() to deliver events + * and update joystick device state. + */ + void (*Update)(SDL_Joystick * joystick); -/* Function to return the stable GUID for a plugged in device */ -extern SDL_JoystickGUID SDL_SYS_JoystickGetDeviceGUID(int device_index); + /* Function to close a joystick after use */ + void (*Close)(SDL_Joystick * joystick); -/* Function to return the stable GUID for a opened joystick */ -extern SDL_JoystickGUID SDL_SYS_JoystickGetGUID(SDL_Joystick * joystick); + /* Function to perform any system-specific joystick related cleanup */ + void (*Quit)(void); -#if SDL_JOYSTICK_XINPUT -/* Function returns SDL_TRUE if this device is an XInput gamepad */ -extern SDL_bool SDL_SYS_IsXInputGamepad_DeviceIndex(int device_index); -#endif +} SDL_JoystickDriver; -#if defined(__ANDROID__) -/* Function returns SDL_TRUE if this device is a DPAD (maybe a TV remote) */ -extern SDL_bool SDL_SYS_IsDPAD_DeviceIndex(int device_index); -#endif +/* The available joystick drivers */ +extern SDL_JoystickDriver SDL_ANDROID_JoystickDriver; +extern SDL_JoystickDriver SDL_BSD_JoystickDriver; +extern SDL_JoystickDriver SDL_DARWIN_JoystickDriver; +extern SDL_JoystickDriver SDL_DUMMY_JoystickDriver; +extern SDL_JoystickDriver SDL_EMSCRIPTEN_JoystickDriver; +extern SDL_JoystickDriver SDL_HAIKU_JoystickDriver; +extern SDL_JoystickDriver SDL_HIDAPI_JoystickDriver; +extern SDL_JoystickDriver SDL_IOS_JoystickDriver; +extern SDL_JoystickDriver SDL_LINUX_JoystickDriver; +extern SDL_JoystickDriver SDL_WINDOWS_JoystickDriver; #endif /* SDL_sysjoystick_h_ */ diff --git a/Engine/lib/sdl/src/joystick/android/SDL_sysjoystick.c b/Engine/lib/sdl/src/joystick/android/SDL_sysjoystick.c index ce5a5df0b..69b657fc9 100644 --- a/Engine/lib/sdl/src/joystick/android/SDL_sysjoystick.c +++ b/Engine/lib/sdl/src/joystick/android/SDL_sysjoystick.c @@ -36,7 +36,7 @@ #include "../SDL_joystick_c.h" #include "../../events/SDL_keyboard_c.h" #include "../../core/android/SDL_android.h" -#include "../steam/SDL_steamcontroller.h" +#include "../hidapi/SDL_hidapijoystick_c.h" #include "android/keycodes.h" @@ -69,9 +69,30 @@ static SDL_joylist_item * JoystickByDeviceId(int device_id); static SDL_joylist_item *SDL_joylist = NULL; static SDL_joylist_item *SDL_joylist_tail = NULL; static int numjoysticks = 0; -static int instance_counter = 0; +/* Public domain CRC implementation adapted from: + http://home.thep.lu.se/~bjorn/crc/crc32_simple.c +*/ +static Uint32 crc32_for_byte(Uint32 r) +{ + int i; + for(i = 0; i < 8; ++i) { + r = (r & 1? 0: (Uint32)0xEDB88320L) ^ r >> 1; + } + return r ^ (Uint32)0xFF000000L; +} + +static Uint32 crc32(const void *data, int count) +{ + Uint32 crc = 0; + int i; + for(i = 0; i < count; ++i) { + crc = crc32_for_byte((Uint8)crc ^ ((const Uint8*)data)[i]) ^ crc >> 8; + } + return crc; +} + /* Function to convert Android keyCodes into SDL ones. * This code manipulation is done to get a sequential list of codes. * FIXME: This is only suited for the case where we use a fixed number of buttons determined by ANDROID_MAX_NBUTTONS @@ -213,7 +234,7 @@ Android_OnPadDown(int device_id, int keycode) if (button >= 0) { item = JoystickByDeviceId(device_id); if (item && item->joystick) { - SDL_PrivateJoystickButton(item->joystick, button , SDL_PRESSED); + SDL_PrivateJoystickButton(item->joystick, button, SDL_PRESSED); } else { SDL_SendKeyboardKey(SDL_PRESSED, button_to_scancode(button)); } @@ -256,16 +277,43 @@ Android_OnJoy(int device_id, int axis, float value) int Android_OnHat(int device_id, int hat_id, int x, int y) { - const Uint8 position_map[3][3] = { - {SDL_HAT_LEFTUP, SDL_HAT_UP, SDL_HAT_RIGHTUP}, - {SDL_HAT_LEFT, SDL_HAT_CENTERED, SDL_HAT_RIGHT}, - {SDL_HAT_LEFTDOWN, SDL_HAT_DOWN, SDL_HAT_RIGHTDOWN} - }; + const int DPAD_UP_MASK = (1 << SDL_CONTROLLER_BUTTON_DPAD_UP); + const int DPAD_DOWN_MASK = (1 << SDL_CONTROLLER_BUTTON_DPAD_DOWN); + const int DPAD_LEFT_MASK = (1 << SDL_CONTROLLER_BUTTON_DPAD_LEFT); + const int DPAD_RIGHT_MASK = (1 << SDL_CONTROLLER_BUTTON_DPAD_RIGHT); - if (x >= -1 && x <=1 && y >= -1 && y <= 1) { + if (x >= -1 && x <= 1 && y >= -1 && y <= 1) { SDL_joylist_item *item = JoystickByDeviceId(device_id); if (item && item->joystick) { - SDL_PrivateJoystickHat(item->joystick, hat_id, position_map[y+1][x+1]); + int dpad_state = 0; + int dpad_delta; + if (x < 0) { + dpad_state |= DPAD_LEFT_MASK; + } else if (x > 0) { + dpad_state |= DPAD_RIGHT_MASK; + } + if (y < 0) { + dpad_state |= DPAD_UP_MASK; + } else if (y > 0) { + dpad_state |= DPAD_DOWN_MASK; + } + + dpad_delta = (dpad_state ^ item->dpad_state); + if (dpad_delta) { + if (dpad_delta & DPAD_UP_MASK) { + SDL_PrivateJoystickButton(item->joystick, SDL_CONTROLLER_BUTTON_DPAD_UP, (dpad_state & DPAD_UP_MASK) ? SDL_PRESSED : SDL_RELEASED); + } + if (dpad_delta & DPAD_DOWN_MASK) { + SDL_PrivateJoystickButton(item->joystick, SDL_CONTROLLER_BUTTON_DPAD_DOWN, (dpad_state & DPAD_DOWN_MASK) ? SDL_PRESSED : SDL_RELEASED); + } + if (dpad_delta & DPAD_LEFT_MASK) { + SDL_PrivateJoystickButton(item->joystick, SDL_CONTROLLER_BUTTON_DPAD_LEFT, (dpad_state & DPAD_LEFT_MASK) ? SDL_PRESSED : SDL_RELEASED); + } + if (dpad_delta & DPAD_RIGHT_MASK) { + SDL_PrivateJoystickButton(item->joystick, SDL_CONTROLLER_BUTTON_DPAD_RIGHT, (dpad_state & DPAD_RIGHT_MASK) ? SDL_PRESSED : SDL_RELEASED); + } + item->dpad_state = dpad_state; + } } return 0; } @@ -275,10 +323,14 @@ Android_OnHat(int device_id, int hat_id, int x, int y) int -Android_AddJoystick(int device_id, const char *name, const char *desc, SDL_bool is_accelerometer, int nbuttons, int naxes, int nhats, int nballs) +Android_AddJoystick(int device_id, const char *name, const char *desc, int vendor_id, int product_id, SDL_bool is_accelerometer, int button_mask, int naxes, int nhats, int nballs) { - SDL_JoystickGUID guid; SDL_joylist_item *item; + SDL_JoystickGUID guid; + Uint16 *guid16 = (Uint16 *)guid.data; + int i; + int axis_mask; + if (!SDL_GetHintBoolean(SDL_HINT_TV_REMOTE_AS_JOYSTICK, SDL_TRUE)) { /* Ignore devices that aren't actually controllers (e.g. remotes), they'll be handled as keyboard input */ @@ -290,10 +342,65 @@ Android_AddJoystick(int device_id, const char *name, const char *desc, SDL_bool if (JoystickByDeviceId(device_id) != NULL || name == NULL) { return -1; } - - /* the GUID is just the first 16 chars of the name for now */ - SDL_zero(guid); - SDL_memcpy(&guid, desc, SDL_min(sizeof(guid), SDL_strlen(desc))); + +#ifdef SDL_JOYSTICK_HIDAPI + if (HIDAPI_IsDevicePresent(vendor_id, product_id, 0)) { + /* The HIDAPI driver is taking care of this device */ + return -1; + } +#endif + +#ifdef DEBUG_JOYSTICK + SDL_Log("Joystick: %s, descriptor %s, vendor = 0x%.4x, product = 0x%.4x, %d axes, %d hats\n", name, desc, vendor_id, product_id, naxes, nhats); +#endif + + /* Add the available buttons and axes + The axis mask should probably come from Java where there is more information about the axes... + */ + axis_mask = 0; + if (!is_accelerometer) { + if (naxes >= 2) { + axis_mask |= ((1 << SDL_CONTROLLER_AXIS_LEFTX) | (1 << SDL_CONTROLLER_AXIS_LEFTY)); + } + if (naxes >= 4) { + axis_mask |= ((1 << SDL_CONTROLLER_AXIS_RIGHTX) | (1 << SDL_CONTROLLER_AXIS_RIGHTY)); + } + if (naxes >= 6) { + axis_mask |= ((1 << SDL_CONTROLLER_AXIS_TRIGGERLEFT) | (1 << SDL_CONTROLLER_AXIS_TRIGGERRIGHT)); + } + } + + if (nhats > 0) { + /* Hat is translated into DPAD buttons */ + button_mask |= ((1 << SDL_CONTROLLER_BUTTON_DPAD_UP) | + (1 << SDL_CONTROLLER_BUTTON_DPAD_DOWN) | + (1 << SDL_CONTROLLER_BUTTON_DPAD_LEFT) | + (1 << SDL_CONTROLLER_BUTTON_DPAD_RIGHT)); + nhats = 0; + } + + SDL_memset(guid.data, 0, sizeof(guid.data)); + + /* We only need 16 bits for each of these; space them out to fill 128. */ + /* Byteswap so devices get same GUID on little/big endian platforms. */ + *guid16++ = SDL_SwapLE16(SDL_HARDWARE_BUS_BLUETOOTH); + *guid16++ = 0; + + if (vendor_id && product_id) { + *guid16++ = SDL_SwapLE16(vendor_id); + *guid16++ = 0; + *guid16++ = SDL_SwapLE16(product_id); + *guid16++ = 0; + } else { + Uint32 crc = crc32(desc, SDL_strlen(desc)); + SDL_memcpy(guid16, desc, SDL_min(2*sizeof(*guid16), SDL_strlen(desc))); + guid16 += 2; + *(Uint32 *)guid16 = SDL_SwapLE32(crc); + guid16 += 2; + } + + *guid16++ = SDL_SwapLE16(button_mask); + *guid16++ = SDL_SwapLE16(axis_mask); item = (SDL_joylist_item *) SDL_malloc(sizeof (SDL_joylist_item)); if (item == NULL) { @@ -310,16 +417,19 @@ Android_AddJoystick(int device_id, const char *name, const char *desc, SDL_bool } item->is_accelerometer = is_accelerometer; - if (nbuttons > -1) { - item->nbuttons = nbuttons; - } - else { + if (button_mask == 0xFFFFFFFF) { item->nbuttons = ANDROID_MAX_NBUTTONS; + } else { + for (i = 0; i < sizeof(button_mask)*8; ++i) { + if (button_mask & (1 << i)) { + item->nbuttons = i+1; + } + } } item->naxes = naxes; item->nhats = nhats; item->nballs = nballs; - item->device_instance = instance_counter++; + item->device_instance = SDL_GetNextJoystickInstanceID(); if (SDL_joylist_tail == NULL) { SDL_joylist = SDL_joylist_tail = item; } else { @@ -330,7 +440,7 @@ Android_AddJoystick(int device_id, const char *name, const char *desc, SDL_bool /* Need to increment the joystick count before we post the event */ ++numjoysticks; - SDL_PrivateJoystickAdded(numjoysticks - 1); + SDL_PrivateJoystickAdded(item->device_instance); #ifdef DEBUG_JOYSTICK SDL_Log("Added joystick %s with device_id %d", name, device_id); @@ -387,104 +497,29 @@ Android_RemoveJoystick(int device_id) } -static SDL_bool SteamControllerConnectedCallback(const char *name, SDL_JoystickGUID guid, int *device_instance) +static void ANDROID_JoystickDetect(); + +static int +ANDROID_JoystickInit(void) { - SDL_joylist_item *item; - - item = (SDL_joylist_item *)SDL_calloc(1, sizeof (SDL_joylist_item)); - if (item == NULL) { - return SDL_FALSE; - } - - *device_instance = item->device_instance = instance_counter++; - item->device_id = -1; - item->name = SDL_strdup(name); - item->guid = guid; - SDL_GetSteamControllerInputs(&item->nbuttons, - &item->naxes, - &item->nhats); - item->m_bSteamController = SDL_TRUE; - - if (SDL_joylist_tail == NULL) { - SDL_joylist = SDL_joylist_tail = item; - } else { - SDL_joylist_tail->next = item; - SDL_joylist_tail = item; - } - - /* Need to increment the joystick count before we post the event */ - ++numjoysticks; - - SDL_PrivateJoystickAdded(numjoysticks - 1); - - return SDL_TRUE; -} - -static void SteamControllerDisconnectedCallback(int device_instance) -{ - SDL_joylist_item *item = SDL_joylist; - SDL_joylist_item *prev = NULL; - - while (item != NULL) { - if (item->device_instance == device_instance) { - break; - } - prev = item; - item = item->next; - } - - if (item == NULL) { - return; - } - - if (item->joystick) { - item->joystick->hwdata = NULL; - } - - if (prev != NULL) { - prev->next = item->next; - } else { - SDL_assert(SDL_joylist == item); - SDL_joylist = item->next; - } - if (item == SDL_joylist_tail) { - SDL_joylist_tail = prev; - } - - /* Need to decrement the joystick count before we post the event */ - --numjoysticks; - - SDL_PrivateJoystickRemoved(item->device_instance); - - SDL_free(item->name); - SDL_free(item); -} - -int -SDL_SYS_JoystickInit(void) -{ - SDL_SYS_JoystickDetect(); + ANDROID_JoystickDetect(); if (SDL_GetHintBoolean(SDL_HINT_ACCELEROMETER_AS_JOYSTICK, SDL_TRUE)) { /* Default behavior, accelerometer as joystick */ - Android_AddJoystick(ANDROID_ACCELEROMETER_DEVICE_ID, ANDROID_ACCELEROMETER_NAME, ANDROID_ACCELEROMETER_NAME, SDL_TRUE, 0, 3, 0, 0); + Android_AddJoystick(ANDROID_ACCELEROMETER_DEVICE_ID, ANDROID_ACCELEROMETER_NAME, ANDROID_ACCELEROMETER_NAME, 0, 0, SDL_TRUE, 0, 3, 0, 0); } - - SDL_InitSteamControllers(SteamControllerConnectedCallback, - SteamControllerDisconnectedCallback); - - return (numjoysticks); + return 0; } -int -SDL_SYS_NumJoysticks(void) +static int +ANDROID_JoystickGetCount(void) { return numjoysticks; } -void -SDL_SYS_JoystickDetect(void) +static void +ANDROID_JoystickDetect(void) { /* Support for device connect/disconnect is API >= 16 only, * so we poll every three seconds @@ -495,8 +530,6 @@ SDL_SYS_JoystickDetect(void) timeout = SDL_GetTicks() + 3000; Android_JNI_PollInputDevices(); } - - SDL_UpdateSteamControllers(); } static SDL_joylist_item * @@ -530,7 +563,7 @@ JoystickByDeviceId(int device_id) } /* Joystick not found, try adding it */ - SDL_SYS_JoystickDetect(); + ANDROID_JoystickDetect(); while (item != NULL) { if (item->device_id == device_id) { @@ -542,26 +575,32 @@ JoystickByDeviceId(int device_id) return NULL; } -/* Function to get the device-dependent name of a joystick */ -const char * -SDL_SYS_JoystickNameForDeviceIndex(int device_index) +static const char * +ANDROID_JoystickGetDeviceName(int device_index) { return JoystickByDevIndex(device_index)->name; } -/* Function to perform the mapping from device index to the instance id for this index */ -SDL_JoystickID SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) +static int +ANDROID_JoystickGetDevicePlayerIndex(int device_index) +{ + return -1; +} + +static SDL_JoystickGUID +ANDROID_JoystickGetDeviceGUID(int device_index) +{ + return JoystickByDevIndex(device_index)->guid; +} + +static SDL_JoystickID +ANDROID_JoystickGetDeviceInstanceID(int device_index) { return JoystickByDevIndex(device_index)->device_instance; } -/* Function to open a joystick for use. - The joystick to open is specified by the device index. - This should fill the nbuttons and naxes fields of the joystick structure. - It returns 0, or -1 if there is an error. - */ -int -SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) +static int +ANDROID_JoystickOpen(SDL_Joystick * joystick, int device_index) { SDL_joylist_item *item = JoystickByDevIndex(device_index); @@ -584,14 +623,14 @@ SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) return (0); } -/* Function to determine if this joystick is attached to the system right now */ -SDL_bool SDL_SYS_JoystickAttached(SDL_Joystick *joystick) +static int +ANDROID_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) { - return joystick->hwdata != NULL; + return SDL_Unsupported(); } -void -SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) +static void +ANDROID_JoystickUpdate(SDL_Joystick * joystick) { SDL_joylist_item *item = (SDL_joylist_item *) joystick->hwdata; @@ -599,11 +638,6 @@ SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) return; } - if (item->m_bSteamController) { - SDL_UpdateSteamController(joystick); - return; - } - if (item->is_accelerometer) { int i; Sint16 value; @@ -624,9 +658,8 @@ SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) } } -/* Function to close a joystick after use */ -void -SDL_SYS_JoystickClose(SDL_Joystick * joystick) +static void +ANDROID_JoystickClose(SDL_Joystick * joystick) { SDL_joylist_item *item = (SDL_joylist_item *) joystick->hwdata; if (item) { @@ -634,9 +667,8 @@ SDL_SYS_JoystickClose(SDL_Joystick * joystick) } } -/* Function to perform any system-specific joystick related cleanup */ -void -SDL_SYS_JoystickQuit(void) +static void +ANDROID_JoystickQuit(void) { /* We don't have any way to scan for joysticks at init, so don't wipe the list * of joysticks here in case this is a reinit. @@ -654,33 +686,24 @@ SDL_SYS_JoystickQuit(void) SDL_joylist = SDL_joylist_tail = NULL; numjoysticks = 0; - instance_counter = 0; #endif /* 0 */ - - SDL_QuitSteamControllers(); } -SDL_JoystickGUID SDL_SYS_JoystickGetDeviceGUID(int device_index) +SDL_JoystickDriver SDL_ANDROID_JoystickDriver = { - return JoystickByDevIndex(device_index)->guid; -} - -SDL_JoystickGUID SDL_SYS_JoystickGetGUID(SDL_Joystick * joystick) -{ - SDL_JoystickGUID guid; - - if (joystick->hwdata != NULL) { - return ((SDL_joylist_item*)joystick->hwdata)->guid; - } - - SDL_zero(guid); - return guid; -} - -SDL_bool SDL_SYS_IsDPAD_DeviceIndex(int device_index) -{ - return JoystickByDevIndex(device_index)->naxes == 0; -} + ANDROID_JoystickInit, + ANDROID_JoystickGetCount, + ANDROID_JoystickDetect, + ANDROID_JoystickGetDeviceName, + ANDROID_JoystickGetDevicePlayerIndex, + ANDROID_JoystickGetDeviceGUID, + ANDROID_JoystickGetDeviceInstanceID, + ANDROID_JoystickOpen, + ANDROID_JoystickRumble, + ANDROID_JoystickUpdate, + ANDROID_JoystickClose, + ANDROID_JoystickQuit, +}; #endif /* SDL_JOYSTICK_ANDROID */ diff --git a/Engine/lib/sdl/src/joystick/android/SDL_sysjoystick_c.h b/Engine/lib/sdl/src/joystick/android/SDL_sysjoystick_c.h index c2cbc4e6a..20d73810d 100644 --- a/Engine/lib/sdl/src/joystick/android/SDL_sysjoystick_c.h +++ b/Engine/lib/sdl/src/joystick/android/SDL_sysjoystick_c.h @@ -32,7 +32,7 @@ extern int Android_OnPadDown(int device_id, int keycode); extern int Android_OnPadUp(int device_id, int keycode); extern int Android_OnJoy(int device_id, int axisnum, float value); extern int Android_OnHat(int device_id, int hat_id, int x, int y); -extern int Android_AddJoystick(int device_id, const char *name, const char *desc, SDL_bool is_accelerometer, int nbuttons, int naxes, int nhats, int nballs); +extern int Android_AddJoystick(int device_id, const char *name, const char *desc, int vendor_id, int product_id, SDL_bool is_accelerometer, int button_mask, int naxes, int nhats, int nballs); extern int Android_RemoveJoystick(int device_id); /* A linked list of available joysticks */ @@ -45,10 +45,8 @@ typedef struct SDL_joylist_item SDL_bool is_accelerometer; SDL_Joystick *joystick; int nbuttons, naxes, nhats, nballs; + int dpad_state; - /* Steam Controller support */ - SDL_bool m_bSteamController; - struct SDL_joylist_item *next; } SDL_joylist_item; diff --git a/Engine/lib/sdl/src/joystick/bsd/SDL_sysjoystick.c b/Engine/lib/sdl/src/joystick/bsd/SDL_sysjoystick.c index 940854504..679b80c10 100644 --- a/Engine/lib/sdl/src/joystick/bsd/SDL_sysjoystick.c +++ b/Engine/lib/sdl/src/joystick/bsd/SDL_sysjoystick.c @@ -161,15 +161,18 @@ static void report_free(struct report *); #define REP_BUF_DATA(rep) ((rep)->buf->data) #endif -static int SDL_SYS_numjoysticks = 0; +static int numjoysticks = 0; -int -SDL_SYS_JoystickInit(void) +static int BSD_JoystickOpen(SDL_Joystick * joy, int device_index); +static void BSD_JoystickClose(SDL_Joystick * joy); + +static int +BSD_JoystickInit(void) { char s[16]; int i, fd; - SDL_SYS_numjoysticks = 0; + numjoysticks = 0; SDL_memset(joynames, 0, sizeof(joynames)); SDL_memset(joydevnames, 0, sizeof(joydevnames)); @@ -179,21 +182,21 @@ SDL_SYS_JoystickInit(void) SDL_snprintf(s, SDL_arraysize(s), "/dev/uhid%d", i); - joynames[SDL_SYS_numjoysticks] = SDL_strdup(s); + joynames[numjoysticks] = SDL_strdup(s); - if (SDL_SYS_JoystickOpen(&nj, SDL_SYS_numjoysticks) == 0) { - SDL_SYS_JoystickClose(&nj); - SDL_SYS_numjoysticks++; + if (BSD_JoystickOpen(&nj, numjoysticks) == 0) { + BSD_JoystickClose(&nj); + numjoysticks++; } else { - SDL_free(joynames[SDL_SYS_numjoysticks]); - joynames[SDL_SYS_numjoysticks] = NULL; + SDL_free(joynames[numjoysticks]); + joynames[numjoysticks] = NULL; } } for (i = 0; i < MAX_JOY_JOYS; i++) { SDL_snprintf(s, SDL_arraysize(s), "/dev/joy%d", i); fd = open(s, O_RDONLY); if (fd != -1) { - joynames[SDL_SYS_numjoysticks++] = SDL_strdup(s); + joynames[numjoysticks++] = SDL_strdup(s); close(fd); } } @@ -201,22 +204,22 @@ SDL_SYS_JoystickInit(void) /* Read the default USB HID usage table. */ hid_init(NULL); - return (SDL_SYS_numjoysticks); + return (numjoysticks); } -int -SDL_SYS_NumJoysticks(void) +static int +BSD_JoystickGetCount(void) { - return SDL_SYS_numjoysticks; + return numjoysticks; } -void -SDL_SYS_JoystickDetect(void) +static void +BSD_JoystickDetect(void) { } -const char * -SDL_SYS_JoystickNameForDeviceIndex(int device_index) +static const char * +BSD_JoystickGetDeviceName(int device_index) { if (joydevnames[device_index] != NULL) { return (joydevnames[device_index]); @@ -224,8 +227,15 @@ SDL_SYS_JoystickNameForDeviceIndex(int device_index) return (joynames[device_index]); } +static int +BSD_JoystickGetDevicePlayerIndex(int device_index) +{ + return -1; +} + /* Function to perform the mapping from device index to the instance id for this index */ -SDL_JoystickID SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) +static SDL_JoystickID +BSD_JoystickGetDeviceInstanceID(int device_index) { return device_index; } @@ -281,8 +291,8 @@ hatval_to_sdl(Sint32 hatval) } -int -SDL_SYS_JoystickOpen(SDL_Joystick * joy, int device_index) +static int +BSD_JoystickOpen(SDL_Joystick * joy, int device_index) { char *path = joynames[device_index]; struct joystick_hwdata *hw; @@ -365,8 +375,8 @@ SDL_SYS_JoystickOpen(SDL_Joystick * joy, int device_index) str[i] = '\0'; asprintf(&new_name, "%s @ %s", str, path); if (new_name != NULL) { - SDL_free(joydevnames[SDL_SYS_numjoysticks]); - joydevnames[SDL_SYS_numjoysticks] = new_name; + SDL_free(joydevnames[numjoysticks]); + joydevnames[numjoysticks] = new_name; } } desc_failed: @@ -467,14 +477,8 @@ desc_failed: return (-1); } -/* Function to determine if this joystick is attached to the system right now */ -SDL_bool SDL_SYS_JoystickAttached(SDL_Joystick *joystick) -{ - return SDL_TRUE; -} - -void -SDL_SYS_JoystickUpdate(SDL_Joystick * joy) +static void +BSD_JoystickUpdate(SDL_Joystick * joy) { struct hid_item hitem; struct hid_data *hdata; @@ -586,8 +590,8 @@ SDL_SYS_JoystickUpdate(SDL_Joystick * joy) } /* Function to close a joystick after use */ -void -SDL_SYS_JoystickClose(SDL_Joystick * joy) +static void +BSD_JoystickClose(SDL_Joystick * joy) { if (SDL_strncmp(joy->hwdata->path, "/dev/joy", 8)) { report_free(&joy->hwdata->inreport); @@ -598,8 +602,8 @@ SDL_SYS_JoystickClose(SDL_Joystick * joy) SDL_free(joy->hwdata); } -void -SDL_SYS_JoystickQuit(void) +static void +BSD_JoystickQuit(void) { int i; @@ -611,21 +615,12 @@ SDL_SYS_JoystickQuit(void) return; } -SDL_JoystickGUID SDL_SYS_JoystickGetDeviceGUID( int device_index ) +static SDL_JoystickGUID +BSD_JoystickGetDeviceGUID( int device_index ) { SDL_JoystickGUID guid; /* the GUID is just the first 16 chars of the name for now */ - const char *name = SDL_SYS_JoystickNameForDeviceIndex( device_index ); - SDL_zero( guid ); - SDL_memcpy( &guid, name, SDL_min( sizeof(guid), SDL_strlen( name ) ) ); - return guid; -} - -SDL_JoystickGUID SDL_SYS_JoystickGetGUID(SDL_Joystick * joystick) -{ - SDL_JoystickGUID guid; - /* the GUID is just the first 16 chars of the name for now */ - const char *name = joystick->name; + const char *name = BSD_JoystickGetDeviceName( device_index ); SDL_zero( guid ); SDL_memcpy( &guid, name, SDL_min( sizeof(guid), SDL_strlen( name ) ) ); return guid; @@ -686,6 +681,28 @@ report_free(struct report *r) r->status = SREPORT_UNINIT; } +static int +BSD_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + return SDL_Unsupported(); +} + +SDL_JoystickDriver SDL_BSD_JoystickDriver = +{ + BSD_JoystickInit, + BSD_JoystickGetCount, + BSD_JoystickDetect, + BSD_JoystickGetDeviceName, + BSD_JoystickGetDevicePlayerIndex, + BSD_JoystickGetDeviceGUID, + BSD_JoystickGetDeviceInstanceID, + BSD_JoystickOpen, + BSD_JoystickRumble, + BSD_JoystickUpdate, + BSD_JoystickClose, + BSD_JoystickQuit, +}; + #endif /* SDL_JOYSTICK_USBHID */ /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/controller_type.h b/Engine/lib/sdl/src/joystick/controller_type.h new file mode 100644 index 000000000..51ac20b4e --- /dev/null +++ b/Engine/lib/sdl/src/joystick/controller_type.h @@ -0,0 +1,427 @@ +/* + Copyright (C) Valve Corporation + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 CONTROLLER_TYPE_H +#define CONTROLLER_TYPE_H +#ifdef _WIN32 +#pragma once +#endif + +#ifndef __cplusplus +#define inline SDL_INLINE +#endif + +//----------------------------------------------------------------------------- +// Purpose: Steam Controller models +// WARNING: DO NOT RENUMBER EXISTING VALUES - STORED IN A DATABASE +//----------------------------------------------------------------------------- +typedef enum +{ + k_eControllerType_None = -1, + k_eControllerType_Unknown = 0, + + // Steam Controllers + k_eControllerType_UnknownSteamController = 1, + k_eControllerType_SteamController = 2, + k_eControllerType_SteamControllerV2 = 3, + + // Other Controllers + k_eControllerType_UnknownNonSteamController = 30, + k_eControllerType_XBox360Controller = 31, + k_eControllerType_XBoxOneController = 32, + k_eControllerType_PS3Controller = 33, + k_eControllerType_PS4Controller = 34, + k_eControllerType_WiiController = 35, + k_eControllerType_AppleController = 36, + k_eControllerType_AndroidController = 37, + k_eControllerType_SwitchProController = 38, + k_eControllerType_SwitchJoyConLeft = 39, + k_eControllerType_SwitchJoyConRight = 40, + k_eControllerType_SwitchJoyConPair = 41, + k_eControllerType_SwitchInputOnlyController = 42, + k_eControllerType_MobileTouch = 43, + k_eControllerType_LastController, // Don't add game controllers below this enumeration - this enumeration can change value + + // Keyboards and Mice + k_eControllertype_GenericKeyboard = 400, + k_eControllertype_GenericMouse = 800, +} EControllerType; + +static inline SDL_bool BIsSteamController( EControllerType eType ) +{ + return ( eType == k_eControllerType_SteamController || eType == k_eControllerType_SteamControllerV2 ); +} + +#define MAKE_CONTROLLER_ID( nVID, nPID ) (unsigned int)( nVID << 16 | nPID ) +typedef struct +{ + unsigned int m_unDeviceID; + EControllerType m_eControllerType; +} ControllerDescription_t; + +static const ControllerDescription_t arrControllers[] = { + { MAKE_CONTROLLER_ID( 0x0079, 0x18d4 ), k_eControllerType_XBox360Controller }, // GPD Win 2 X-Box Controller + { MAKE_CONTROLLER_ID( 0x044f, 0xb326 ), k_eControllerType_XBox360Controller }, // Thrustmaster Gamepad GP XID + { MAKE_CONTROLLER_ID( 0x045e, 0x028e ), k_eControllerType_XBox360Controller }, // Microsoft X-Box 360 pad + { MAKE_CONTROLLER_ID( 0x045e, 0x028f ), k_eControllerType_XBox360Controller }, // Microsoft X-Box 360 pad v2 + { MAKE_CONTROLLER_ID( 0x045e, 0x0291 ), k_eControllerType_XBox360Controller }, // Xbox 360 Wireless Receiver (XBOX) + { MAKE_CONTROLLER_ID( 0x045e, 0x02a0 ), k_eControllerType_XBox360Controller }, // Microsoft X-Box 360 Big Button IR + { MAKE_CONTROLLER_ID( 0x045e, 0x02a1 ), k_eControllerType_XBox360Controller }, // Microsoft X-Box 360 pad + { MAKE_CONTROLLER_ID( 0x045e, 0x02d1 ), k_eControllerType_XBoxOneController }, // Microsoft X-Box One pad + { MAKE_CONTROLLER_ID( 0x045e, 0x02dd ), k_eControllerType_XBoxOneController }, // Microsoft X-Box One pad (Firmware 2015) + { MAKE_CONTROLLER_ID( 0x045e, 0x02e0 ), k_eControllerType_XBoxOneController }, // Microsoft X-Box One S pad (Bluetooth) + { MAKE_CONTROLLER_ID( 0x045e, 0x02e3 ), k_eControllerType_XBoxOneController }, // Microsoft X-Box One Elite pad + { MAKE_CONTROLLER_ID( 0x045e, 0x02ea ), k_eControllerType_XBoxOneController }, // Microsoft X-Box One S pad + { MAKE_CONTROLLER_ID( 0x045e, 0x02fd ), k_eControllerType_XBoxOneController }, // Microsoft X-Box One S pad (Bluetooth) + { MAKE_CONTROLLER_ID( 0x045e, 0x02ff ), k_eControllerType_XBoxOneController }, // Microsoft X-Box One Elite pad + { MAKE_CONTROLLER_ID( 0x045e, 0x0719 ), k_eControllerType_XBox360Controller }, // Xbox 360 Wireless Receiver + { MAKE_CONTROLLER_ID( 0x046d, 0xc21d ), k_eControllerType_XBox360Controller }, // Logitech Gamepad F310 + { MAKE_CONTROLLER_ID( 0x046d, 0xc21e ), k_eControllerType_XBox360Controller }, // Logitech Gamepad F510 + { MAKE_CONTROLLER_ID( 0x046d, 0xc21f ), k_eControllerType_XBox360Controller }, // Logitech Gamepad F710 + { MAKE_CONTROLLER_ID( 0x046d, 0xc242 ), k_eControllerType_XBox360Controller }, // Logitech Chillstream Controller + + { MAKE_CONTROLLER_ID( 0x054c, 0x0268 ), k_eControllerType_PS3Controller }, // Sony PS3 Controller + { MAKE_CONTROLLER_ID( 0x0925, 0x0005 ), k_eControllerType_PS3Controller }, // Sony PS3 Controller + { MAKE_CONTROLLER_ID( 0x8888, 0x0308 ), k_eControllerType_PS3Controller }, // Sony PS3 Controller + { MAKE_CONTROLLER_ID( 0x1a34, 0x0836 ), k_eControllerType_PS3Controller }, // Afterglow ps3 + { MAKE_CONTROLLER_ID( 0x0f0d, 0x006e ), k_eControllerType_PS3Controller }, // HORI horipad4 ps3 + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0066 ), k_eControllerType_PS3Controller }, // HORI horipad4 ps4 + { MAKE_CONTROLLER_ID( 0x0f0d, 0x005f ), k_eControllerType_PS3Controller }, // HORI Fighting commander ps3 + { MAKE_CONTROLLER_ID( 0x0f0d, 0x005e ), k_eControllerType_PS3Controller }, // HORI Fighting commander ps4 + //{ MAKE_CONTROLLER_ID( 0x0738, 0x3250 ), k_eControllerType_PS3Controller }, // madcats fightpad pro ps3 already in ps4 list.. does this work?? + { MAKE_CONTROLLER_ID( 0x0738, 0x8250 ), k_eControllerType_PS3Controller }, // madcats fightpad pro ps4 + { MAKE_CONTROLLER_ID( 0x0079, 0x181a ), k_eControllerType_PS3Controller }, // Venom Arcade Stick + { MAKE_CONTROLLER_ID( 0x0079, 0x0006 ), k_eControllerType_PS3Controller }, // PC Twin Shock Controller - looks like a DS3 but the face buttons are 1-4 instead of symbols + { MAKE_CONTROLLER_ID( 0x0079, 0x1844 ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x8888, 0x0308 ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x2563, 0x0575 ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x0810, 0x0001 ), k_eControllerType_PS3Controller }, // actually ps2 - maybe break out later + { MAKE_CONTROLLER_ID( 0x0810, 0x0003 ), k_eControllerType_PS3Controller }, // actually ps2 - maybe break out later + { MAKE_CONTROLLER_ID( 0x2563, 0x0523 ), k_eControllerType_PS3Controller }, // Digiflip GP006 + { MAKE_CONTROLLER_ID( 0x11ff, 0x3331 ), k_eControllerType_PS3Controller }, // SRXJ-PH2400 + { MAKE_CONTROLLER_ID( 0x20bc, 0x5500 ), k_eControllerType_PS3Controller }, // ShanWan PS3 + { MAKE_CONTROLLER_ID( 0x05b8, 0x1004 ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x146b, 0x0603 ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x044f, 0xb315 ), k_eControllerType_PS3Controller }, // Firestorm Dual Analog 3 + { MAKE_CONTROLLER_ID( 0x0925, 0x8888 ), k_eControllerType_PS3Controller }, // Actually ps2 -maybe break out later Lakeview Research WiseGroup Ltd, MP-8866 Dual Joypad + { MAKE_CONTROLLER_ID( 0x0f0d, 0x004d ), k_eControllerType_PS3Controller }, // Horipad 3 + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0009 ), k_eControllerType_PS3Controller }, // HORI BDA GP1 + { MAKE_CONTROLLER_ID( 0x0e8f, 0x0008 ), k_eControllerType_PS3Controller }, // Green Asia + { MAKE_CONTROLLER_ID( 0x0f0d, 0x006a ), k_eControllerType_PS3Controller }, // Real Arcade Pro 4 + { MAKE_CONTROLLER_ID( 0x0e6f, 0x011e ), k_eControllerType_PS3Controller }, // Rock Candy PS4 + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0214 ), k_eControllerType_PS3Controller }, // afterglow ps3 + { MAKE_CONTROLLER_ID( 0x0925, 0x8866 ), k_eControllerType_PS3Controller }, // PS2 maybe break out later + { MAKE_CONTROLLER_ID( 0x0e8f, 0x310d ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x2c22, 0x2003 ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x056e, 0x2013 ), k_eControllerType_PS3Controller }, // JC-U4113SBK + { MAKE_CONTROLLER_ID( 0x0738, 0x8838 ), k_eControllerType_PS3Controller }, // Madcatz Fightstick Pro + { MAKE_CONTROLLER_ID( 0x1a34, 0x0836 ), k_eControllerType_PS3Controller }, // Afterglow PS3 + { MAKE_CONTROLLER_ID( 0x0f30, 0x1100 ), k_eControllerType_PS3Controller }, // Quanba Q1 fight stick + { MAKE_CONTROLLER_ID( 0x1345, 0x6005 ), k_eControllerType_PS3Controller }, // ps2 maybe break out later + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0087 ), k_eControllerType_PS3Controller }, // HORI fighting mini stick + { MAKE_CONTROLLER_ID( 0x146b, 0x5500 ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x20d6, 0xca6d ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x25f0, 0xc121 ), k_eControllerType_PS3Controller }, // + { MAKE_CONTROLLER_ID( 0x8380, 0x0003 ), k_eControllerType_PS3Controller }, // BTP 2163 + { MAKE_CONTROLLER_ID( 0x1345, 0x1000 ), k_eControllerType_PS3Controller }, // PS2 ACME GA-D5 + { MAKE_CONTROLLER_ID( 0x0e8f, 0x3075 ), k_eControllerType_PS3Controller }, // SpeedLink Strike FX + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0128 ), k_eControllerType_PS3Controller }, // Rock Candy PS3 + { MAKE_CONTROLLER_ID( 0x2c22, 0x2000 ), k_eControllerType_PS3Controller }, // Quanba Drone + { MAKE_CONTROLLER_ID( 0x06a3, 0xf622 ), k_eControllerType_PS3Controller }, // Cyborg V3 + { MAKE_CONTROLLER_ID( 0x044f, 0xd007 ), k_eControllerType_PS3Controller }, // Thrustmaster wireless 3-1 + { MAKE_CONTROLLER_ID( 0x25f0, 0x83c3 ), k_eControllerType_PS3Controller }, // gioteck vx2 + { MAKE_CONTROLLER_ID( 0x05b8, 0x1006 ), k_eControllerType_PS3Controller }, // JC-U3412SBK + { MAKE_CONTROLLER_ID( 0x20d6, 0x576d ), k_eControllerType_PS3Controller }, // Power A PS3 + { MAKE_CONTROLLER_ID( 0x0e6f, 0x6302 ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x056e, 0x200f ), k_eControllerType_PS3Controller }, // From SDL + { MAKE_CONTROLLER_ID( 0x0e6f, 0x1314 ), k_eControllerType_PS3Controller }, // PDP Afterglow Wireless PS3 controller + + { MAKE_CONTROLLER_ID( 0x054c, 0x05c4 ), k_eControllerType_PS4Controller }, // Sony PS4 Controller + { MAKE_CONTROLLER_ID( 0x054c, 0x09cc ), k_eControllerType_PS4Controller }, // Sony PS4 Slim Controller + { MAKE_CONTROLLER_ID( 0x054c, 0x0ba0 ), k_eControllerType_PS4Controller }, // Sony PS4 Controller (Wireless dongle) + { MAKE_CONTROLLER_ID( 0x0f0d, 0x008a ), k_eControllerType_PS4Controller }, // HORI Real Arcade Pro 4 + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0055 ), k_eControllerType_PS4Controller }, // HORIPAD 4 FPS + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0066 ), k_eControllerType_PS4Controller }, // HORIPAD 4 FPS Plus + { MAKE_CONTROLLER_ID( 0x0738, 0x8384 ), k_eControllerType_PS4Controller }, // Mad Catz FightStick TE S+ PS4 + { MAKE_CONTROLLER_ID( 0x0738, 0x8250 ), k_eControllerType_PS4Controller }, // Mad Catz FightPad Pro PS4 + { MAKE_CONTROLLER_ID( 0x0C12, 0x0E10 ), k_eControllerType_PS4Controller }, // Armor Armor 3 Pad PS4 + { MAKE_CONTROLLER_ID( 0x0C12, 0x1CF6 ), k_eControllerType_PS4Controller }, // EMIO PS4 Elite Controller + { MAKE_CONTROLLER_ID( 0x1532, 0x1000 ), k_eControllerType_PS4Controller }, // Razer Raiju PS4 Controller + { MAKE_CONTROLLER_ID( 0x1532, 0X0401 ), k_eControllerType_PS4Controller }, // Razer Panthera PS4 Controller + { MAKE_CONTROLLER_ID( 0x054c, 0x05c5 ), k_eControllerType_PS4Controller }, // STRIKEPAD PS4 Grip Add-on + { MAKE_CONTROLLER_ID( 0x146b, 0x0d01 ), k_eControllerType_PS4Controller }, // Nacon Revolution Pro Controller - has gyro + { MAKE_CONTROLLER_ID( 0x146b, 0x0d02 ), k_eControllerType_PS4Controller }, // Nacon Revolution Pro Controller v2 - has gyro + { MAKE_CONTROLLER_ID( 0x0f0d, 0x00a0 ), k_eControllerType_PS4Controller }, // HORI TAC4 mousething + { MAKE_CONTROLLER_ID( 0x0f0d, 0x009c ), k_eControllerType_PS4Controller }, // HORI TAC PRO mousething + { MAKE_CONTROLLER_ID( 0x0c12, 0x0ef6 ), k_eControllerType_PS4Controller }, // Hitbox Arcade Stick + { MAKE_CONTROLLER_ID( 0x0079, 0x181b ), k_eControllerType_PS4Controller }, // Venom Arcade Stick - XXX:this may not work and may need to be called a ps3 controller + { MAKE_CONTROLLER_ID( 0x0738, 0x3250 ), k_eControllerType_PS4Controller }, // Mad Catz FightPad PRO - controller shaped with 6 face buttons + { MAKE_CONTROLLER_ID( 0x0f0d, 0x00ee ), k_eControllerType_PS4Controller }, // Hori mini wired https://www.playstation.com/en-us/explore/accessories/gaming-controllers/mini-wired-gamepad/ + { MAKE_CONTROLLER_ID( 0x0738, 0x8481 ), k_eControllerType_PS4Controller }, // Mad Catz FightStick TE 2+ PS4 + { MAKE_CONTROLLER_ID( 0x0738, 0x8480 ), k_eControllerType_PS4Controller }, // Mad Catz FightStick TE 2 PS4 + { MAKE_CONTROLLER_ID( 0x7545, 0x0104 ), k_eControllerType_PS4Controller }, // Armor 3 or Level Up Cobra - At least one variant has gyro + { MAKE_CONTROLLER_ID( 0x0c12, 0x0e15 ), k_eControllerType_PS4Controller }, // Game:Pad 4 + { MAKE_CONTROLLER_ID( 0x11c0, 0x4001 ), k_eControllerType_PS4Controller }, // "PS4 Fun Controller" added from user log + + { MAKE_CONTROLLER_ID( 0x1532, 0x1007 ), k_eControllerType_PS4Controller }, // Razer Raiju 2 Tournament edition USB- untested and added for razer + { MAKE_CONTROLLER_ID( 0x1532, 0x100A ), k_eControllerType_PS4Controller }, // Razer Raiju 2 Tournament edition BT - untested and added for razer + { MAKE_CONTROLLER_ID( 0x1532, 0x1004 ), k_eControllerType_PS4Controller }, // Razer Raiju 2 Ultimate USB - untested and added for razer + { MAKE_CONTROLLER_ID( 0x1532, 0x1009 ), k_eControllerType_PS4Controller }, // Razer Raiju 2 Ultimate BT - untested and added for razer + { MAKE_CONTROLLER_ID( 0x1532, 0x1008 ), k_eControllerType_PS4Controller }, // Razer Panthera Evo Fightstick - untested and added for razer + + { MAKE_CONTROLLER_ID( 0x056e, 0x2004 ), k_eControllerType_XBox360Controller }, // Elecom JC-U3613M + { MAKE_CONTROLLER_ID( 0x06a3, 0xf51a ), k_eControllerType_XBox360Controller }, // Saitek P3600 + { MAKE_CONTROLLER_ID( 0x0738, 0x4716 ), k_eControllerType_XBox360Controller }, // Mad Catz Wired Xbox 360 Controller + { MAKE_CONTROLLER_ID( 0x0738, 0x4718 ), k_eControllerType_XBox360Controller }, // Mad Catz Street Fighter IV FightStick SE + { MAKE_CONTROLLER_ID( 0x0738, 0x4726 ), k_eControllerType_XBox360Controller }, // Mad Catz Xbox 360 Controller + { MAKE_CONTROLLER_ID( 0x0738, 0x4728 ), k_eControllerType_XBox360Controller }, // Mad Catz Street Fighter IV FightPad + { MAKE_CONTROLLER_ID( 0x0738, 0x4736 ), k_eControllerType_XBox360Controller }, // Mad Catz MicroCon Gamepad + { MAKE_CONTROLLER_ID( 0x0738, 0x4738 ), k_eControllerType_XBox360Controller }, // Mad Catz Wired Xbox 360 Controller (SFIV) + { MAKE_CONTROLLER_ID( 0x0738, 0x4740 ), k_eControllerType_XBox360Controller }, // Mad Catz Beat Pad + { MAKE_CONTROLLER_ID( 0x0738, 0x4a01 ), k_eControllerType_XBoxOneController }, // Mad Catz FightStick TE 2 + { MAKE_CONTROLLER_ID( 0x0738, 0xb726 ), k_eControllerType_XBox360Controller }, // Mad Catz Xbox controller - MW2 + { MAKE_CONTROLLER_ID( 0x0738, 0xbeef ), k_eControllerType_XBox360Controller }, // Mad Catz JOYTECH NEO SE Advanced GamePad + { MAKE_CONTROLLER_ID( 0x0738, 0xcb02 ), k_eControllerType_XBox360Controller }, // Saitek Cyborg Rumble Pad - PC/Xbox 360 + { MAKE_CONTROLLER_ID( 0x0738, 0xcb03 ), k_eControllerType_XBox360Controller }, // Saitek P3200 Rumble Pad - PC/Xbox 360 + { MAKE_CONTROLLER_ID( 0x0738, 0xf738 ), k_eControllerType_XBox360Controller }, // Super SFIV FightStick TE S + { MAKE_CONTROLLER_ID( 0x0955, 0xb400 ), k_eControllerType_XBox360Controller }, // NVIDIA Shield streaming controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0105 ), k_eControllerType_XBox360Controller }, // HSM3 Xbox360 dancepad + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0113 ), k_eControllerType_XBox360Controller }, // Afterglow AX.1 Gamepad for Xbox 360 + { MAKE_CONTROLLER_ID( 0x0e6f, 0x011f ), k_eControllerType_XBox360Controller }, // Rock Candy Gamepad Wired Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0131 ), k_eControllerType_XBox360Controller }, // PDP EA Sports Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0133 ), k_eControllerType_XBox360Controller }, // Xbox 360 Wired Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0139 ), k_eControllerType_XBoxOneController }, // Afterglow Prismatic Wired Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x013a ), k_eControllerType_XBoxOneController }, // PDP Xbox One Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0146 ), k_eControllerType_XBoxOneController }, // Rock Candy Wired Controller for Xbox One + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0147 ), k_eControllerType_XBoxOneController }, // PDP Marvel Xbox One Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x015c ), k_eControllerType_XBoxOneController }, // PDP Xbox One Arcade Stick + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0161 ), k_eControllerType_XBoxOneController }, // PDP Xbox One Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0162 ), k_eControllerType_XBoxOneController }, // PDP Xbox One Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0163 ), k_eControllerType_XBoxOneController }, // PDP Xbox One Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0164 ), k_eControllerType_XBoxOneController }, // PDP Battlefield One + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0165 ), k_eControllerType_XBoxOneController }, // PDP Titanfall 2 + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0201 ), k_eControllerType_XBox360Controller }, // Pelican PL-3601 'TSZ' Wired Xbox 360 Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0213 ), k_eControllerType_XBox360Controller }, // Afterglow Gamepad for Xbox 360 + { MAKE_CONTROLLER_ID( 0x0e6f, 0x021f ), k_eControllerType_XBox360Controller }, // Rock Candy Gamepad for Xbox 360 + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0246 ), k_eControllerType_XBoxOneController }, // Rock Candy Gamepad for Xbox One 2015 + { MAKE_CONTROLLER_ID( 0x0e6f, 0x02a0 ), k_eControllerType_XBox360Controller }, // Counterfeit 360Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0301 ), k_eControllerType_XBox360Controller }, // Logic3 Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0346 ), k_eControllerType_XBoxOneController }, // Rock Candy Gamepad for Xbox One 2016 + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0401 ), k_eControllerType_XBox360Controller }, // Logic3 Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0413 ), k_eControllerType_XBox360Controller }, // Afterglow AX.1 Gamepad for Xbox 360 + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0501 ), k_eControllerType_XBox360Controller }, // PDP Xbox 360 Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0xf501 ), k_eControllerType_XBox360Controller }, // Counterfeit 360 Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0xf900 ), k_eControllerType_XBox360Controller }, // PDP Afterglow AX.1 + { MAKE_CONTROLLER_ID( 0x0f0d, 0x000a ), k_eControllerType_XBox360Controller }, // Hori Co. DOA4 FightStick + { MAKE_CONTROLLER_ID( 0x0f0d, 0x000c ), k_eControllerType_XBox360Controller }, // Hori PadEX Turbo + { MAKE_CONTROLLER_ID( 0x0f0d, 0x000d ), k_eControllerType_XBox360Controller }, // Hori Fighting Stick EX2 + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0016 ), k_eControllerType_XBox360Controller }, // Hori Real Arcade Pro.EX + { MAKE_CONTROLLER_ID( 0x0f0d, 0x001b ), k_eControllerType_XBox360Controller }, // Hori Real Arcade Pro VX + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0063 ), k_eControllerType_XBoxOneController }, // Hori Real Arcade Pro Hayabusa (USA) Xbox One + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0067 ), k_eControllerType_XBoxOneController }, // HORIPAD ONE + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0078 ), k_eControllerType_XBoxOneController }, // Hori Real Arcade Pro V Kai Xbox One + { MAKE_CONTROLLER_ID( 0x0f0d, 0x008c ), k_eControllerType_XBox360Controller }, // Hori Real Arcade Pro 4 + { MAKE_CONTROLLER_ID( 0x11c9, 0x55f0 ), k_eControllerType_XBox360Controller }, // Nacon GC-100XF + { MAKE_CONTROLLER_ID( 0x12ab, 0x0004 ), k_eControllerType_XBox360Controller }, // Honey Bee Xbox360 dancepad + { MAKE_CONTROLLER_ID( 0x12ab, 0x0301 ), k_eControllerType_XBox360Controller }, // PDP AFTERGLOW AX.1 + { MAKE_CONTROLLER_ID( 0x12ab, 0x0303 ), k_eControllerType_XBox360Controller }, // Mortal Kombat Klassic FightStick + { MAKE_CONTROLLER_ID( 0x1430, 0x02a0 ), k_eControllerType_XBox360Controller }, // RedOctane Controller Adapter + { MAKE_CONTROLLER_ID( 0x1430, 0x4748 ), k_eControllerType_XBox360Controller }, // RedOctane Guitar Hero X-plorer + { MAKE_CONTROLLER_ID( 0x1430, 0xf801 ), k_eControllerType_XBox360Controller }, // RedOctane Controller + { MAKE_CONTROLLER_ID( 0x146b, 0x0601 ), k_eControllerType_XBox360Controller }, // BigBen Interactive XBOX 360 Controller + { MAKE_CONTROLLER_ID( 0x1532, 0x0037 ), k_eControllerType_XBox360Controller }, // Razer Sabertooth + { MAKE_CONTROLLER_ID( 0x1532, 0x0a00 ), k_eControllerType_XBoxOneController }, // Razer Atrox Arcade Stick + { MAKE_CONTROLLER_ID( 0x1532, 0x0a03 ), k_eControllerType_XBoxOneController }, // Razer Wildcat + { MAKE_CONTROLLER_ID( 0x15e4, 0x3f00 ), k_eControllerType_XBox360Controller }, // Power A Mini Pro Elite + { MAKE_CONTROLLER_ID( 0x15e4, 0x3f0a ), k_eControllerType_XBox360Controller }, // Xbox Airflo wired controller + { MAKE_CONTROLLER_ID( 0x15e4, 0x3f10 ), k_eControllerType_XBox360Controller }, // Batarang Xbox 360 controller + { MAKE_CONTROLLER_ID( 0x162e, 0xbeef ), k_eControllerType_XBox360Controller }, // Joytech Neo-Se Take2 + { MAKE_CONTROLLER_ID( 0x1689, 0xfd00 ), k_eControllerType_XBox360Controller }, // Razer Onza Tournament Edition + { MAKE_CONTROLLER_ID( 0x1689, 0xfd01 ), k_eControllerType_XBox360Controller }, // Razer Onza Classic Edition + { MAKE_CONTROLLER_ID( 0x1689, 0xfe00 ), k_eControllerType_XBox360Controller }, // Razer Sabertooth + { MAKE_CONTROLLER_ID( 0x1bad, 0x0002 ), k_eControllerType_XBox360Controller }, // Harmonix Rock Band Guitar + { MAKE_CONTROLLER_ID( 0x1bad, 0x0003 ), k_eControllerType_XBox360Controller }, // Harmonix Rock Band Drumkit + { MAKE_CONTROLLER_ID( 0x1bad, 0xf016 ), k_eControllerType_XBox360Controller }, // Mad Catz Xbox 360 Controller + { MAKE_CONTROLLER_ID( 0x1bad, 0xf018 ), k_eControllerType_XBox360Controller }, // Mad Catz Street Fighter IV SE Fighting Stick + { MAKE_CONTROLLER_ID( 0x1bad, 0xf019 ), k_eControllerType_XBox360Controller }, // Mad Catz Brawlstick for Xbox 360 + { MAKE_CONTROLLER_ID( 0x1bad, 0xf021 ), k_eControllerType_XBox360Controller }, // Mad Cats Ghost Recon FS GamePad + { MAKE_CONTROLLER_ID( 0x1bad, 0xf023 ), k_eControllerType_XBox360Controller }, // MLG Pro Circuit Controller (Xbox) + { MAKE_CONTROLLER_ID( 0x1bad, 0xf025 ), k_eControllerType_XBox360Controller }, // Mad Catz Call Of Duty + { MAKE_CONTROLLER_ID( 0x1bad, 0xf027 ), k_eControllerType_XBox360Controller }, // Mad Catz FPS Pro + { MAKE_CONTROLLER_ID( 0x1bad, 0xf028 ), k_eControllerType_XBox360Controller }, // Street Fighter IV FightPad + { MAKE_CONTROLLER_ID( 0x1bad, 0xf02e ), k_eControllerType_XBox360Controller }, // Mad Catz Fightpad + { MAKE_CONTROLLER_ID( 0x1bad, 0xf036 ), k_eControllerType_XBox360Controller }, // Mad Catz MicroCon GamePad Pro + { MAKE_CONTROLLER_ID( 0x1bad, 0xf038 ), k_eControllerType_XBox360Controller }, // Street Fighter IV FightStick TE + { MAKE_CONTROLLER_ID( 0x1bad, 0xf039 ), k_eControllerType_XBox360Controller }, // Mad Catz MvC2 TE + { MAKE_CONTROLLER_ID( 0x1bad, 0xf03a ), k_eControllerType_XBox360Controller }, // Mad Catz SFxT Fightstick Pro + { MAKE_CONTROLLER_ID( 0x1bad, 0xf03d ), k_eControllerType_XBox360Controller }, // Street Fighter IV Arcade Stick TE - Chun Li + { MAKE_CONTROLLER_ID( 0x1bad, 0xf03e ), k_eControllerType_XBox360Controller }, // Mad Catz MLG FightStick TE + { MAKE_CONTROLLER_ID( 0x1bad, 0xf03f ), k_eControllerType_XBox360Controller }, // Mad Catz FightStick SoulCaliber + { MAKE_CONTROLLER_ID( 0x1bad, 0xf042 ), k_eControllerType_XBox360Controller }, // Mad Catz FightStick TES+ + { MAKE_CONTROLLER_ID( 0x1bad, 0xf080 ), k_eControllerType_XBox360Controller }, // Mad Catz FightStick TE2 + { MAKE_CONTROLLER_ID( 0x1bad, 0xf501 ), k_eControllerType_XBox360Controller }, // HoriPad EX2 Turbo + { MAKE_CONTROLLER_ID( 0x1bad, 0xf502 ), k_eControllerType_XBox360Controller }, // Hori Real Arcade Pro.VX SA + { MAKE_CONTROLLER_ID( 0x1bad, 0xf503 ), k_eControllerType_XBox360Controller }, // Hori Fighting Stick VX + { MAKE_CONTROLLER_ID( 0x1bad, 0xf504 ), k_eControllerType_XBox360Controller }, // Hori Real Arcade Pro. EX + { MAKE_CONTROLLER_ID( 0x1bad, 0xf505 ), k_eControllerType_XBox360Controller }, // Hori Fighting Stick EX2B + { MAKE_CONTROLLER_ID( 0x1bad, 0xf506 ), k_eControllerType_XBox360Controller }, // Hori Real Arcade Pro.EX Premium VLX + { MAKE_CONTROLLER_ID( 0x1bad, 0xf900 ), k_eControllerType_XBox360Controller }, // Harmonix Xbox 360 Controller + { MAKE_CONTROLLER_ID( 0x1bad, 0xf901 ), k_eControllerType_XBox360Controller }, // Gamestop Xbox 360 Controller + { MAKE_CONTROLLER_ID( 0x1bad, 0xf902 ), k_eControllerType_XBox360Controller }, // Mad Catz Gamepad2 + { MAKE_CONTROLLER_ID( 0x1bad, 0xf903 ), k_eControllerType_XBox360Controller }, // Tron Xbox 360 controller + { MAKE_CONTROLLER_ID( 0x1bad, 0xf904 ), k_eControllerType_XBox360Controller }, // PDP Versus Fighting Pad + { MAKE_CONTROLLER_ID( 0x1bad, 0xf906 ), k_eControllerType_XBox360Controller }, // MortalKombat FightStick + { MAKE_CONTROLLER_ID( 0x1bad, 0xfa01 ), k_eControllerType_XBox360Controller }, // MadCatz GamePad + { MAKE_CONTROLLER_ID( 0x1bad, 0xfd00 ), k_eControllerType_XBox360Controller }, // Razer Onza TE + { MAKE_CONTROLLER_ID( 0x1bad, 0xfd01 ), k_eControllerType_XBox360Controller }, // Razer Onza + { MAKE_CONTROLLER_ID( 0x24c6, 0x5000 ), k_eControllerType_XBox360Controller }, // Razer Atrox Arcade Stick + { MAKE_CONTROLLER_ID( 0x24c6, 0x5300 ), k_eControllerType_XBox360Controller }, // PowerA MINI PROEX Controller + { MAKE_CONTROLLER_ID( 0x24c6, 0x5303 ), k_eControllerType_XBox360Controller }, // Xbox Airflo wired controller + { MAKE_CONTROLLER_ID( 0x24c6, 0x530a ), k_eControllerType_XBox360Controller }, // Xbox 360 Pro EX Controller + { MAKE_CONTROLLER_ID( 0x24c6, 0x531a ), k_eControllerType_XBox360Controller }, // PowerA Pro Ex + { MAKE_CONTROLLER_ID( 0x24c6, 0x5397 ), k_eControllerType_XBox360Controller }, // FUS1ON Tournament Controller + { MAKE_CONTROLLER_ID( 0x24c6, 0x541a ), k_eControllerType_XBoxOneController }, // PowerA Xbox One Mini Wired Controller + { MAKE_CONTROLLER_ID( 0x24c6, 0x542a ), k_eControllerType_XBoxOneController }, // Xbox ONE spectra + { MAKE_CONTROLLER_ID( 0x24c6, 0x543a ), k_eControllerType_XBoxOneController }, // PowerA Xbox One wired controller + { MAKE_CONTROLLER_ID( 0x24c6, 0x5500 ), k_eControllerType_XBox360Controller }, // Hori XBOX 360 EX 2 with Turbo + { MAKE_CONTROLLER_ID( 0x24c6, 0x5501 ), k_eControllerType_XBox360Controller }, // Hori Real Arcade Pro VX-SA + { MAKE_CONTROLLER_ID( 0x24c6, 0x5502 ), k_eControllerType_XBox360Controller }, // Hori Fighting Stick VX Alt + { MAKE_CONTROLLER_ID( 0x24c6, 0x5503 ), k_eControllerType_XBox360Controller }, // Hori Fighting Edge + { MAKE_CONTROLLER_ID( 0x24c6, 0x5506 ), k_eControllerType_XBox360Controller }, // Hori SOULCALIBUR V Stick + { MAKE_CONTROLLER_ID( 0x24c6, 0x5510 ), k_eControllerType_XBox360Controller }, // Hori Fighting Commander ONE + { MAKE_CONTROLLER_ID( 0x24c6, 0x550d ), k_eControllerType_XBox360Controller }, // Hori GEM Xbox controller + { MAKE_CONTROLLER_ID( 0x24c6, 0x550e ), k_eControllerType_XBox360Controller }, // Hori Real Arcade Pro V Kai 360 + { MAKE_CONTROLLER_ID( 0x24c6, 0x551a ), k_eControllerType_XBoxOneController }, // PowerA FUSION Pro Controller + { MAKE_CONTROLLER_ID( 0x24c6, 0x561a ), k_eControllerType_XBoxOneController }, // PowerA FUSION Controller + { MAKE_CONTROLLER_ID( 0x24c6, 0x5b00 ), k_eControllerType_XBox360Controller }, // ThrustMaster Ferrari Italia 458 Racing Wheel + { MAKE_CONTROLLER_ID( 0x24c6, 0x5b02 ), k_eControllerType_XBox360Controller }, // Thrustmaster, Inc. GPX Controller + { MAKE_CONTROLLER_ID( 0x24c6, 0x5b03 ), k_eControllerType_XBox360Controller }, // Thrustmaster Ferrari 458 Racing Wheel + { MAKE_CONTROLLER_ID( 0x24c6, 0x5d04 ), k_eControllerType_XBox360Controller }, // Razer Sabertooth + { MAKE_CONTROLLER_ID( 0x24c6, 0xfafa ), k_eControllerType_XBox360Controller }, // Aplay Controller + { MAKE_CONTROLLER_ID( 0x24c6, 0xfafb ), k_eControllerType_XBox360Controller }, // Aplay Controller + { MAKE_CONTROLLER_ID( 0x24c6, 0xfafc ), k_eControllerType_XBox360Controller }, // Afterglow Gamepad 1 + { MAKE_CONTROLLER_ID( 0x24c6, 0xfafe ), k_eControllerType_XBox360Controller }, // Rock Candy Gamepad for Xbox 360 + { MAKE_CONTROLLER_ID( 0x24c6, 0xfafd ), k_eControllerType_XBox360Controller }, // Afterglow Gamepad 3 + + // These have been added via Minidump for unrecognized Xinput controller assert + { MAKE_CONTROLLER_ID( 0x0000, 0x0000 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x045e, 0x02a2 ), k_eControllerType_XBox360Controller }, // Unknown Controller - Microsoft VID + { MAKE_CONTROLLER_ID( 0x0e6f, 0x1414 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x1314 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0e6f, 0x0159 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x24c6, 0xfaff ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0086 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0f0d, 0x006d ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0f0d, 0x00a4 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0079, 0x1832 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0079, 0x187f ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0079, 0x1883 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x03eb, 0xff01 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x2c22, 0x2303 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0c12, 0x0ef8 ), k_eControllerType_XBox360Controller }, // Homemade fightstick based on brook pcb (with XInput driver??) + { MAKE_CONTROLLER_ID( 0x046d, 0x1000 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x1345, 0x6006 ), k_eControllerType_XBox360Controller }, // Unknown Controller + + { MAKE_CONTROLLER_ID( 0x056e, 0x2012 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x146b, 0x0602 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0f0d, 0x00ae ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x146b, 0x0603 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x056e, 0x2013 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x046d, 0x0401 ), k_eControllerType_XBox360Controller }, // logitech xinput + { MAKE_CONTROLLER_ID( 0x046d, 0x0301 ), k_eControllerType_XBox360Controller }, // logitech xinput + { MAKE_CONTROLLER_ID( 0x046d, 0xcaa3 ), k_eControllerType_XBox360Controller }, // logitech xinput + { MAKE_CONTROLLER_ID( 0x046d, 0xc261 ), k_eControllerType_XBox360Controller }, // logitech xinput + { MAKE_CONTROLLER_ID( 0x046d, 0x0291 ), k_eControllerType_XBox360Controller }, // logitech xinput + { MAKE_CONTROLLER_ID( 0x0079, 0x18d3 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0f0d, 0x00b1 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0001, 0x0001 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x1345, 0x6005 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0079, 0x188e ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0079, 0x18d4 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x2c22, 0x2003 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0f0d, 0x00b1 ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0079, 0x187c ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0079, 0x189c ), k_eControllerType_XBox360Controller }, // Unknown Controller + { MAKE_CONTROLLER_ID( 0x0079, 0x1874 ), k_eControllerType_XBox360Controller }, // Unknown Controller + + { MAKE_CONTROLLER_ID( 0x1038, 0xb360 ), k_eControllerType_XBox360Controller }, // SteelSeries Nimbus/Stratus XL + + + //{ MAKE_CONTROLLER_ID( 0x1949, 0x0402 ), /*android*/ }, // Unknown Controller + + { MAKE_CONTROLLER_ID( 0x05ac, 0x0001 ), k_eControllerType_AppleController }, // MFI Extended Gamepad (generic entry for iOS/tvOS) + { MAKE_CONTROLLER_ID( 0x05ac, 0x0002 ), k_eControllerType_AppleController }, // MFI Standard Gamepad (generic entry for iOS/tvOS) + + // We currently don't support using a pair of Switch Joy-Con's as a single + // controller and we don't want to support using them individually for the + // time being, so these should be disabled until one of the above is true + // { MAKE_CONTROLLER_ID( 0x057e, 0x2006 ), k_eControllerType_SwitchJoyConLeft }, // Nintendo Switch Joy-Con (Left) + // { MAKE_CONTROLLER_ID( 0x057e, 0x2007 ), k_eControllerType_SwitchJoyConRight }, // Nintendo Switch Joy-Con (Right) + + // This same controller ID is spoofed by many 3rd-party Switch controllers. + // The ones we currently know of are: + // * Any 8bitdo controller with Switch support + // * ORTZ Gaming Wireless Pro Controller + // * ZhiXu Gamepad Wireless + // * Sunwaytek Wireless Motion Controller for Nintendo Switch + { MAKE_CONTROLLER_ID( 0x057e, 0x2009 ), k_eControllerType_SwitchProController }, // Nintendo Switch Pro Controller + + { MAKE_CONTROLLER_ID( 0x0f0d, 0x00c1 ), k_eControllerType_SwitchInputOnlyController }, // HORIPAD for Nintendo Switch + { MAKE_CONTROLLER_ID( 0x20d6, 0xa711 ), k_eControllerType_SwitchInputOnlyController }, // PowerA Wired Controller Plus + { MAKE_CONTROLLER_ID( 0x0f0d, 0x0092 ), k_eControllerType_SwitchInputOnlyController }, // HORI Pokken Tournament DX Pro Pad + + + // Valve products - don't add to public list + { MAKE_CONTROLLER_ID( 0x0000, 0x11fb ), k_eControllerType_MobileTouch }, // Streaming mobile touch virtual controls + { MAKE_CONTROLLER_ID( 0x28de, 0x1101 ), k_eControllerType_SteamController }, // Valve Legacy Steam Controller (CHELL) + { MAKE_CONTROLLER_ID( 0x28de, 0x1102 ), k_eControllerType_SteamController }, // Valve wired Steam Controller (D0G) + { MAKE_CONTROLLER_ID( 0x28de, 0x1105 ), k_eControllerType_SteamControllerV2 }, // Valve Bluetooth Steam Controller (D0G) + { MAKE_CONTROLLER_ID( 0x28de, 0x1106 ), k_eControllerType_SteamControllerV2 }, // Valve Bluetooth Steam Controller (D0G) + { MAKE_CONTROLLER_ID( 0x28de, 0x1142 ), k_eControllerType_SteamController }, // Valve wireless Steam Controller + { MAKE_CONTROLLER_ID( 0x28de, 0x1201 ), k_eControllerType_SteamController }, // Valve wired Steam Controller (HEADCRAB) +}; + + +#if 0 /* these are currently unused, so #if 0'd out to prevent compiler warnings for now */ +static inline const ControllerDescription_t * GetControllerArray( int* nLength /* Out */) +{ + *nLength = sizeof( arrControllers ) / sizeof( arrControllers[0] ); + return arrControllers; +} +#endif + +static inline EControllerType GuessControllerType( int nVID, int nPID ) +{ + unsigned int unDeviceID = MAKE_CONTROLLER_ID( nVID, nPID ); + int iIndex; + for ( iIndex = 0; iIndex < sizeof( arrControllers ) / sizeof( arrControllers[0] ); ++iIndex ) + { + if ( unDeviceID == arrControllers[ iIndex ].m_unDeviceID ) + { + return arrControllers[ iIndex ].m_eControllerType; + } + } +#undef MAKE_CONTROLLER_ID + + return k_eControllerType_UnknownNonSteamController; +} + +#endif // CONSTANTS_H + diff --git a/Engine/lib/sdl/src/joystick/darwin/SDL_sysjoystick.c b/Engine/lib/sdl/src/joystick/darwin/SDL_sysjoystick.c index abfb1c62b..8af3b9610 100644 --- a/Engine/lib/sdl/src/joystick/darwin/SDL_sysjoystick.c +++ b/Engine/lib/sdl/src/joystick/darwin/SDL_sysjoystick.c @@ -22,29 +22,79 @@ #ifdef SDL_JOYSTICK_IOKIT -#include - -/* For force feedback testing. */ -#include -#include - +#include "SDL_events.h" #include "SDL_joystick.h" #include "../SDL_sysjoystick.h" #include "../SDL_joystick_c.h" #include "SDL_sysjoystick_c.h" -#include "SDL_events.h" +#include "../hidapi/SDL_hidapijoystick_c.h" #include "../../haptic/darwin/SDL_syshaptic_c.h" /* For haptic hot plugging */ + #define SDL_JOYSTICK_RUNLOOP_MODE CFSTR("SDLJoystick") +#define CONVERT_MAGNITUDE(x) (((x)*10000) / 0x7FFF) + /* The base object of the HID Manager API */ static IOHIDManagerRef hidman = NULL; /* Linked list of all available devices */ static recDevice *gpDeviceList = NULL; -/* static incrementing counter for new joystick devices seen on the system. Devices should start with index 0 */ -static int s_joystick_instance_id = -1; +void FreeRumbleEffectData(FFEFFECT *effect) +{ + if (!effect) { + return; + } + SDL_free(effect->rgdwAxes); + SDL_free(effect->rglDirection); + SDL_free(effect->lpvTypeSpecificParams); + SDL_free(effect); +} + +FFEFFECT *CreateRumbleEffectData(Sint16 magnitude, Uint32 duration_ms) +{ + FFEFFECT *effect; + FFPERIODIC *periodic; + + /* Create the effect */ + effect = (FFEFFECT *)SDL_calloc(1, sizeof(*effect)); + if (!effect) { + return NULL; + } + effect->dwSize = sizeof(*effect); + effect->dwGain = 10000; + effect->dwFlags = FFEFF_OBJECTOFFSETS; + effect->dwDuration = duration_ms * 1000; /* In microseconds. */ + effect->dwTriggerButton = FFEB_NOTRIGGER; + + effect->cAxes = 2; + effect->rgdwAxes = (DWORD *)SDL_calloc(effect->cAxes, sizeof(DWORD)); + if (!effect->rgdwAxes) { + FreeRumbleEffectData(effect); + return NULL; + } + + effect->rglDirection = (LONG *)SDL_calloc(effect->cAxes, sizeof(LONG)); + if (!effect->rglDirection) { + FreeRumbleEffectData(effect); + return NULL; + } + effect->dwFlags |= FFEFF_CARTESIAN; + + periodic = (FFPERIODIC *)SDL_calloc(1, sizeof(*periodic)); + if (!periodic) { + FreeRumbleEffectData(effect); + return NULL; + } + periodic->dwMagnitude = CONVERT_MAGNITUDE(magnitude); + periodic->dwPeriod = 1000000; + + effect->cbTypeSpecificParams = sizeof(*periodic); + effect->lpvTypeSpecificParams = periodic; + + return effect; +} static recDevice *GetDeviceForIndex(int device_index) { @@ -157,6 +207,19 @@ JoystickDeviceWasRemovedCallback(void *ctx, IOReturn result, void *sender) recDevice *device = (recDevice *) ctx; device->removed = SDL_TRUE; device->deviceRef = NULL; // deviceRef was invalidated due to the remove + if (device->ffeffect_ref) { + FFDeviceReleaseEffect(device->ffdevice, device->ffeffect_ref); + device->ffeffect_ref = NULL; + } + if (device->ffeffect) { + FreeRumbleEffectData(device->ffeffect); + device->ffeffect = NULL; + } + if (device->ffdevice) { + FFReleaseDevice(device->ffdevice); + device->ffdevice = NULL; + device->ff_initialized = SDL_FALSE; + } #if SDL_HAPTIC_IOKIT MacHaptic_MaybeRemoveDevice(device->ffservice); #endif @@ -333,8 +396,6 @@ AddHIDElement(const void *value, void *parameter) static SDL_bool GetDeviceInfo(IOHIDDeviceRef hidDevice, recDevice *pDevice) { - const Uint16 BUS_USB = 0x03; - const Uint16 BUS_BLUETOOTH = 0x05; Sint32 vendor = 0; Sint32 product = 0; Sint32 version = 0; @@ -389,10 +450,17 @@ GetDeviceInfo(IOHIDDeviceRef hidDevice, recDevice *pDevice) CFNumberGetValue(refCF, kCFNumberSInt32Type, &version); } +#ifdef SDL_JOYSTICK_HIDAPI + if (HIDAPI_IsDevicePresent(vendor, product, version)) { + /* The HIDAPI driver is taking care of this device */ + return 0; + } +#endif + SDL_memset(pDevice->guid.data, 0, sizeof(pDevice->guid.data)); if (vendor && product) { - *guid16++ = SDL_SwapLE16(BUS_USB); + *guid16++ = SDL_SwapLE16(SDL_HARDWARE_BUS_USB); *guid16++ = 0; *guid16++ = SDL_SwapLE16((Uint16)vendor); *guid16++ = 0; @@ -401,7 +469,7 @@ GetDeviceInfo(IOHIDDeviceRef hidDevice, recDevice *pDevice) *guid16++ = SDL_SwapLE16((Uint16)version); *guid16++ = 0; } else { - *guid16++ = SDL_SwapLE16(BUS_BLUETOOTH); + *guid16++ = SDL_SwapLE16(SDL_HARDWARE_BUS_BLUETOOTH); *guid16++ = 0; SDL_strlcpy((char*)guid16, pDevice->product, sizeof(pDevice->guid.data) - 4); } @@ -444,7 +512,6 @@ JoystickDeviceWasAddedCallback(void *ctx, IOReturn res, void *sender, IOHIDDevic } device = (recDevice *) SDL_calloc(1, sizeof(recDevice)); - if (!device) { SDL_OutOfMemory(); return; @@ -455,8 +522,7 @@ JoystickDeviceWasAddedCallback(void *ctx, IOReturn res, void *sender, IOHIDDevic return; /* not a device we care about, probably. */ } - if (SDL_IsGameControllerNameAndGUID(device->product, device->guid) && - SDL_ShouldIgnoreGameController(device->product, device->guid)) { + if (SDL_ShouldIgnoreJoystick(device->product, device->guid)) { SDL_free(device); return; } @@ -466,16 +532,16 @@ JoystickDeviceWasAddedCallback(void *ctx, IOReturn res, void *sender, IOHIDDevic IOHIDDeviceScheduleWithRunLoop(ioHIDDeviceObject, CFRunLoopGetCurrent(), SDL_JOYSTICK_RUNLOOP_MODE); /* Allocate an instance ID for this device */ - device->instance_id = ++s_joystick_instance_id; + device->instance_id = SDL_GetNextJoystickInstanceID(); /* We have to do some storage of the io_service_t for SDL_HapticOpenFromJoystick */ ioservice = IOHIDDeviceGetService(ioHIDDeviceObject); -#if SDL_HAPTIC_IOKIT if ((ioservice) && (FFIsForceFeedback(ioservice) == FF_OK)) { device->ffservice = ioservice; +#if SDL_HAPTIC_IOKIT MacHaptic_MaybeAddDevice(ioservice); - } #endif + } /* Add device to the end of the list */ if ( !gpDeviceList ) { @@ -492,7 +558,7 @@ JoystickDeviceWasAddedCallback(void *ctx, IOReturn res, void *sender, IOHIDDevic ++device_index; /* bump by one since we counted by pNext. */ } - SDL_PrivateJoystickAdded(device_index); + SDL_PrivateJoystickAdded(device->instance_id); } static SDL_bool @@ -577,13 +643,8 @@ CreateHIDManager(void) } -/* Function to scan the system for joysticks. - * Joystick 0 should be the system default joystick. - * This function should return the number of available joysticks, or -1 - * on an unrecoverable fatal error. - */ -int -SDL_SYS_JoystickInit(void) +static int +DARWIN_JoystickInit(void) { if (gpDeviceList) { return SDL_SetError("Joystick: Device list already inited."); @@ -593,12 +654,11 @@ SDL_SYS_JoystickInit(void) return SDL_SetError("Joystick: Couldn't initialize HID Manager"); } - return SDL_SYS_NumJoysticks(); + return 0; } -/* Function to return the number of joystick devices plugged in right now */ -int -SDL_SYS_NumJoysticks(void) +static int +DARWIN_JoystickGetCount(void) { recDevice *device = gpDeviceList; int nJoySticks = 0; @@ -613,10 +673,8 @@ SDL_SYS_NumJoysticks(void) return nJoySticks; } -/* Function to cause any queued joystick insertions to be processed - */ -void -SDL_SYS_JoystickDetect(void) +static void +DARWIN_JoystickDetect(void) { recDevice *device = gpDeviceList; while (device) { @@ -627,37 +685,49 @@ SDL_SYS_JoystickDetect(void) } } - /* run this after the checks above so we don't set device->removed and delete the device before - SDL_SYS_JoystickUpdate can run to clean up the SDL_Joystick object that owns this device */ - while (CFRunLoopRunInMode(SDL_JOYSTICK_RUNLOOP_MODE,0,TRUE) == kCFRunLoopRunHandledSource) { - /* no-op. Pending callbacks will fire in CFRunLoopRunInMode(). */ - } + /* run this after the checks above so we don't set device->removed and delete the device before + DARWIN_JoystickUpdate can run to clean up the SDL_Joystick object that owns this device */ + while (CFRunLoopRunInMode(SDL_JOYSTICK_RUNLOOP_MODE,0,TRUE) == kCFRunLoopRunHandledSource) { + /* no-op. Pending callbacks will fire in CFRunLoopRunInMode(). */ + } } /* Function to get the device-dependent name of a joystick */ const char * -SDL_SYS_JoystickNameForDeviceIndex(int device_index) +DARWIN_JoystickGetDeviceName(int device_index) { recDevice *device = GetDeviceForIndex(device_index); return device ? device->product : "UNKNOWN"; } -/* Function to return the instance id of the joystick at device_index - */ -SDL_JoystickID -SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) +static int +DARWIN_JoystickGetDevicePlayerIndex(int device_index) +{ + return -1; +} + +static SDL_JoystickGUID +DARWIN_JoystickGetDeviceGUID( int device_index ) +{ + recDevice *device = GetDeviceForIndex(device_index); + SDL_JoystickGUID guid; + if (device) { + guid = device->guid; + } else { + SDL_zero(guid); + } + return guid; +} + +static SDL_JoystickID +DARWIN_JoystickGetDeviceInstanceID(int device_index) { recDevice *device = GetDeviceForIndex(device_index); return device ? device->instance_id : 0; } -/* Function to open a joystick for use. - * The joystick to open is specified by the device index. - * This should fill the nbuttons and naxes fields of the joystick structure. - * It returns 0, or -1 if there is an error. - */ -int -SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) +static int +DARWIN_JoystickOpen(SDL_Joystick * joystick, int device_index) { recDevice *device = GetDeviceForIndex(device_index); @@ -672,22 +742,138 @@ SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) return 0; } -/* Function to query if the joystick is currently attached - * It returns SDL_TRUE if attached, SDL_FALSE otherwise. +/* + * Like strerror but for force feedback errors. */ -SDL_bool -SDL_SYS_JoystickAttached(SDL_Joystick * joystick) +static const char * +FFStrError(unsigned int err) { - return joystick->hwdata != NULL; + switch (err) { + case FFERR_DEVICEFULL: + return "device full"; + /* This should be valid, but for some reason isn't defined... */ + /* case FFERR_DEVICENOTREG: + return "device not registered"; */ + case FFERR_DEVICEPAUSED: + return "device paused"; + case FFERR_DEVICERELEASED: + return "device released"; + case FFERR_EFFECTPLAYING: + return "effect playing"; + case FFERR_EFFECTTYPEMISMATCH: + return "effect type mismatch"; + case FFERR_EFFECTTYPENOTSUPPORTED: + return "effect type not supported"; + case FFERR_GENERIC: + return "undetermined error"; + case FFERR_HASEFFECTS: + return "device has effects"; + case FFERR_INCOMPLETEEFFECT: + return "incomplete effect"; + case FFERR_INTERNAL: + return "internal fault"; + case FFERR_INVALIDDOWNLOADID: + return "invalid download id"; + case FFERR_INVALIDPARAM: + return "invalid parameter"; + case FFERR_MOREDATA: + return "more data"; + case FFERR_NOINTERFACE: + return "interface not supported"; + case FFERR_NOTDOWNLOADED: + return "effect is not downloaded"; + case FFERR_NOTINITIALIZED: + return "object has not been initialized"; + case FFERR_OUTOFMEMORY: + return "out of memory"; + case FFERR_UNPLUGGED: + return "device is unplugged"; + case FFERR_UNSUPPORTED: + return "function call unsupported"; + case FFERR_UNSUPPORTEDAXIS: + return "axis unsupported"; + + default: + return "unknown error"; + } } -/* Function to update the state of a joystick - called as a device poll. - * This function shouldn't update the joystick structure directly, - * but instead should call SDL_PrivateJoystick*() to deliver events - * and update joystick device state. - */ -void -SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) +static int +DARWIN_JoystickInitRumble(recDevice *device, Sint16 magnitude, Uint32 duration_ms) +{ + HRESULT result; + + if (!device->ffdevice) { + result = FFCreateDevice(device->ffservice, &device->ffdevice); + if (result != FF_OK) { + return SDL_SetError("Unable to create force feedback device from service: %s", FFStrError(result)); + } + } + + /* Reset and then enable actuators */ + result = FFDeviceSendForceFeedbackCommand(device->ffdevice, FFSFFC_RESET); + if (result != FF_OK) { + return SDL_SetError("Unable to reset force feedback device: %s", FFStrError(result)); + } + + result = FFDeviceSendForceFeedbackCommand(device->ffdevice, FFSFFC_SETACTUATORSON); + if (result != FF_OK) { + return SDL_SetError("Unable to enable force feedback actuators: %s", FFStrError(result)); + } + + /* Create the effect */ + device->ffeffect = CreateRumbleEffectData(magnitude, duration_ms); + if (!device->ffeffect) { + return SDL_OutOfMemory(); + } + + result = FFDeviceCreateEffect(device->ffdevice, kFFEffectType_Sine_ID, + device->ffeffect, &device->ffeffect_ref); + if (result != FF_OK) { + return SDL_SetError("Haptic: Unable to create effect: %s", FFStrError(result)); + } + return 0; +} + +static int +DARWIN_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + HRESULT result; + recDevice *device = joystick->hwdata; + + /* Scale and average the two rumble strengths */ + Sint16 magnitude = (Sint16)(((low_frequency_rumble / 2) + (high_frequency_rumble / 2)) / 2); + + if (!device->ffservice) { + return SDL_Unsupported(); + } + + if (device->ff_initialized) { + FFPERIODIC *periodic = ((FFPERIODIC *)device->ffeffect->lpvTypeSpecificParams); + device->ffeffect->dwDuration = duration_ms * 1000; /* In microseconds. */ + periodic->dwMagnitude = CONVERT_MAGNITUDE(magnitude); + + result = FFEffectSetParameters(device->ffeffect_ref, device->ffeffect, + (FFEP_DURATION | FFEP_TYPESPECIFICPARAMS)); + if (result != FF_OK) { + return SDL_SetError("Unable to update rumble effect: %s", FFStrError(result)); + } + } else { + if (DARWIN_JoystickInitRumble(device, magnitude, duration_ms) < 0) { + return -1; + } + device->ff_initialized = SDL_TRUE; + } + + result = FFEffectStart(device->ffeffect_ref, 1, 0); + if (result != FF_OK) { + return SDL_SetError("Unable to run the rumble effect: %s", FFStrError(result)); + } + return 0; +} + +static void +DARWIN_JoystickUpdate(SDL_Joystick * joystick) { recDevice *device = joystick->hwdata; recElement *element; @@ -792,15 +978,13 @@ SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) } } -/* Function to close a joystick after use */ -void -SDL_SYS_JoystickClose(SDL_Joystick * joystick) +static void +DARWIN_JoystickClose(SDL_Joystick * joystick) { } -/* Function to perform any system-specific joystick related cleanup */ -void -SDL_SYS_JoystickQuit(void) +static void +DARWIN_JoystickQuit(void) { while (FreeDevice(gpDeviceList)) { /* spin */ @@ -814,23 +998,21 @@ SDL_SYS_JoystickQuit(void) } } - -SDL_JoystickGUID SDL_SYS_JoystickGetDeviceGUID( int device_index ) +SDL_JoystickDriver SDL_DARWIN_JoystickDriver = { - recDevice *device = GetDeviceForIndex(device_index); - SDL_JoystickGUID guid; - if (device) { - guid = device->guid; - } else { - SDL_zero(guid); - } - return guid; -} - -SDL_JoystickGUID SDL_SYS_JoystickGetGUID(SDL_Joystick *joystick) -{ - return joystick->hwdata->guid; -} + DARWIN_JoystickInit, + DARWIN_JoystickGetCount, + DARWIN_JoystickDetect, + DARWIN_JoystickGetDeviceName, + DARWIN_JoystickGetDevicePlayerIndex, + DARWIN_JoystickGetDeviceGUID, + DARWIN_JoystickGetDeviceInstanceID, + DARWIN_JoystickOpen, + DARWIN_JoystickRumble, + DARWIN_JoystickUpdate, + DARWIN_JoystickClose, + DARWIN_JoystickQuit, +}; #endif /* SDL_JOYSTICK_IOKIT */ diff --git a/Engine/lib/sdl/src/joystick/darwin/SDL_sysjoystick_c.h b/Engine/lib/sdl/src/joystick/darwin/SDL_sysjoystick_c.h index cde6a5c21..2168f912e 100644 --- a/Engine/lib/sdl/src/joystick/darwin/SDL_sysjoystick_c.h +++ b/Engine/lib/sdl/src/joystick/darwin/SDL_sysjoystick_c.h @@ -24,6 +24,8 @@ #define SDL_JOYSTICK_IOKIT_H #include +#include +#include struct recElement { @@ -45,6 +47,10 @@ struct joystick_hwdata { IOHIDDeviceRef deviceRef; /* HIDManager device handle */ io_service_t ffservice; /* Interface for force feedback, 0 = no ff */ + FFDeviceObjectReference ffdevice; + FFEFFECT *ffeffect; + FFEffectObjectReference ffeffect_ref; + SDL_bool ff_initialized; char product[256]; /* name of product */ uint32_t usage; /* usage page from IOUSBHID Parser.h which defines general usage */ diff --git a/Engine/lib/sdl/src/joystick/dummy/SDL_sysjoystick.c b/Engine/lib/sdl/src/joystick/dummy/SDL_sysjoystick.c index 3dd96a001..ce0965daf 100644 --- a/Engine/lib/sdl/src/joystick/dummy/SDL_sysjoystick.c +++ b/Engine/lib/sdl/src/joystick/dummy/SDL_sysjoystick.c @@ -28,99 +28,92 @@ #include "../SDL_sysjoystick.h" #include "../SDL_joystick_c.h" -/* Function to scan the system for joysticks. - * It should return 0, or -1 on an unrecoverable fatal error. - */ -int -SDL_SYS_JoystickInit(void) + +static int +DUMMY_JoystickInit(void) { return 0; } -int -SDL_SYS_NumJoysticks(void) +static int +DUMMY_JoystickGetCount(void) { return 0; } -void -SDL_SYS_JoystickDetect(void) +static void +DUMMY_JoystickDetect(void) { } -/* Function to get the device-dependent name of a joystick */ -const char * -SDL_SYS_JoystickNameForDeviceIndex(int device_index) +static const char * +DUMMY_JoystickGetDeviceName(int device_index) { - SDL_SetError("Logic error: No joysticks available"); - return (NULL); + return NULL; } -/* Function to perform the mapping from device index to the instance id for this index */ -SDL_JoystickID SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) +static int +DUMMY_JoystickGetDevicePlayerIndex(int device_index) { - return device_index; + return -1; } -/* Function to open a joystick for use. - The joystick to open is specified by the device index. - This should fill the nbuttons and naxes fields of the joystick structure. - It returns 0, or -1 if there is an error. - */ -int -SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) +static SDL_JoystickGUID +DUMMY_JoystickGetDeviceGUID(int device_index) +{ + SDL_JoystickGUID guid; + SDL_zero(guid); + return guid; +} + +static SDL_JoystickID +DUMMY_JoystickGetDeviceInstanceID(int device_index) +{ + return -1; +} + +static int +DUMMY_JoystickOpen(SDL_Joystick * joystick, int device_index) { return SDL_SetError("Logic error: No joysticks available"); } -/* Function to determine if this joystick is attached to the system right now */ -SDL_bool SDL_SYS_JoystickAttached(SDL_Joystick *joystick) +static int +DUMMY_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) { - return SDL_TRUE; + return SDL_Unsupported(); } -/* Function to update the state of a joystick - called as a device poll. - * This function shouldn't update the joystick structure directly, - * but instead should call SDL_PrivateJoystick*() to deliver events - * and update joystick device state. - */ -void -SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) +static void +DUMMY_JoystickUpdate(SDL_Joystick * joystick) { } -/* Function to close a joystick after use */ -void -SDL_SYS_JoystickClose(SDL_Joystick * joystick) +static void +DUMMY_JoystickClose(SDL_Joystick * joystick) { } -/* Function to perform any system-specific joystick related cleanup */ -void -SDL_SYS_JoystickQuit(void) +static void +DUMMY_JoystickQuit(void) { } -SDL_JoystickGUID SDL_SYS_JoystickGetDeviceGUID( int device_index ) +SDL_JoystickDriver SDL_DUMMY_JoystickDriver = { - SDL_JoystickGUID guid; - /* the GUID is just the first 16 chars of the name for now */ - const char *name = SDL_SYS_JoystickNameForDeviceIndex( device_index ); - SDL_zero( guid ); - SDL_memcpy( &guid, name, SDL_min( sizeof(guid), SDL_strlen( name ) ) ); - return guid; -} - - -SDL_JoystickGUID SDL_SYS_JoystickGetGUID(SDL_Joystick * joystick) -{ - SDL_JoystickGUID guid; - /* the GUID is just the first 16 chars of the name for now */ - const char *name = joystick->name; - SDL_zero( guid ); - SDL_memcpy( &guid, name, SDL_min( sizeof(guid), SDL_strlen( name ) ) ); - return guid; -} + DUMMY_JoystickInit, + DUMMY_JoystickGetCount, + DUMMY_JoystickDetect, + DUMMY_JoystickGetDeviceName, + DUMMY_JoystickGetDevicePlayerIndex, + DUMMY_JoystickGetDeviceGUID, + DUMMY_JoystickGetDeviceInstanceID, + DUMMY_JoystickOpen, + DUMMY_JoystickRumble, + DUMMY_JoystickUpdate, + DUMMY_JoystickClose, + DUMMY_JoystickQuit, +}; #endif /* SDL_JOYSTICK_DUMMY || SDL_JOYSTICK_DISABLED */ diff --git a/Engine/lib/sdl/src/joystick/emscripten/SDL_sysjoystick.c b/Engine/lib/sdl/src/joystick/emscripten/SDL_sysjoystick.c index b5bcaad6c..d551c8aa1 100644 --- a/Engine/lib/sdl/src/joystick/emscripten/SDL_sysjoystick.c +++ b/Engine/lib/sdl/src/joystick/emscripten/SDL_sysjoystick.c @@ -156,11 +156,34 @@ Emscripten_JoyStickDisconnected(int eventType, const EmscriptenGamepadEvent *gam return 1; } +/* Function to perform any system-specific joystick related cleanup */ +static void +EMSCRIPTEN_JoystickQuit(void) +{ + SDL_joylist_item *item = NULL; + SDL_joylist_item *next = NULL; + + for (item = SDL_joylist; item; item = next) { + next = item->next; + SDL_free(item->mapping); + SDL_free(item->name); + SDL_free(item); + } + + SDL_joylist = SDL_joylist_tail = NULL; + + numjoysticks = 0; + instance_counter = 0; + + emscripten_set_gamepadconnected_callback(NULL, 0, NULL); + emscripten_set_gamepaddisconnected_callback(NULL, 0, NULL); +} + /* Function to scan the system for joysticks. * It should return 0, or -1 on an unrecoverable fatal error. */ -int -SDL_SYS_JoystickInit(void) +static int +EMSCRIPTEN_JoystickInit(void) { int retval, i, numjs; EmscriptenGamepadEvent gamepadState; @@ -190,7 +213,7 @@ SDL_SYS_JoystickInit(void) Emscripten_JoyStickConnected); if(retval != EMSCRIPTEN_RESULT_SUCCESS) { - SDL_SYS_JoystickQuit(); + EMSCRIPTEN_JoystickQuit(); return SDL_SetError("Could not set gamepad connect callback"); } @@ -198,7 +221,7 @@ SDL_SYS_JoystickInit(void) 0, Emscripten_JoyStickDisconnected); if(retval != EMSCRIPTEN_RESULT_SUCCESS) { - SDL_SYS_JoystickQuit(); + EMSCRIPTEN_JoystickQuit(); return SDL_SetError("Could not set gamepad disconnect callback"); } @@ -239,26 +262,31 @@ JoystickByIndex(int index) return item; } -int -SDL_SYS_NumJoysticks(void) +static int +EMSCRIPTEN_JoystickGetCount(void) { return numjoysticks; } -void -SDL_SYS_JoystickDetect(void) +static void +EMSCRIPTEN_JoystickDetect(void) { } -/* Function to get the device-dependent name of a joystick */ -const char * -SDL_SYS_JoystickNameForDeviceIndex(int device_index) +static const char * +EMSCRIPTEN_JoystickGetDeviceName(int device_index) { return JoystickByDeviceIndex(device_index)->name; } -/* Function to perform the mapping from device index to the instance id for this index */ -SDL_JoystickID SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) +static int +EMSCRIPTEN_JoystickGetDevicePlayerIndex(int device_index) +{ + return -1; +} + +static SDL_JoystickID +EMSCRIPTEN_JoystickGetDeviceInstanceID(int device_index) { return JoystickByDeviceIndex(device_index)->device_instance; } @@ -268,8 +296,8 @@ SDL_JoystickID SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) This should fill the nbuttons and naxes fields of the joystick structure. It returns 0, or -1 if there is an error. */ -int -SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) +static int +EMSCRIPTEN_JoystickOpen(SDL_Joystick * joystick, int device_index) { SDL_joylist_item *item = JoystickByDeviceIndex(device_index); @@ -295,19 +323,13 @@ SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) return (0); } -/* Function to determine if this joystick is attached to the system right now */ -SDL_bool SDL_SYS_JoystickAttached(SDL_Joystick *joystick) -{ - return joystick->hwdata != NULL; -} - /* Function to update the state of a joystick - called as a device poll. * This function shouldn't update the joystick structure directly, * but instead should call SDL_PrivateJoystick*() to deliver events * and update joystick device state. */ -void -SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) +static void +EMSCRIPTEN_JoystickUpdate(SDL_Joystick * joystick) { EmscriptenGamepadEvent gamepadState; SDL_joylist_item *item = (SDL_joylist_item *) joystick->hwdata; @@ -346,8 +368,8 @@ SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) } /* Function to close a joystick after use */ -void -SDL_SYS_JoystickClose(SDL_Joystick * joystick) +static void +EMSCRIPTEN_JoystickClose(SDL_Joystick * joystick) { SDL_joylist_item *item = (SDL_joylist_item *) joystick->hwdata; if (item) { @@ -355,49 +377,39 @@ SDL_SYS_JoystickClose(SDL_Joystick * joystick) } } -/* Function to perform any system-specific joystick related cleanup */ -void -SDL_SYS_JoystickQuit(void) -{ - SDL_joylist_item *item = NULL; - SDL_joylist_item *next = NULL; - - for (item = SDL_joylist; item; item = next) { - next = item->next; - SDL_free(item->mapping); - SDL_free(item->name); - SDL_free(item); - } - - SDL_joylist = SDL_joylist_tail = NULL; - - numjoysticks = 0; - instance_counter = 0; - - emscripten_set_gamepadconnected_callback(NULL, 0, NULL); - emscripten_set_gamepaddisconnected_callback(NULL, 0, NULL); -} - -SDL_JoystickGUID -SDL_SYS_JoystickGetDeviceGUID(int device_index) +static SDL_JoystickGUID +EMSCRIPTEN_JoystickGetDeviceGUID(int device_index) { SDL_JoystickGUID guid; /* the GUID is just the first 16 chars of the name for now */ - const char *name = SDL_SYS_JoystickNameForDeviceIndex(device_index); + const char *name = EMSCRIPTEN_JoystickGetDeviceName(device_index); SDL_zero(guid); SDL_memcpy(&guid, name, SDL_min(sizeof(guid), SDL_strlen(name))); return guid; } -SDL_JoystickGUID -SDL_SYS_JoystickGetGUID(SDL_Joystick * joystick) +static int +EMSCRIPTEN_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) { - SDL_JoystickGUID guid; - /* the GUID is just the first 16 chars of the name for now */ - const char *name = joystick->name; - SDL_zero(guid); - SDL_memcpy(&guid, name, SDL_min(sizeof(guid), SDL_strlen(name))); - return guid; + return SDL_Unsupported(); } +SDL_JoystickDriver SDL_EMSCRIPTEN_JoystickDriver = +{ + EMSCRIPTEN_JoystickInit, + EMSCRIPTEN_JoystickGetCount, + EMSCRIPTEN_JoystickDetect, + EMSCRIPTEN_JoystickGetDeviceName, + EMSCRIPTEN_JoystickGetDevicePlayerIndex, + EMSCRIPTEN_JoystickGetDeviceGUID, + EMSCRIPTEN_JoystickGetDeviceInstanceID, + EMSCRIPTEN_JoystickOpen, + EMSCRIPTEN_JoystickRumble, + EMSCRIPTEN_JoystickUpdate, + EMSCRIPTEN_JoystickClose, + EMSCRIPTEN_JoystickQuit, +}; + #endif /* SDL_JOYSTICK_EMSCRIPTEN */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/haiku/SDL_haikujoystick.cc b/Engine/lib/sdl/src/joystick/haiku/SDL_haikujoystick.cc index 9ab2c72fd..9fa8ca992 100644 --- a/Engine/lib/sdl/src/joystick/haiku/SDL_haikujoystick.cc +++ b/Engine/lib/sdl/src/joystick/haiku/SDL_haikujoystick.cc @@ -36,7 +36,7 @@ extern "C" /* The maximum number of joysticks we'll detect */ -#define MAX_JOYSTICKS 16 +#define MAX_JOYSTICKS 16 /* A list of available joysticks */ static char *SDL_joyport[MAX_JOYSTICKS]; @@ -50,13 +50,13 @@ extern "C" int16 *new_axes; }; - static int SDL_SYS_numjoysticks = 0; + static int numjoysticks = 0; /* Function to scan the system for joysticks. * Joystick 0 should be the system default joystick. * It should return 0, or -1 on an unrecoverable fatal error. */ - int SDL_SYS_JoystickInit(void) + static int HAIKU_JoystickInit(void) { BJoystick joystick; int i; @@ -65,52 +65,59 @@ extern "C" /* Search for attached joysticks */ nports = joystick.CountDevices(); - SDL_SYS_numjoysticks = 0; + numjoysticks = 0; SDL_memset(SDL_joyport, 0, (sizeof SDL_joyport)); SDL_memset(SDL_joyname, 0, (sizeof SDL_joyname)); - for (i = 0; (SDL_SYS_numjoysticks < MAX_JOYSTICKS) && (i < nports); ++i) + for (i = 0; (numjoysticks < MAX_JOYSTICKS) && (i < nports); ++i) { if (joystick.GetDeviceName(i, name) == B_OK) { if (joystick.Open(name) != B_ERROR) { BString stick_name; joystick.GetControllerName(&stick_name); - SDL_joyport[SDL_SYS_numjoysticks] = SDL_strdup(name); - SDL_joyname[SDL_SYS_numjoysticks] = SDL_strdup(stick_name.String()); - SDL_SYS_numjoysticks++; + SDL_joyport[numjoysticks] = SDL_strdup(name); + SDL_joyname[numjoysticks] = SDL_strdup(stick_name.String()); + numjoysticks++; joystick.Close(); } } } - return (SDL_SYS_numjoysticks); + return (numjoysticks); } - int SDL_SYS_NumJoysticks(void) + static int HAIKU_JoystickGetCount(void) { - return SDL_SYS_numjoysticks; + return numjoysticks; } - void SDL_SYS_JoystickDetect(void) + static void HAIKU_JoystickDetect(void) { } /* Function to get the device-dependent name of a joystick */ - const char *SDL_SYS_JoystickNameForDeviceIndex(int device_index) + static const char *HAIKU_JoystickGetDeviceName(int device_index) { return SDL_joyname[device_index]; } + static int HAIKU_JoystickGetDevicePlayerIndex(int device_index) + { + return -1; + } + /* Function to perform the mapping from device index to the instance id for this index */ - SDL_JoystickID SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) + static SDL_JoystickID HAIKU_JoystickGetDeviceInstanceID(int device_index) { return device_index; } + static void HAIKU_JoystickClose(SDL_Joystick * joystick); + /* Function to open a joystick for use. The joystick to open is specified by the device index. This should fill the nbuttons and naxes fields of the joystick structure. It returns 0, or -1 if there is an error. */ - int SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) + static int HAIKU_JoystickOpen(SDL_Joystick * joystick, int device_index) { BJoystick *stick; @@ -127,7 +134,7 @@ extern "C" /* Open the requested joystick for use */ if (stick->Open(SDL_joyport[device_index]) == B_ERROR) { - SDL_SYS_JoystickClose(joystick); + HAIKU_JoystickClose(joystick); return SDL_SetError("Unable to open joystick"); } @@ -144,18 +151,12 @@ extern "C" joystick->hwdata->new_hats = (uint8 *) SDL_malloc(joystick->nhats * sizeof(uint8)); if (!joystick->hwdata->new_hats || !joystick->hwdata->new_axes) { - SDL_SYS_JoystickClose(joystick); + HAIKU_JoystickClose(joystick); return SDL_OutOfMemory(); } /* We're done! */ - return (0); - } - -/* Function to determine if this joystick is attached to the system right now */ - SDL_bool SDL_SYS_JoystickAttached(SDL_Joystick *joystick) - { - return SDL_TRUE; + return 0; } /* Function to update the state of a joystick - called as a device poll. @@ -163,7 +164,7 @@ extern "C" * but instead should call SDL_PrivateJoystick*() to deliver events * and update joystick device state. */ - void SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) + static void HAIKU_JoystickUpdate(SDL_Joystick * joystick) { static const Uint8 hat_map[9] = { SDL_HAT_CENTERED, @@ -212,7 +213,7 @@ extern "C" } /* Function to close a joystick after use */ - void SDL_SYS_JoystickClose(SDL_Joystick * joystick) + static void HAIKU_JoystickClose(SDL_Joystick * joystick) { if (joystick->hwdata) { joystick->hwdata->stick->Close(); @@ -224,42 +225,53 @@ extern "C" } /* Function to perform any system-specific joystick related cleanup */ - void SDL_SYS_JoystickQuit(void) + static void HAIKU_JoystickQuit(void) { int i; - for (i = 0; i < SDL_SYS_numjoysticks; ++i) { + for (i = 0; i < numjoysticks; ++i) { SDL_free(SDL_joyport[i]); } SDL_joyport[0] = NULL; - for (i = 0; i < SDL_SYS_numjoysticks; ++i) { + for (i = 0; i < numjoysticks; ++i) { SDL_free(SDL_joyname[i]); } SDL_joyname[0] = NULL; } - SDL_JoystickGUID SDL_SYS_JoystickGetDeviceGUID( int device_index ) + static SDL_JoystickGUID HAIKU_JoystickGetDeviceGUID( int device_index ) { SDL_JoystickGUID guid; /* the GUID is just the first 16 chars of the name for now */ - const char *name = SDL_SYS_JoystickNameForDeviceIndex( device_index ); + const char *name = HAIKU_JoystickGetDeviceName( device_index ); SDL_zero( guid ); SDL_memcpy( &guid, name, SDL_min( sizeof(guid), SDL_strlen( name ) ) ); return guid; } - SDL_JoystickGUID SDL_SYS_JoystickGetGUID(SDL_Joystick * joystick) + static int HAIKU_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) { - SDL_JoystickGUID guid; - /* the GUID is just the first 16 chars of the name for now */ - const char *name = joystick->name; - SDL_zero( guid ); - SDL_memcpy( &guid, name, SDL_min( sizeof(guid), SDL_strlen( name ) ) ); - return guid; + return SDL_Unsupported(); } -}; // extern "C" + SDL_JoystickDriver SDL_HAIKU_JoystickDriver = + { + HAIKU_JoystickInit, + HAIKU_JoystickGetCount, + HAIKU_JoystickDetect, + HAIKU_JoystickGetDeviceName, + HAIKU_JoystickGetDevicePlayerIndex, + HAIKU_JoystickGetDeviceGUID, + HAIKU_JoystickGetDeviceInstanceID, + HAIKU_JoystickOpen, + HAIKU_JoystickRumble, + HAIKU_JoystickUpdate, + HAIKU_JoystickClose, + HAIKU_JoystickQuit, + }; + +} // extern "C" #endif /* SDL_JOYSTICK_HAIKU */ diff --git a/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_ps4.c b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_ps4.c new file mode 100644 index 000000000..cdd478a07 --- /dev/null +++ b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_ps4.c @@ -0,0 +1,566 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 driver supports both simplified reports and the extended input reports enabled by Steam. + Code and logic contributed by Valve Corporation under the SDL zlib license. +*/ +#include "../../SDL_internal.h" + +#ifdef SDL_JOYSTICK_HIDAPI + +#include "SDL_hints.h" +#include "SDL_log.h" +#include "SDL_events.h" +#include "SDL_timer.h" +#include "SDL_joystick.h" +#include "SDL_gamecontroller.h" +#include "../SDL_sysjoystick.h" +#include "SDL_hidapijoystick_c.h" + + +#ifdef SDL_JOYSTICK_HIDAPI_PS4 + +#define SONY_USB_VID 0x054C +#define SONY_DS4_PID 0x05C4 +#define SONY_DS4_DONGLE_PID 0x0BA0 +#define SONY_DS4_SLIM_PID 0x09CC + +#define RAZER_USB_VID 0x1532 +#define RAZER_PANTHERA_PID 0X0401 +#define RAZER_PANTHERA_EVO_PID 0x1008 + +#define USB_PACKET_LENGTH 64 + +#define VOLUME_CHECK_INTERVAL_MS (10 * 1000) + +typedef enum +{ + k_EPS4ReportIdUsbState = 1, + k_EPS4ReportIdUsbEffects = 5, + k_EPS4ReportIdBluetoothState = 17, + k_EPS4ReportIdBluetoothEffects = 17, + k_EPS4ReportIdDisconnectMessage = 226, +} EPS4ReportId; + +typedef enum +{ + k_ePS4FeatureReportIdGyroCalibration_USB = 0x02, + k_ePS4FeatureReportIdGyroCalibration_BT = 0x05, + k_ePS4FeatureReportIdSerialNumber = 0x12, +} EPS4FeatureReportID; + +typedef struct +{ + Uint8 ucLeftJoystickX; + Uint8 ucLeftJoystickY; + Uint8 ucRightJoystickX; + Uint8 ucRightJoystickY; + Uint8 rgucButtonsHatAndCounter[ 3 ]; + Uint8 ucTriggerLeft; + Uint8 ucTriggerRight; + Uint8 _rgucPad0[ 3 ]; + Sint16 sGyroX; + Sint16 sGyroY; + Sint16 sGyroZ; + Sint16 sAccelX; + Sint16 sAccelY; + Sint16 sAccelZ; + Uint8 _rgucPad1[ 5 ]; + Uint8 ucBatteryLevel; + Uint8 _rgucPad2[ 4 ]; + Uint8 ucTrackpadCounter1; + Uint8 rgucTrackpadData1[ 3 ]; + Uint8 ucTrackpadCounter2; + Uint8 rgucTrackpadData2[ 3 ]; +} PS4StatePacket_t; + +typedef struct +{ + Uint8 ucRumbleRight; + Uint8 ucRumbleLeft; + Uint8 ucLedRed; + Uint8 ucLedGreen; + Uint8 ucLedBlue; + Uint8 ucLedDelayOn; + Uint8 ucLedDelayOff; + Uint8 _rgucPad0[ 8 ]; + Uint8 ucVolumeLeft; + Uint8 ucVolumeRight; + Uint8 ucVolumeMic; + Uint8 ucVolumeSpeaker; +} DS4EffectsState_t; + +typedef struct { + SDL_bool is_dongle; + SDL_bool is_bluetooth; + SDL_bool audio_supported; + SDL_bool rumble_supported; + Uint8 volume; + Uint32 last_volume_check; + Uint32 rumble_expiration; + PS4StatePacket_t last_state; +} SDL_DriverPS4_Context; + + +/* Public domain CRC implementation adapted from: + http://home.thep.lu.se/~bjorn/crc/crc32_simple.c +*/ +static Uint32 crc32_for_byte(Uint32 r) +{ + int i; + for(i = 0; i < 8; ++i) { + r = (r & 1? 0: (Uint32)0xEDB88320L) ^ r >> 1; + } + return r ^ (Uint32)0xFF000000L; +} + +static Uint32 crc32(Uint32 crc, const void *data, int count) +{ + int i; + for(i = 0; i < count; ++i) { + crc = crc32_for_byte((Uint8)crc ^ ((const Uint8*)data)[i]) ^ crc >> 8; + } + return crc; +} + +#if defined(__WIN32__) && defined(HAVE_ENDPOINTVOLUME_H) +#include "../../core/windows/SDL_windows.h" + +#ifndef NTDDI_VISTA +#define NTDDI_VISTA 0x06000000 +#endif +#ifndef _WIN32_WINNT_VISTA +#define _WIN32_WINNT_VISTA 0x0600 +#endif + +/* Define Vista for the Audio related includes below to work */ +#undef NTDDI_VERSION +#define NTDDI_VERSION NTDDI_VISTA +#undef _WIN32_WINNT +#define _WIN32_WINNT _WIN32_WINNT_VISTA +#define COBJMACROS +#include +#include +#include + +#undef DEFINE_GUID +#define DEFINE_GUID(n,l,w1,w2,b1,b2,b3,b4,b5,b6,b7,b8) static const GUID n = {l,w1,w2,{b1,b2,b3,b4,b5,b6,b7,b8}} +DEFINE_GUID(SDL_CLSID_MMDeviceEnumerator, 0xBCDE0395, 0xE52F, 0x467C, 0x8E, 0x3D, 0xC4, 0x57, 0x92, 0x91, 0x69, 0x2E); +DEFINE_GUID(SDL_IID_IMMDeviceEnumerator, 0xA95664D2, 0x9614, 0x4F35, 0xA7, 0x46, 0xDE, 0x8D, 0xB6, 0x36, 0x17, 0xE6); +DEFINE_GUID(SDL_IID_IAudioEndpointVolume, 0x5CDF2C82, 0x841E, 0x4546, 0x97, 0x22, 0x0C, 0xF7, 0x40, 0x78, 0x22, 0x9A); +#endif + + + +static float GetSystemVolume(void) +{ + float volume = -1.0f; /* Return this if we can't get system volume */ + +#if defined(__WIN32__) && defined(HAVE_ENDPOINTVOLUME_H) + HRESULT hr = WIN_CoInitialize(); + if (SUCCEEDED(hr)) { + IMMDeviceEnumerator *pEnumerator; + + /* This should gracefully fail on XP and succeed on everything Vista and above */ + hr = CoCreateInstance(&SDL_CLSID_MMDeviceEnumerator, NULL, CLSCTX_ALL, &SDL_IID_IMMDeviceEnumerator, (LPVOID*)&pEnumerator); + if (SUCCEEDED(hr)) { + IMMDevice *pDevice; + + hr = IMMDeviceEnumerator_GetDefaultAudioEndpoint(pEnumerator, eRender, eConsole, &pDevice); + if (SUCCEEDED(hr)) { + IAudioEndpointVolume *pEndpointVolume; + + hr = IMMDevice_Activate(pDevice, &SDL_IID_IAudioEndpointVolume, CLSCTX_ALL, NULL, (LPVOID*)&pEndpointVolume); + if (SUCCEEDED(hr)) { + IAudioEndpointVolume_GetMasterVolumeLevelScalar(pEndpointVolume, &volume); + IUnknown_Release(pEndpointVolume); + } + IUnknown_Release(pDevice); + } + IUnknown_Release(pEnumerator); + } + WIN_CoUninitialize(); + } +#endif /* __WIN32__ */ + + return volume; +} + +static uint8_t GetPlaystationVolumeFromFloat(float fVolume) +{ + const int k_nVolumeFitRatio = 15; + const int k_nVolumeFitOffset = 9; + float fVolLog; + + if (fVolume > 1.0f || fVolume < 0.0f) { + fVolume = 0.30f; + } + fVolLog = SDL_logf(fVolume * 100); + + return (Uint8)((fVolLog * k_nVolumeFitRatio) + k_nVolumeFitOffset); +} + +static SDL_bool +HIDAPI_DriverPS4_IsSupportedDevice(Uint16 vendor_id, Uint16 product_id, Uint16 version, int interface_number) +{ + return SDL_IsJoystickPS4(vendor_id, product_id); +} + +static const char * +HIDAPI_DriverPS4_GetDeviceName(Uint16 vendor_id, Uint16 product_id) +{ + if (vendor_id == SONY_USB_VID) { + return "PS4 Controller"; + } + return NULL; +} + +static SDL_bool ReadFeatureReport(hid_device *dev, Uint8 report_id, Uint8 *data, size_t size) +{ + Uint8 report[USB_PACKET_LENGTH + 1]; + + SDL_memset(report, 0, sizeof(report)); + report[0] = report_id; + if (hid_get_feature_report(dev, report, sizeof(report)) < 0) { + return SDL_FALSE; + } + SDL_memcpy(data, report, SDL_min(size, sizeof(report))); + return SDL_TRUE; +} + +static SDL_bool CheckUSBConnected(hid_device *dev) +{ + int i; + Uint8 data[16]; + + /* This will fail if we're on Bluetooth */ + if (ReadFeatureReport(dev, k_ePS4FeatureReportIdSerialNumber, data, sizeof(data))) { + for (i = 0; i < sizeof(data); ++i) { + if (data[i] != 0x00) { + return SDL_TRUE; + } + } + /* Maybe the dongle without a connected controller? */ + } + return SDL_FALSE; +} + +static SDL_bool HIDAPI_DriverPS4_CanRumble(Uint16 vendor_id, Uint16 product_id) +{ + /* The Razer Panthera fight stick hangs when trying to rumble */ + if (vendor_id == RAZER_USB_VID && + (product_id == RAZER_PANTHERA_PID || product_id == RAZER_PANTHERA_EVO_PID)) { + return SDL_FALSE; + } + return SDL_TRUE; +} + +static int HIDAPI_DriverPS4_Rumble(SDL_Joystick *joystick, hid_device *dev, void *context, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms); + +static SDL_bool +HIDAPI_DriverPS4_Init(SDL_Joystick *joystick, hid_device *dev, Uint16 vendor_id, Uint16 product_id, void **context) +{ + SDL_DriverPS4_Context *ctx; + + ctx = (SDL_DriverPS4_Context *)SDL_calloc(1, sizeof(*ctx)); + if (!ctx) { + SDL_OutOfMemory(); + return SDL_FALSE; + } + *context = ctx; + + /* Check for type of connection */ + ctx->is_dongle = (vendor_id == SONY_USB_VID && product_id == SONY_DS4_DONGLE_PID); + if (ctx->is_dongle) { + ctx->is_bluetooth = SDL_FALSE; + } else if (vendor_id == SONY_USB_VID) { + ctx->is_bluetooth = !CheckUSBConnected(dev); + } else { + /* Third party controllers appear to all be wired */ + ctx->is_bluetooth = SDL_FALSE; + } +#ifdef DEBUG_PS4 + SDL_Log("PS4 dongle = %s, bluetooth = %s\n", ctx->is_dongle ? "TRUE" : "FALSE", ctx->is_bluetooth ? "TRUE" : "FALSE"); +#endif + + /* Check to see if audio is supported */ + if (vendor_id == SONY_USB_VID && + (product_id == SONY_DS4_SLIM_PID || product_id == SONY_DS4_DONGLE_PID )) { + ctx->audio_supported = SDL_TRUE; + } + + if (HIDAPI_DriverPS4_CanRumble(vendor_id, product_id)) { + if (ctx->is_bluetooth) { + ctx->rumble_supported = SDL_GetHintBoolean(SDL_HINT_JOYSTICK_HIDAPI_PS4_RUMBLE, SDL_FALSE); + } else { + ctx->rumble_supported = SDL_TRUE; + } + } + + /* Initialize LED and effect state */ + HIDAPI_DriverPS4_Rumble(joystick, dev, ctx, 0, 0, 0); + + /* Initialize the joystick capabilities */ + joystick->nbuttons = SDL_CONTROLLER_BUTTON_MAX; + joystick->naxes = SDL_CONTROLLER_AXIS_MAX; + joystick->epowerlevel = SDL_JOYSTICK_POWER_WIRED; + + return SDL_TRUE; +} + +static int +HIDAPI_DriverPS4_Rumble(SDL_Joystick *joystick, hid_device *dev, void *context, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + SDL_DriverPS4_Context *ctx = (SDL_DriverPS4_Context *)context; + DS4EffectsState_t *effects; + Uint8 data[78]; + int report_size, offset; + + if (!ctx->rumble_supported) { + return SDL_Unsupported(); + } + + /* In order to send rumble, we have to send a complete effect packet */ + SDL_memset(data, 0, sizeof(data)); + + if (ctx->is_bluetooth) { + data[0] = k_EPS4ReportIdBluetoothEffects; + data[1] = 0xC0 | 0x04; /* Magic value HID + CRC, also sets interval to 4ms for samples */ + data[3] = 0x03; /* 0x1 is rumble, 0x2 is lightbar, 0x4 is the blink interval */ + + report_size = 78; + offset = 6; + } else { + data[0] = k_EPS4ReportIdUsbEffects; + data[1] = 0x07; /* Magic value */ + + report_size = 32; + offset = 4; + } + effects = (DS4EffectsState_t *)&data[offset]; + + effects->ucRumbleLeft = (low_frequency_rumble >> 8); + effects->ucRumbleRight = (high_frequency_rumble >> 8); + + effects->ucLedRed = 0; + effects->ucLedGreen = 0; + effects->ucLedBlue = 80; + + if (ctx->audio_supported) { + Uint32 now = SDL_GetTicks(); + if (!ctx->last_volume_check || + SDL_TICKS_PASSED(now, ctx->last_volume_check + VOLUME_CHECK_INTERVAL_MS)) { + ctx->volume = GetPlaystationVolumeFromFloat(GetSystemVolume()); + ctx->last_volume_check = now; + } + + effects->ucVolumeRight = ctx->volume; + effects->ucVolumeLeft = ctx->volume; + effects->ucVolumeSpeaker = ctx->volume; + effects->ucVolumeMic = 0xFF; + } + + if (ctx->is_bluetooth) { + /* Bluetooth reports need a CRC at the end of the packet (at least on Linux) */ + Uint8 ubHdr = 0xA2; /* hidp header is part of the CRC calculation */ + Uint32 unCRC; + unCRC = crc32(0, &ubHdr, 1); + unCRC = crc32(unCRC, data, (Uint32)(report_size - sizeof(unCRC))); + SDL_memcpy(&data[report_size - sizeof(unCRC)], &unCRC, sizeof(unCRC)); + } + + if (hid_write(dev, data, report_size) != report_size) { + return SDL_SetError("Couldn't send rumble packet"); + } + + if ((low_frequency_rumble || high_frequency_rumble) && duration_ms) { + ctx->rumble_expiration = SDL_GetTicks() + duration_ms; + } else { + ctx->rumble_expiration = 0; + } + return 0; +} + +static void +HIDAPI_DriverPS4_HandleStatePacket(SDL_Joystick *joystick, hid_device *dev, SDL_DriverPS4_Context *ctx, PS4StatePacket_t *packet) +{ + Sint16 axis; + + if (ctx->last_state.rgucButtonsHatAndCounter[0] != packet->rgucButtonsHatAndCounter[0]) { + { + Uint8 data = (packet->rgucButtonsHatAndCounter[0] >> 4); + + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_X, (data & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_A, (data & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_B, (data & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_Y, (data & 0x08) ? SDL_PRESSED : SDL_RELEASED); + } + { + Uint8 data = (packet->rgucButtonsHatAndCounter[0] & 0x0F); + SDL_bool dpad_up = SDL_FALSE; + SDL_bool dpad_down = SDL_FALSE; + SDL_bool dpad_left = SDL_FALSE; + SDL_bool dpad_right = SDL_FALSE; + + switch (data) { + case 0: + dpad_up = SDL_TRUE; + break; + case 1: + dpad_up = SDL_TRUE; + dpad_right = SDL_TRUE; + break; + case 2: + dpad_right = SDL_TRUE; + break; + case 3: + dpad_right = SDL_TRUE; + dpad_down = SDL_TRUE; + break; + case 4: + dpad_down = SDL_TRUE; + break; + case 5: + dpad_left = SDL_TRUE; + dpad_down = SDL_TRUE; + break; + case 6: + dpad_left = SDL_TRUE; + break; + case 7: + dpad_up = SDL_TRUE; + dpad_left = SDL_TRUE; + break; + default: + break; + } + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_DOWN, dpad_down); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_UP, dpad_up); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_RIGHT, dpad_right); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_LEFT, dpad_left); + } + } + + if (ctx->last_state.rgucButtonsHatAndCounter[1] != packet->rgucButtonsHatAndCounter[1]) { + Uint8 data = packet->rgucButtonsHatAndCounter[1]; + + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSHOULDER, (data & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSHOULDER, (data & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_BACK, (data & 0x10) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_START, (data & 0x20) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSTICK, (data & 0x40) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSTICK, (data & 0x80) ? SDL_PRESSED : SDL_RELEASED); + } + + if (ctx->last_state.rgucButtonsHatAndCounter[2] != packet->rgucButtonsHatAndCounter[2]) { + Uint8 data = (packet->rgucButtonsHatAndCounter[2] & 0x03); + + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_GUIDE, (data & 0x01) ? SDL_PRESSED : SDL_RELEASED); + } + + axis = ((int)packet->ucTriggerLeft * 257) - 32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERLEFT, axis); + axis = ((int)packet->ucTriggerRight * 257) - 32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERRIGHT, axis); + axis = ((int)packet->ucLeftJoystickX * 257) - 32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTX, axis); + axis = ((int)packet->ucLeftJoystickY * 257) - 32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTY, axis); + axis = ((int)packet->ucRightJoystickX * 257) - 32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTX, axis); + axis = ((int)packet->ucRightJoystickY * 257) - 32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTY, axis); + + if (packet->ucBatteryLevel & 0x10) { + joystick->epowerlevel = SDL_JOYSTICK_POWER_WIRED; + } else { + /* Battery level ranges from 0 to 10 */ + int level = (packet->ucBatteryLevel & 0xF); + if (level == 0) { + joystick->epowerlevel = SDL_JOYSTICK_POWER_EMPTY; + } else if (level <= 2) { + joystick->epowerlevel = SDL_JOYSTICK_POWER_LOW; + } else if (level <= 7) { + joystick->epowerlevel = SDL_JOYSTICK_POWER_MEDIUM; + } else { + joystick->epowerlevel = SDL_JOYSTICK_POWER_FULL; + } + } + + SDL_memcpy(&ctx->last_state, packet, sizeof(ctx->last_state)); +} + +static SDL_bool +HIDAPI_DriverPS4_Update(SDL_Joystick *joystick, hid_device *dev, void *context) +{ + SDL_DriverPS4_Context *ctx = (SDL_DriverPS4_Context *)context; + Uint8 data[USB_PACKET_LENGTH]; + int size; + + while ((size = hid_read_timeout(dev, data, sizeof(data), 0)) > 0) { + switch (data[0]) { + case k_EPS4ReportIdUsbState: + HIDAPI_DriverPS4_HandleStatePacket(joystick, dev, ctx, (PS4StatePacket_t *)&data[1]); + break; + case k_EPS4ReportIdBluetoothState: + /* Bluetooth state packets have two additional bytes at the beginning */ + HIDAPI_DriverPS4_HandleStatePacket(joystick, dev, ctx, (PS4StatePacket_t *)&data[3]); + break; + default: +#ifdef DEBUG_JOYSTICK + SDL_Log("Unknown PS4 packet: 0x%.2x\n", data[0]); +#endif + break; + } + } + + if (ctx->rumble_expiration) { + Uint32 now = SDL_GetTicks(); + if (SDL_TICKS_PASSED(now, ctx->rumble_expiration)) { + HIDAPI_DriverPS4_Rumble(joystick, dev, context, 0, 0, 0); + } + } + + return (size >= 0); +} + +static void +HIDAPI_DriverPS4_Quit(SDL_Joystick *joystick, hid_device *dev, void *context) +{ + SDL_free(context); +} + +SDL_HIDAPI_DeviceDriver SDL_HIDAPI_DriverPS4 = +{ + SDL_HINT_JOYSTICK_HIDAPI_PS4, + SDL_TRUE, + HIDAPI_DriverPS4_IsSupportedDevice, + HIDAPI_DriverPS4_GetDeviceName, + HIDAPI_DriverPS4_Init, + HIDAPI_DriverPS4_Rumble, + HIDAPI_DriverPS4_Update, + HIDAPI_DriverPS4_Quit +}; + +#endif /* SDL_JOYSTICK_HIDAPI_PS4 */ + +#endif /* SDL_JOYSTICK_HIDAPI */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_switch.c b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_switch.c new file mode 100644 index 000000000..16e4ea3c2 --- /dev/null +++ b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_switch.c @@ -0,0 +1,905 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 driver supports the Nintendo Switch Pro controller. + Code and logic contributed by Valve Corporation under the SDL zlib license. +*/ +#include "../../SDL_internal.h" + +#ifdef SDL_JOYSTICK_HIDAPI + +#include "SDL_hints.h" +#include "SDL_log.h" +#include "SDL_events.h" +#include "SDL_timer.h" +#include "SDL_joystick.h" +#include "SDL_gamecontroller.h" +#include "../SDL_sysjoystick.h" +#include "SDL_hidapijoystick_c.h" + + +#ifdef SDL_JOYSTICK_HIDAPI_SWITCH + +typedef enum { + k_eSwitchInputReportIDs_SubcommandReply = 0x21, + k_eSwitchInputReportIDs_FullControllerState = 0x30, + k_eSwitchInputReportIDs_SimpleControllerState = 0x3F, + k_eSwitchInputReportIDs_CommandAck = 0x81, +} ESwitchInputReportIDs; + +typedef enum { + k_eSwitchOutputReportIDs_RumbleAndSubcommand = 0x01, + k_eSwitchOutputReportIDs_Rumble = 0x10, + k_eSwitchOutputReportIDs_Proprietary = 0x80, +} ESwitchOutputReportIDs; + +typedef enum { + k_eSwitchSubcommandIDs_BluetoothManualPair = 0x01, + k_eSwitchSubcommandIDs_RequestDeviceInfo = 0x02, + k_eSwitchSubcommandIDs_SetInputReportMode = 0x03, + k_eSwitchSubcommandIDs_SetHCIState = 0x06, + k_eSwitchSubcommandIDs_SPIFlashRead = 0x10, + k_eSwitchSubcommandIDs_SetPlayerLights = 0x30, + k_eSwitchSubcommandIDs_SetHomeLight = 0x38, + k_eSwitchSubcommandIDs_EnableIMU = 0x40, + k_eSwitchSubcommandIDs_SetIMUSensitivity = 0x41, + k_eSwitchSubcommandIDs_EnableVibration = 0x48, +} ESwitchSubcommandIDs; + +typedef enum { + k_eSwitchProprietaryCommandIDs_Handshake = 0x02, + k_eSwitchProprietaryCommandIDs_HighSpeed = 0x03, + k_eSwitchProprietaryCommandIDs_ForceUSB = 0x04, + k_eSwitchProprietaryCommandIDs_ClearUSB = 0x05, + k_eSwitchProprietaryCommandIDs_ResetMCU = 0x06, +} ESwitchProprietaryCommandIDs; + +typedef enum { + k_eSwitchDeviceInfoControllerType_JoyConLeft = 0x1, + k_eSwitchDeviceInfoControllerType_JoyConRight = 0x2, + k_eSwitchDeviceInfoControllerType_ProController = 0x3, +} ESwitchDeviceInfoControllerType; + +#define k_unSwitchOutputPacketDataLength 49 +#define k_unSwitchMaxOutputPacketLength 64 +#define k_unSwitchBluetoothPacketLength k_unSwitchOutputPacketDataLength +#define k_unSwitchUSBPacketLength k_unSwitchMaxOutputPacketLength + +#define k_unSPIStickCalibrationStartOffset 0x603D +#define k_unSPIStickCalibrationEndOffset 0x604E +#define k_unSPIStickCalibrationLength (k_unSPIStickCalibrationEndOffset - k_unSPIStickCalibrationStartOffset + 1) + +#pragma pack(1) +typedef struct +{ + Uint8 rgucButtons[2]; + Uint8 ucStickHat; + Sint16 sJoystickLeft[2]; + Sint16 sJoystickRight[2]; +} SwitchSimpleStatePacket_t; + +typedef struct +{ + Uint8 ucCounter; + Uint8 ucBatteryAndConnection; + Uint8 rgucButtons[3]; + Uint8 rgucJoystickLeft[3]; + Uint8 rgucJoystickRight[3]; + Uint8 ucVibrationCode; +} SwitchControllerStatePacket_t; + +typedef struct +{ + SwitchControllerStatePacket_t controllerState; + + struct { + Sint16 sAccelX; + Sint16 sAccelY; + Sint16 sAccelZ; + + Sint16 sGyroX; + Sint16 sGyroY; + Sint16 sGyroZ; + } imuState[3]; +} SwitchStatePacket_t; + +typedef struct +{ + Uint32 unAddress; + Uint8 ucLength; +} SwitchSPIOpData_t; + +typedef struct +{ + SwitchControllerStatePacket_t m_controllerState; + + Uint8 ucSubcommandAck; + Uint8 ucSubcommandID; + + #define k_unSubcommandDataBytes 35 + union { + Uint8 rgucSubcommandData[ k_unSubcommandDataBytes ]; + + struct { + SwitchSPIOpData_t opData; + Uint8 rgucReadData[ k_unSubcommandDataBytes - sizeof(SwitchSPIOpData_t) ]; + } spiReadData; + + struct { + Uint8 rgucFirmwareVersion[2]; + Uint8 ucDeviceType; + Uint8 ucFiller1; + Uint8 rgucMACAddress[6]; + Uint8 ucFiller2; + Uint8 ucColorLocation; + } deviceInfo; + }; +} SwitchSubcommandInputPacket_t; + +typedef struct +{ + Uint8 rgucData[4]; +} SwitchRumbleData_t; + +typedef struct +{ + Uint8 ucPacketType; + Uint8 ucPacketNumber; + SwitchRumbleData_t rumbleData[2]; +} SwitchCommonOutputPacket_t; + +typedef struct +{ + SwitchCommonOutputPacket_t commonData; + + Uint8 ucSubcommandID; + Uint8 rgucSubcommandData[ k_unSwitchOutputPacketDataLength - sizeof(SwitchCommonOutputPacket_t) - 1 ]; +} SwitchSubcommandOutputPacket_t; + +typedef struct +{ + Uint8 ucPacketType; + Uint8 ucProprietaryID; + + Uint8 rgucProprietaryData[ k_unSwitchOutputPacketDataLength - 1 - 1 ]; +} SwitchProprietaryOutputPacket_t; +#pragma pack() + +typedef struct { + hid_device *dev; + SDL_bool m_bIsUsingBluetooth; + Uint8 m_nCommandNumber; + SwitchCommonOutputPacket_t m_RumblePacket; + Uint32 m_nRumbleExpiration; + Uint8 m_rgucReadBuffer[k_unSwitchMaxOutputPacketLength]; + SwitchSimpleStatePacket_t m_lastSimpleState; + SwitchStatePacket_t m_lastFullState; + + struct StickCalibrationData { + struct { + Sint16 sCenter; + Sint16 sMin; + Sint16 sMax; + } axis[2]; + } m_StickCalData[2]; + + struct StickExtents { + struct { + Sint16 sMin; + Sint16 sMax; + } axis[2]; + } m_StickExtents[2]; +} SDL_DriverSwitch_Context; + + +static SDL_bool +HIDAPI_DriverSwitch_IsSupportedDevice(Uint16 vendor_id, Uint16 product_id, Uint16 version, int interface_number) +{ + return SDL_IsJoystickNintendoSwitchPro(vendor_id, product_id); +} + +static const char * +HIDAPI_DriverSwitch_GetDeviceName(Uint16 vendor_id, Uint16 product_id) +{ + /* Give a user friendly name for this controller */ + if (SDL_IsJoystickNintendoSwitchPro(vendor_id, product_id)) { + return "Nintendo Switch Pro Controller"; + } + return NULL; +} + +static int ReadInput(SDL_DriverSwitch_Context *ctx) +{ + return hid_read_timeout(ctx->dev, ctx->m_rgucReadBuffer, sizeof(ctx->m_rgucReadBuffer), 0); +} + +static int WriteOutput(SDL_DriverSwitch_Context *ctx, Uint8 *data, int size) +{ + return hid_write(ctx->dev, data, size); +} + +static SwitchSubcommandInputPacket_t *ReadSubcommandReply(SDL_DriverSwitch_Context *ctx, ESwitchSubcommandIDs expectedID) +{ + /* Average response time for messages is ~30ms */ + Uint32 TimeoutMs = 100; + Uint32 startTicks = SDL_GetTicks(); + + int nRead = 0; + while ((nRead = ReadInput(ctx)) != -1) { + if (nRead > 0) { + if (ctx->m_rgucReadBuffer[0] == k_eSwitchInputReportIDs_SubcommandReply) { + SwitchSubcommandInputPacket_t *reply = (SwitchSubcommandInputPacket_t *)&ctx->m_rgucReadBuffer[ 1 ]; + if (reply->ucSubcommandID == expectedID && (reply->ucSubcommandAck & 0x80)) { + return reply; + } + } + } else { + SDL_Delay(1); + } + + if (SDL_TICKS_PASSED(SDL_GetTicks(), startTicks + TimeoutMs)) { + break; + } + } + return NULL; +} + +static SDL_bool ReadProprietaryReply(SDL_DriverSwitch_Context *ctx, ESwitchProprietaryCommandIDs expectedID) +{ + /* Average response time for messages is ~30ms */ + Uint32 TimeoutMs = 100; + Uint32 startTicks = SDL_GetTicks(); + + int nRead = 0; + while ((nRead = ReadInput(ctx)) != -1) { + if (nRead > 0) { + if (ctx->m_rgucReadBuffer[0] == k_eSwitchInputReportIDs_CommandAck && ctx->m_rgucReadBuffer[ 1 ] == expectedID) { + return SDL_TRUE; + } + } else { + SDL_Delay(1); + } + + if (SDL_TICKS_PASSED(SDL_GetTicks(), startTicks + TimeoutMs)) { + break; + } + } + return SDL_FALSE; +} + +static void ConstructSubcommand(SDL_DriverSwitch_Context *ctx, ESwitchSubcommandIDs ucCommandID, Uint8 *pBuf, Uint8 ucLen, SwitchSubcommandOutputPacket_t *outPacket) +{ + SDL_memset(outPacket, 0, sizeof(*outPacket)); + + outPacket->commonData.ucPacketType = k_eSwitchOutputReportIDs_RumbleAndSubcommand; + outPacket->commonData.ucPacketNumber = ctx->m_nCommandNumber; + + SDL_memcpy(&outPacket->commonData.rumbleData, &ctx->m_RumblePacket.rumbleData, sizeof(ctx->m_RumblePacket.rumbleData)); + + outPacket->ucSubcommandID = ucCommandID; + SDL_memcpy(outPacket->rgucSubcommandData, pBuf, ucLen); + + ctx->m_nCommandNumber = (ctx->m_nCommandNumber + 1) & 0xF; +} + +static SDL_bool WritePacket(SDL_DriverSwitch_Context *ctx, void *pBuf, Uint8 ucLen) +{ + Uint8 rgucBuf[k_unSwitchMaxOutputPacketLength]; + const size_t unWriteSize = ctx->m_bIsUsingBluetooth ? k_unSwitchBluetoothPacketLength : k_unSwitchUSBPacketLength; + + if (ucLen > k_unSwitchOutputPacketDataLength) { + return SDL_FALSE; + } + + if (ucLen < unWriteSize) { + SDL_memcpy(rgucBuf, pBuf, ucLen); + SDL_memset(rgucBuf+ucLen, 0, unWriteSize-ucLen); + pBuf = rgucBuf; + ucLen = (Uint8)unWriteSize; + } + return (WriteOutput(ctx, (Uint8 *)pBuf, ucLen) >= 0); +} + +static SDL_bool WriteSubcommand(SDL_DriverSwitch_Context *ctx, ESwitchSubcommandIDs ucCommandID, Uint8 *pBuf, Uint8 ucLen, SwitchSubcommandInputPacket_t **ppReply) +{ + int nRetries = 5; + SwitchSubcommandInputPacket_t *reply = NULL; + + while (!reply && nRetries--) { + SwitchSubcommandOutputPacket_t commandPacket; + ConstructSubcommand(ctx, ucCommandID, pBuf, ucLen, &commandPacket); + + if (!WritePacket(ctx, &commandPacket, sizeof(commandPacket))) { + continue; + } + + reply = ReadSubcommandReply(ctx, ucCommandID); + } + + if (ppReply) { + *ppReply = reply; + } + return reply != NULL; +} + +static SDL_bool WriteProprietary(SDL_DriverSwitch_Context *ctx, ESwitchProprietaryCommandIDs ucCommand, Uint8 *pBuf, Uint8 ucLen, SDL_bool waitForReply) +{ + int nRetries = 5; + + while (nRetries--) { + SwitchProprietaryOutputPacket_t packet; + + if ((!pBuf && ucLen > 0) || ucLen > sizeof(packet.rgucProprietaryData)) { + return SDL_FALSE; + } + + packet.ucPacketType = k_eSwitchOutputReportIDs_Proprietary; + packet.ucProprietaryID = ucCommand; + SDL_memcpy(packet.rgucProprietaryData, pBuf, ucLen); + + if (!WritePacket(ctx, &packet, sizeof(packet))) { + continue; + } + + if (!waitForReply || ReadProprietaryReply(ctx, ucCommand)) { + return SDL_TRUE; + } + } + return SDL_FALSE; +} + +static void SetNeutralRumble(SwitchRumbleData_t *pRumble) +{ + pRumble->rgucData[0] = 0x00; + pRumble->rgucData[1] = 0x01; + pRumble->rgucData[2] = 0x40; + pRumble->rgucData[3] = 0x40; +} + +static void EncodeRumble(SwitchRumbleData_t *pRumble, Uint16 usHighFreq, Uint8 ucHighFreqAmp, Uint8 ucLowFreq, Uint16 usLowFreqAmp) +{ + if (ucHighFreqAmp > 0 || usLowFreqAmp > 0) { + // High-band frequency and low-band amplitude are actually nine-bits each so they + // take a bit from the high-band amplitude and low-band frequency bytes respectively + pRumble->rgucData[0] = usHighFreq & 0xFF; + pRumble->rgucData[1] = ucHighFreqAmp | ((usHighFreq >> 8) & 0x01); + + pRumble->rgucData[2] = ucLowFreq | ((usLowFreqAmp >> 8) & 0x80); + pRumble->rgucData[3] = usLowFreqAmp & 0xFF; + +#ifdef DEBUG_RUMBLE + SDL_Log("Freq: %.2X %.2X %.2X, Amp: %.2X %.2X %.2X\n", + usHighFreq & 0xFF, ((usHighFreq >> 8) & 0x01), ucLowFreq, + ucHighFreqAmp, ((usLowFreqAmp >> 8) & 0x80), usLowFreqAmp & 0xFF); +#endif + } else { + SetNeutralRumble(pRumble); + } +} + +static SDL_bool WriteRumble(SDL_DriverSwitch_Context *ctx) +{ + /* Write into m_RumblePacket rather than a temporary buffer to allow the current rumble state + * to be retained for subsequent rumble or subcommand packets sent to the controller + */ + ctx->m_RumblePacket.ucPacketType = k_eSwitchOutputReportIDs_Rumble; + ctx->m_RumblePacket.ucPacketNumber = ctx->m_nCommandNumber; + ctx->m_nCommandNumber = (ctx->m_nCommandNumber + 1) & 0xF; + + return WritePacket(ctx, (Uint8 *)&ctx->m_RumblePacket, sizeof(ctx->m_RumblePacket)); +} + +static SDL_bool BTrySetupUSB(SDL_DriverSwitch_Context *ctx) +{ + /* We have to send a connection handshake to the controller when communicating over USB + * before we're able to send it other commands. Luckily this command is not supported + * over Bluetooth, so we can use the controller's lack of response as a way to + * determine if the connection is over USB or Bluetooth + */ + if (!WriteProprietary(ctx, k_eSwitchProprietaryCommandIDs_Handshake, NULL, 0, SDL_TRUE)) { + return SDL_FALSE; + } + if (!WriteProprietary(ctx, k_eSwitchProprietaryCommandIDs_HighSpeed, NULL, 0, SDL_TRUE)) { + return SDL_FALSE; + } + if (!WriteProprietary(ctx, k_eSwitchProprietaryCommandIDs_Handshake, NULL, 0, SDL_TRUE)) { + return SDL_FALSE; + } + return SDL_TRUE; +} + +static SDL_bool SetVibrationEnabled(SDL_DriverSwitch_Context *ctx, Uint8 enabled) +{ + return WriteSubcommand(ctx, k_eSwitchSubcommandIDs_EnableVibration, &enabled, sizeof(enabled), NULL); + +} +static SDL_bool SetInputMode(SDL_DriverSwitch_Context *ctx, Uint8 input_mode) +{ + return WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SetInputReportMode, &input_mode, 1, NULL); +} + +static SDL_bool SetHomeLED(SDL_DriverSwitch_Context *ctx, Uint8 brightness) +{ + Uint8 ucLedIntensity = 0; + Uint8 rgucBuffer[4]; + + if (brightness > 0) { + if (brightness < 65) { + ucLedIntensity = (brightness + 5) / 10; + } else { + ucLedIntensity = (Uint8)SDL_ceilf(0xF * SDL_powf((float)brightness / 100.f, 2.13f)); + } + } + + rgucBuffer[0] = (0x0 << 4) | 0x1; /* 0 mini cycles (besides first), cycle duration 8ms */ + rgucBuffer[1] = ((ucLedIntensity & 0xF) << 4) | 0x0; /* LED start intensity (0x0-0xF), 0 cycles (LED stays on at start intensity after first cycle) */ + rgucBuffer[2] = ((ucLedIntensity & 0xF) << 4) | 0x0; /* First cycle LED intensity, 0x0 intensity for second cycle */ + rgucBuffer[3] = (0x0 << 4) | 0x0; /* 8ms fade transition to first cycle, 8ms first cycle LED duration */ + + return WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SetHomeLight, rgucBuffer, sizeof(rgucBuffer), NULL); +} + +static SDL_bool SetSlotLED(SDL_DriverSwitch_Context *ctx, Uint8 slot) +{ + Uint8 led_data = (1 << slot); + return WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SetPlayerLights, &led_data, sizeof(led_data), NULL); +} + +static SDL_bool LoadStickCalibration(SDL_DriverSwitch_Context *ctx) +{ + Uint8 *pStickCal; + size_t stick, axis; + SwitchSubcommandInputPacket_t *reply = NULL; + + /* Read Calibration Info */ + SwitchSPIOpData_t readParams; + readParams.unAddress = k_unSPIStickCalibrationStartOffset; + readParams.ucLength = k_unSPIStickCalibrationLength; + + if (!WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply)) { + return SDL_FALSE; + } + + /* Stick calibration values are 12-bits each and are packed by bit + * For whatever reason the fields are in a different order for each stick + * Left: X-Max, Y-Max, X-Center, Y-Center, X-Min, Y-Min + * Right: X-Center, Y-Center, X-Min, Y-Min, X-Max, Y-Max + */ + pStickCal = reply->spiReadData.rgucReadData; + + /* Left stick */ + ctx->m_StickCalData[0].axis[0].sMax = ((pStickCal[1] << 8) & 0xF00) | pStickCal[0]; /* X Axis max above center */ + ctx->m_StickCalData[0].axis[1].sMax = (pStickCal[2] << 4) | (pStickCal[1] >> 4); /* Y Axis max above center */ + ctx->m_StickCalData[0].axis[0].sCenter = ((pStickCal[4] << 8) & 0xF00) | pStickCal[3]; /* X Axis center */ + ctx->m_StickCalData[0].axis[1].sCenter = (pStickCal[5] << 4) | (pStickCal[4] >> 4); /* Y Axis center */ + ctx->m_StickCalData[0].axis[0].sMin = ((pStickCal[7] << 8) & 0xF00) | pStickCal[6]; /* X Axis min below center */ + ctx->m_StickCalData[0].axis[1].sMin = (pStickCal[8] << 4) | (pStickCal[7] >> 4); /* Y Axis min below center */ + + /* Right stick */ + ctx->m_StickCalData[1].axis[0].sCenter = ((pStickCal[10] << 8) & 0xF00) | pStickCal[9]; /* X Axis center */ + ctx->m_StickCalData[1].axis[1].sCenter = (pStickCal[11] << 4) | (pStickCal[10] >> 4); /* Y Axis center */ + ctx->m_StickCalData[1].axis[0].sMin = ((pStickCal[13] << 8) & 0xF00) | pStickCal[12]; /* X Axis min below center */ + ctx->m_StickCalData[1].axis[1].sMin = (pStickCal[14] << 4) | (pStickCal[13] >> 4); /* Y Axis min below center */ + ctx->m_StickCalData[1].axis[0].sMax = ((pStickCal[16] << 8) & 0xF00) | pStickCal[15]; /* X Axis max above center */ + ctx->m_StickCalData[1].axis[1].sMax = (pStickCal[17] << 4) | (pStickCal[16] >> 4); /* Y Axis max above center */ + + /* Filter out any values that were uninitialized (0xFFF) in the SPI read */ + for (stick = 0; stick < 2; ++stick) { + for (axis = 0; axis < 2; ++axis) { + if (ctx->m_StickCalData[stick].axis[axis].sCenter == 0xFFF) { + ctx->m_StickCalData[stick].axis[axis].sCenter = 0; + } + if (ctx->m_StickCalData[stick].axis[axis].sMax == 0xFFF) { + ctx->m_StickCalData[stick].axis[axis].sMax = 0; + } + if (ctx->m_StickCalData[stick].axis[axis].sMin == 0xFFF) { + ctx->m_StickCalData[stick].axis[axis].sMin = 0; + } + } + } + + if (ctx->m_bIsUsingBluetooth) { + for (stick = 0; stick < 2; ++stick) { + for(axis = 0; axis < 2; ++axis) { + ctx->m_StickExtents[stick].axis[axis].sMin = (Sint16)(SDL_MIN_SINT16 * 0.5f); + ctx->m_StickExtents[stick].axis[axis].sMax = (Sint16)(SDL_MAX_SINT16 * 0.5f); + } + } + } else { + for (stick = 0; stick < 2; ++stick) { + for(axis = 0; axis < 2; ++axis) { + ctx->m_StickExtents[stick].axis[axis].sMin = -(Sint16)(ctx->m_StickCalData[stick].axis[axis].sMin * 0.7f); + ctx->m_StickExtents[stick].axis[axis].sMax = (Sint16)(ctx->m_StickCalData[stick].axis[axis].sMax * 0.7f); + } + } + } + return SDL_TRUE; +} + +static float fsel(float fComparand, float fValGE, float fLT) +{ + return fComparand >= 0 ? fValGE : fLT; +} + +static float RemapVal(float val, float A, float B, float C, float D) +{ + if (A == B) { + return fsel(val - B , D , C); + } + return C + (D - C) * (val - A) / (B - A); +} + +static Sint16 ApplyStickCalibrationCentered(SDL_DriverSwitch_Context *ctx, int nStick, int nAxis, Sint16 sRawValue, Sint16 sCenter) +{ + sRawValue -= sCenter; + + if (sRawValue > ctx->m_StickExtents[nStick].axis[nAxis].sMax) { + ctx->m_StickExtents[nStick].axis[nAxis].sMax = sRawValue; + } + if (sRawValue < ctx->m_StickExtents[nStick].axis[nAxis].sMin) { + ctx->m_StickExtents[nStick].axis[nAxis].sMin = sRawValue; + } + + if (sRawValue > 0) { + return (Sint16)(RemapVal(sRawValue, 0, ctx->m_StickExtents[nStick].axis[nAxis].sMax, 0, SDL_MAX_SINT16)); + } else { + return (Sint16)(RemapVal(sRawValue, ctx->m_StickExtents[nStick].axis[nAxis].sMin, 0, SDL_MIN_SINT16, 0)); + } +} + +static Sint16 ApplyStickCalibration(SDL_DriverSwitch_Context *ctx, int nStick, int nAxis, Sint16 sRawValue) +{ + return ApplyStickCalibrationCentered(ctx, nStick, nAxis, sRawValue, ctx->m_StickCalData[nStick].axis[nAxis].sCenter); +} + +static SDL_bool +HIDAPI_DriverSwitch_Init(SDL_Joystick *joystick, hid_device *dev, Uint16 vendor_id, Uint16 product_id, void **context) +{ + SDL_DriverSwitch_Context *ctx; + Uint8 input_mode; + + ctx = (SDL_DriverSwitch_Context *)SDL_calloc(1, sizeof(*ctx)); + if (!ctx) { + SDL_OutOfMemory(); + return SDL_FALSE; + } + ctx->dev = dev; + + *context = ctx; + + /* Initialize rumble data */ + SetNeutralRumble(&ctx->m_RumblePacket.rumbleData[0]); + SetNeutralRumble(&ctx->m_RumblePacket.rumbleData[1]); + + /* Try setting up USB mode, and if that fails we're using Bluetooth */ + if (!BTrySetupUSB(ctx)) { + ctx->m_bIsUsingBluetooth = SDL_TRUE; + } + + if (!LoadStickCalibration(ctx)) { + SDL_SetError("Couldn't load stick calibration"); + SDL_free(ctx); + return SDL_FALSE; + } + + if (!SetVibrationEnabled(ctx, 1)) { + SDL_SetError("Couldn't enable vibration"); + SDL_free(ctx); + return SDL_FALSE; + } + + /* Set the desired input mode */ + if (ctx->m_bIsUsingBluetooth) { + input_mode = k_eSwitchInputReportIDs_SimpleControllerState; + } else { + input_mode = k_eSwitchInputReportIDs_FullControllerState; + } + if (!SetInputMode(ctx, input_mode)) { + SDL_SetError("Couldn't set input mode"); + SDL_free(ctx); + return SDL_FALSE; + } + + /* Start sending USB reports */ + if (!ctx->m_bIsUsingBluetooth) { + /* ForceUSB doesn't generate an ACK, so don't wait for a reply */ + if (!WriteProprietary(ctx, k_eSwitchProprietaryCommandIDs_ForceUSB, NULL, 0, SDL_FALSE)) { + SDL_SetError("Couldn't start USB reports"); + SDL_free(ctx); + return SDL_FALSE; + } + } + + /* Set the LED state */ + SetHomeLED(ctx, 100); + SetSlotLED(ctx, (joystick->instance_id % 4)); + + /* Initialize the joystick capabilities */ + joystick->nbuttons = SDL_CONTROLLER_BUTTON_MAX; + joystick->naxes = SDL_CONTROLLER_AXIS_MAX; + joystick->epowerlevel = SDL_JOYSTICK_POWER_WIRED; + + return SDL_TRUE; +} + +static int +HIDAPI_DriverSwitch_Rumble(SDL_Joystick *joystick, hid_device *dev, void *context, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + SDL_DriverSwitch_Context *ctx = (SDL_DriverSwitch_Context *)context; + + /* Experimentally determined rumble values. These will only matter on some controllers as tested ones + * seem to disregard these and just use any non-zero rumble values as a binary flag for constant rumble + * + * More information about these values can be found here: + * https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/rumble_data_table.md + */ + const Uint16 k_usHighFreq = 0x0074; + const Uint8 k_ucHighFreqAmp = 0xBE; + const Uint8 k_ucLowFreq = 0x3D; + const Uint16 k_usLowFreqAmp = 0x806F; + + if (low_frequency_rumble) { + EncodeRumble(&ctx->m_RumblePacket.rumbleData[0], k_usHighFreq, k_ucHighFreqAmp, k_ucLowFreq, k_usLowFreqAmp); + } else { + SetNeutralRumble(&ctx->m_RumblePacket.rumbleData[0]); + } + + if (high_frequency_rumble) { + EncodeRumble(&ctx->m_RumblePacket.rumbleData[1], k_usHighFreq, k_ucHighFreqAmp, k_ucLowFreq, k_usLowFreqAmp); + } else { + SetNeutralRumble(&ctx->m_RumblePacket.rumbleData[1]); + } + + if (!WriteRumble(ctx)) { + SDL_SetError("Couldn't send rumble packet"); + return -1; + } + + if ((low_frequency_rumble || high_frequency_rumble) && duration_ms) { + ctx->m_nRumbleExpiration = SDL_GetTicks() + duration_ms; + } else { + ctx->m_nRumbleExpiration = 0; + } + return 0; +} + +static void HandleSimpleControllerState(SDL_Joystick *joystick, SDL_DriverSwitch_Context *ctx, SwitchSimpleStatePacket_t *packet) +{ + /* 0x8000 is the neutral value for all joystick axes */ + const Uint16 usJoystickCenter = 0x8000; + Sint16 axis; + + if (packet->rgucButtons[0] != ctx->m_lastSimpleState.rgucButtons[0]) { + Uint8 data = packet->rgucButtons[0]; + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_A, (data & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_B, (data & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_X, (data & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_Y, (data & 0x08) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSHOULDER, (data & 0x10) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSHOULDER, (data & 0x20) ? SDL_PRESSED : SDL_RELEASED); + + axis = (data & 0x40) ? 32767 : -32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERLEFT, axis); + + axis = (data & 0x80) ? 32767 : -32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERRIGHT, axis); + } + + if (packet->rgucButtons[1] != ctx->m_lastSimpleState.rgucButtons[1]) { + Uint8 data = packet->rgucButtons[1]; + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_BACK, (data & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_START, (data & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSTICK, (data & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSTICK, (data & 0x08) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_GUIDE, (data & 0x10) ? SDL_PRESSED : SDL_RELEASED); + } + + if (packet->ucStickHat != ctx->m_lastSimpleState.ucStickHat) { + SDL_bool dpad_up = SDL_FALSE; + SDL_bool dpad_down = SDL_FALSE; + SDL_bool dpad_left = SDL_FALSE; + SDL_bool dpad_right = SDL_FALSE; + + switch (packet->ucStickHat) { + case 0: + dpad_up = SDL_TRUE; + break; + case 1: + dpad_up = SDL_TRUE; + dpad_right = SDL_TRUE; + break; + case 2: + dpad_right = SDL_TRUE; + break; + case 3: + dpad_right = SDL_TRUE; + dpad_down = SDL_TRUE; + break; + case 4: + dpad_down = SDL_TRUE; + break; + case 5: + dpad_left = SDL_TRUE; + dpad_down = SDL_TRUE; + break; + case 6: + dpad_left = SDL_TRUE; + break; + case 7: + dpad_up = SDL_TRUE; + dpad_left = SDL_TRUE; + break; + default: + break; + } + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_DOWN, dpad_down); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_UP, dpad_up); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_RIGHT, dpad_right); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_LEFT, dpad_left); + } + + axis = ApplyStickCalibrationCentered(ctx, 0, 0, packet->sJoystickLeft[0], (Sint16)usJoystickCenter); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTX, axis); + + axis = ApplyStickCalibrationCentered(ctx, 0, 1, packet->sJoystickLeft[1], (Sint16)usJoystickCenter); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTY, axis); + + axis = ApplyStickCalibrationCentered(ctx, 1, 0, packet->sJoystickRight[0], (Sint16)usJoystickCenter); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTX, axis); + + axis = ApplyStickCalibrationCentered(ctx, 1, 1, packet->sJoystickRight[1], (Sint16)usJoystickCenter); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTY, axis); + + ctx->m_lastSimpleState = *packet; +} + +static void HandleFullControllerState(SDL_Joystick *joystick, SDL_DriverSwitch_Context *ctx, SwitchStatePacket_t *packet) +{ + Sint16 axis; + + if (packet->controllerState.rgucButtons[0] != ctx->m_lastFullState.controllerState.rgucButtons[0]) { + Uint8 data = packet->controllerState.rgucButtons[0]; + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_X, (data & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_Y, (data & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_A, (data & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_B, (data & 0x08) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSHOULDER, (data & 0x40) ? SDL_PRESSED : SDL_RELEASED); + axis = (data & 0x80) ? 32767 : -32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERRIGHT, axis); + } + + if (packet->controllerState.rgucButtons[1] != ctx->m_lastFullState.controllerState.rgucButtons[1]) { + Uint8 data = packet->controllerState.rgucButtons[1]; + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_BACK, (data & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_START, (data & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSTICK, (data & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSTICK, (data & 0x08) ? SDL_PRESSED : SDL_RELEASED); + + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_GUIDE, (data & 0x10) ? SDL_PRESSED : SDL_RELEASED); + } + + if (packet->controllerState.rgucButtons[2] != ctx->m_lastFullState.controllerState.rgucButtons[2]) { + Uint8 data = packet->controllerState.rgucButtons[2]; + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_DOWN, (data & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_UP, (data & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_RIGHT, (data & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_LEFT, (data & 0x08) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSHOULDER, (data & 0x40) ? SDL_PRESSED : SDL_RELEASED); + axis = (data & 0x80) ? 32767 : -32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERLEFT, axis); + } + + axis = packet->controllerState.rgucJoystickLeft[0] | ((packet->controllerState.rgucJoystickLeft[1] & 0xF) << 8); + axis = ApplyStickCalibration(ctx, 0, 0, axis); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTX, axis); + + axis = ((packet->controllerState.rgucJoystickLeft[1] & 0xF0) >> 4) | (packet->controllerState.rgucJoystickLeft[2] << 4); + axis = ApplyStickCalibration(ctx, 0, 1, axis); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTY, ~axis); + + axis = packet->controllerState.rgucJoystickRight[0] | ((packet->controllerState.rgucJoystickRight[1] & 0xF) << 8); + axis = ApplyStickCalibration(ctx, 1, 0, axis); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTX, axis); + + axis = ((packet->controllerState.rgucJoystickRight[1] & 0xF0) >> 4) | (packet->controllerState.rgucJoystickRight[2] << 4); + axis = ApplyStickCalibration(ctx, 1, 1, axis); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTY, ~axis); + + /* High nibble of battery/connection byte is battery level, low nibble is connection status + * LSB of connection nibble is USB/Switch connection status + */ + if (packet->controllerState.ucBatteryAndConnection & 0x1) { + joystick->epowerlevel = SDL_JOYSTICK_POWER_WIRED; + } else { + /* LSB of the battery nibble is used to report charging. + * The battery level is reported from 0(empty)-8(full) + */ + int level = (packet->controllerState.ucBatteryAndConnection & 0xE0) >> 4; + if (level == 0) { + joystick->epowerlevel = SDL_JOYSTICK_POWER_EMPTY; + } else if (level <= 2) { + joystick->epowerlevel = SDL_JOYSTICK_POWER_LOW; + } else if (level <= 6) { + joystick->epowerlevel = SDL_JOYSTICK_POWER_MEDIUM; + } else { + joystick->epowerlevel = SDL_JOYSTICK_POWER_FULL; + } + } + + ctx->m_lastFullState = *packet; +} + +static SDL_bool +HIDAPI_DriverSwitch_Update(SDL_Joystick *joystick, hid_device *dev, void *context) +{ + SDL_DriverSwitch_Context *ctx = (SDL_DriverSwitch_Context *)context; + int size; + + while ((size = ReadInput(ctx)) > 0) { + switch (ctx->m_rgucReadBuffer[0]) { + case k_eSwitchInputReportIDs_SimpleControllerState: + HandleSimpleControllerState(joystick, ctx, (SwitchSimpleStatePacket_t *)&ctx->m_rgucReadBuffer[1]); + break; + case k_eSwitchInputReportIDs_FullControllerState: + HandleFullControllerState(joystick, ctx, (SwitchStatePacket_t *)&ctx->m_rgucReadBuffer[1]); + break; + default: + break; + } + } + + if (ctx->m_nRumbleExpiration) { + Uint32 now = SDL_GetTicks(); + if (SDL_TICKS_PASSED(now, ctx->m_nRumbleExpiration)) { + HIDAPI_DriverSwitch_Rumble(joystick, dev, context, 0, 0, 0); + } + } + + return (size >= 0); +} + +static void +HIDAPI_DriverSwitch_Quit(SDL_Joystick *joystick, hid_device *dev, void *context) +{ + SDL_DriverSwitch_Context *ctx = (SDL_DriverSwitch_Context *)context; + + /* Restore simple input mode for other applications */ + SetInputMode(ctx, k_eSwitchInputReportIDs_SimpleControllerState); + + SDL_free(context); +} + +SDL_HIDAPI_DeviceDriver SDL_HIDAPI_DriverSwitch = +{ + SDL_HINT_JOYSTICK_HIDAPI_SWITCH, + SDL_TRUE, + HIDAPI_DriverSwitch_IsSupportedDevice, + HIDAPI_DriverSwitch_GetDeviceName, + HIDAPI_DriverSwitch_Init, + HIDAPI_DriverSwitch_Rumble, + HIDAPI_DriverSwitch_Update, + HIDAPI_DriverSwitch_Quit +}; + +#endif /* SDL_JOYSTICK_HIDAPI_SWITCH */ + +#endif /* SDL_JOYSTICK_HIDAPI */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_xbox360.c b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_xbox360.c new file mode 100644 index 000000000..84c63c667 --- /dev/null +++ b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_xbox360.c @@ -0,0 +1,787 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "../../SDL_internal.h" + +#ifdef SDL_JOYSTICK_HIDAPI + +#include "SDL_hints.h" +#include "SDL_log.h" +#include "SDL_events.h" +#include "SDL_timer.h" +#include "SDL_joystick.h" +#include "SDL_gamecontroller.h" +#include "../SDL_sysjoystick.h" +#include "SDL_hidapijoystick_c.h" + + +#ifdef SDL_JOYSTICK_HIDAPI_XBOX360 + +#ifdef __WIN32__ +#define SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT +/* This requires the Windows 10 SDK to build */ +/*#define SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT*/ +#endif + +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT +#include "../../core/windows/SDL_xinput.h" +#endif + +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT +#include "../../core/windows/SDL_windows.h" +#define COBJMACROS +#include "windows.gaming.input.h" +#endif + +#define USB_PACKET_LENGTH 64 + + +typedef struct { + Uint8 last_state[USB_PACKET_LENGTH]; + Uint32 rumble_expiration; +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT + SDL_bool xinput_enabled; + Uint8 xinput_slot; +#endif +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT + SDL_bool coinitialized; + __x_ABI_CWindows_CGaming_CInput_CIGamepadStatics *gamepad_statics; + __x_ABI_CWindows_CGaming_CInput_CIGamepad *gamepad; + struct __x_ABI_CWindows_CGaming_CInput_CGamepadVibration vibration; +#endif +} SDL_DriverXbox360_Context; + + +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT +static Uint8 xinput_slots; + +static void +HIDAPI_DriverXbox360_MarkXInputSlotUsed(Uint8 xinput_slot) +{ + if (xinput_slot != XUSER_INDEX_ANY) { + xinput_slots |= (0x01 << xinput_slot); + } +} + +static void +HIDAPI_DriverXbox360_MarkXInputSlotFree(Uint8 xinput_slot) +{ + if (xinput_slot != XUSER_INDEX_ANY) { + xinput_slots &= ~(0x01 << xinput_slot); + } +} + +static SDL_bool +HIDAPI_DriverXbox360_MissingXInputSlot() +{ + return xinput_slots != 0x0F; +} + +static Uint8 +HIDAPI_DriverXbox360_GuessXInputSlot(WORD wButtons) +{ + DWORD user_index; + int match_count; + Uint8 match_slot; + + if (!XINPUTGETSTATE) { + return XUSER_INDEX_ANY; + } + + match_count = 0; + for (user_index = 0; user_index < XUSER_MAX_COUNT; ++user_index) { + XINPUT_STATE_EX xinput_state; + + if (XINPUTGETSTATE(user_index, &xinput_state) == ERROR_SUCCESS) { + if (xinput_state.Gamepad.wButtons == wButtons) { + ++match_count; + match_slot = (Uint8)user_index; + } + } + } + if (match_count == 1) { + return match_slot; + } + return XUSER_INDEX_ANY; +} + +#endif /* SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT */ + +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT + +static void +HIDAPI_DriverXbox360_InitWindowsGamingInput(SDL_DriverXbox360_Context *ctx) +{ + /* I think this takes care of RoInitialize() in a way that is compatible with the rest of SDL */ + if (FAILED(WIN_CoInitialize())) { + return; + } + ctx->coinitialized = SDL_TRUE; + + { + static const IID SDL_IID_IGamepadStatics = { 0x8BBCE529, 0xD49C, 0x39E9, { 0x95, 0x60, 0xE4, 0x7D, 0xDE, 0x96, 0xB7, 0xC8 } }; + HRESULT hr; + HMODULE hModule = LoadLibraryA("combase.dll"); + if (hModule != NULL) { + typedef HRESULT (WINAPI *WindowsCreateString_t)(PCNZWCH sourceString, UINT32 length, HSTRING* string); + typedef HRESULT (WINAPI *WindowsDeleteString_t)(HSTRING string); + typedef HRESULT (WINAPI *RoGetActivationFactory_t)(HSTRING activatableClassId, REFIID iid, void** factory); + + WindowsCreateString_t WindowsCreateStringFunc = (WindowsCreateString_t)GetProcAddress(hModule, "WindowsCreateString"); + WindowsDeleteString_t WindowsDeleteStringFunc = (WindowsDeleteString_t)GetProcAddress(hModule, "WindowsDeleteString"); + RoGetActivationFactory_t RoGetActivationFactoryFunc = (RoGetActivationFactory_t)GetProcAddress(hModule, "RoGetActivationFactory"); + if (WindowsCreateStringFunc && WindowsDeleteStringFunc && RoGetActivationFactoryFunc) { + LPTSTR pNamespace = L"Windows.Gaming.Input.Gamepad"; + HSTRING hNamespaceString; + + hr = WindowsCreateStringFunc(pNamespace, SDL_wcslen(pNamespace), &hNamespaceString); + if (SUCCEEDED(hr)) { + RoGetActivationFactoryFunc(hNamespaceString, &SDL_IID_IGamepadStatics, &ctx->gamepad_statics); + WindowsDeleteStringFunc(hNamespaceString); + } + } + FreeLibrary(hModule); + } + } +} + +static Uint8 +HIDAPI_DriverXbox360_GetGamepadButtonsForMatch(__x_ABI_CWindows_CGaming_CInput_CIGamepad *gamepad) +{ + HRESULT hr; + struct __x_ABI_CWindows_CGaming_CInput_CGamepadReading state; + Uint8 buttons = 0; + + hr = __x_ABI_CWindows_CGaming_CInput_CIGamepad_GetCurrentReading(gamepad, &state); + if (SUCCEEDED(hr)) { + if (state.Buttons & GamepadButtons_A) { + buttons |= (1 << SDL_CONTROLLER_BUTTON_A); + } + if (state.Buttons & GamepadButtons_B) { + buttons |= (1 << SDL_CONTROLLER_BUTTON_B); + } + if (state.Buttons & GamepadButtons_X) { + buttons |= (1 << SDL_CONTROLLER_BUTTON_X); + } + if (state.Buttons & GamepadButtons_Y) { + buttons |= (1 << SDL_CONTROLLER_BUTTON_Y); + } + } + return buttons; +} + +static void +HIDAPI_DriverXbox360_GuessGamepad(SDL_DriverXbox360_Context *ctx, Uint8 buttons) +{ + HRESULT hr; + __FIVectorView_1_Windows__CGaming__CInput__CGamepad *gamepads; + + hr = __x_ABI_CWindows_CGaming_CInput_CIGamepadStatics_get_Gamepads(ctx->gamepad_statics, &gamepads); + if (SUCCEEDED(hr)) { + unsigned int i, num_gamepads; + + hr = __FIVectorView_1_Windows__CGaming__CInput__CGamepad_get_Size(gamepads, &num_gamepads); + if (SUCCEEDED(hr)) { + int match_count; + unsigned int match_slot; + + match_count = 0; + for (i = 0; i < num_gamepads; ++i) { + __x_ABI_CWindows_CGaming_CInput_CIGamepad *gamepad; + + hr = __FIVectorView_1_Windows__CGaming__CInput__CGamepad_GetAt(gamepads, i, &gamepad); + if (SUCCEEDED(hr)) { + Uint8 gamepad_buttons = HIDAPI_DriverXbox360_GetGamepadButtonsForMatch(gamepad); + if (buttons == gamepad_buttons) { + ++match_count; + match_slot = i; + } + __x_ABI_CWindows_CGaming_CInput_CIGamepad_Release(gamepad); + } + } + if (match_count == 1) { + hr = __FIVectorView_1_Windows__CGaming__CInput__CGamepad_GetAt(gamepads, match_slot, &ctx->gamepad); + if (SUCCEEDED(hr)) { + } + } + } + __FIVectorView_1_Windows__CGaming__CInput__CGamepad_Release(gamepads); + } +} + +static void +HIDAPI_DriverXbox360_QuitWindowsGamingInput(SDL_DriverXbox360_Context *ctx) +{ + if (ctx->gamepad_statics) { + __x_ABI_CWindows_CGaming_CInput_CIGamepadStatics_Release(ctx->gamepad_statics); + ctx->gamepad_statics = NULL; + } + if (ctx->gamepad) { + __x_ABI_CWindows_CGaming_CInput_CIGamepad_Release(ctx->gamepad); + ctx->gamepad = NULL; + } + + if (ctx->coinitialized) { + WIN_CoUninitialize(); + ctx->coinitialized = SDL_FALSE; + } +} + +#endif /* SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT */ + +static SDL_bool +HIDAPI_DriverXbox360_IsSupportedDevice(Uint16 vendor_id, Uint16 product_id, Uint16 version, int interface_number) +{ +#if defined(__MACOSX__) || defined(__WIN32__) + if (vendor_id == 0x045e && product_id == 0x028e && version == 1) { + /* This is the Steam Virtual Gamepad, which isn't supported by this driver */ + return SDL_FALSE; + } + return SDL_IsJoystickXbox360(vendor_id, product_id) || SDL_IsJoystickXboxOne(vendor_id, product_id); +#else + return SDL_IsJoystickXbox360(vendor_id, product_id); +#endif +} + +static const char * +HIDAPI_DriverXbox360_GetDeviceName(Uint16 vendor_id, Uint16 product_id) +{ + return HIDAPI_XboxControllerName(vendor_id, product_id); +} + +static SDL_bool SetSlotLED(hid_device *dev, Uint8 slot) +{ + const Uint8 led_packet[] = { 0x01, 0x03, (2 + slot) }; + + if (hid_write(dev, led_packet, sizeof(led_packet)) != sizeof(led_packet)) { + return SDL_FALSE; + } + return SDL_TRUE; +} + +static SDL_bool +HIDAPI_DriverXbox360_Init(SDL_Joystick *joystick, hid_device *dev, Uint16 vendor_id, Uint16 product_id, void **context) +{ + SDL_DriverXbox360_Context *ctx; + + ctx = (SDL_DriverXbox360_Context *)SDL_calloc(1, sizeof(*ctx)); + if (!ctx) { + SDL_OutOfMemory(); + return SDL_FALSE; + } +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT + ctx->xinput_enabled = SDL_GetHintBoolean(SDL_HINT_XINPUT_ENABLED, SDL_TRUE); + if (ctx->xinput_enabled && WIN_LoadXInputDLL() < 0) { + ctx->xinput_enabled = SDL_FALSE; + } + ctx->xinput_slot = XUSER_INDEX_ANY; +#endif +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT + HIDAPI_DriverXbox360_InitWindowsGamingInput(ctx); +#endif + *context = ctx; + + /* Set the controller LED */ + SetSlotLED(dev, (joystick->instance_id % 4)); + + /* Initialize the joystick capabilities */ + joystick->nbuttons = SDL_CONTROLLER_BUTTON_MAX; + joystick->naxes = SDL_CONTROLLER_AXIS_MAX; + joystick->epowerlevel = SDL_JOYSTICK_POWER_WIRED; + + return SDL_TRUE; +} + +static int +HIDAPI_DriverXbox360_Rumble(SDL_Joystick *joystick, hid_device *dev, void *context, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + SDL_DriverXbox360_Context *ctx = (SDL_DriverXbox360_Context *)context; + +#ifdef __WIN32__ + SDL_bool rumbled = SDL_FALSE; + +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT + if (!rumbled && ctx->gamepad) { + HRESULT hr; + + ctx->vibration.LeftMotor = (DOUBLE)low_frequency_rumble / SDL_MAX_UINT16; + ctx->vibration.RightMotor = (DOUBLE)high_frequency_rumble / SDL_MAX_UINT16; + hr = __x_ABI_CWindows_CGaming_CInput_CIGamepad_put_Vibration(ctx->gamepad, ctx->vibration); + if (SUCCEEDED(hr)) { + rumbled = SDL_TRUE; + } + } +#endif + +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT + if (!rumbled && ctx->xinput_slot != XUSER_INDEX_ANY) { + XINPUT_VIBRATION XVibration; + + if (!XINPUTSETSTATE) { + return SDL_Unsupported(); + } + + XVibration.wLeftMotorSpeed = low_frequency_rumble; + XVibration.wRightMotorSpeed = high_frequency_rumble; + if (XINPUTSETSTATE(ctx->xinput_slot, &XVibration) == ERROR_SUCCESS) { + rumbled = SDL_TRUE; + } else { + return SDL_SetError("XInputSetState() failed"); + } + } +#endif /* SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT */ + +#else /* !__WIN32__ */ + +#ifdef __MACOSX__ + /* On Mac OS X the 360Controller driver uses this short report, + and we need to prefix it with a magic token so hidapi passes it through untouched + */ + Uint8 rumble_packet[] = { 'M', 'A', 'G', 'I', 'C', '0', 0x00, 0x04, 0x00, 0x00 }; + + rumble_packet[6+2] = (low_frequency_rumble >> 8); + rumble_packet[6+3] = (high_frequency_rumble >> 8); +#else + Uint8 rumble_packet[] = { 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + + rumble_packet[3] = (low_frequency_rumble >> 8); + rumble_packet[4] = (high_frequency_rumble >> 8); +#endif + + if (hid_write(dev, rumble_packet, sizeof(rumble_packet)) != sizeof(rumble_packet)) { + return SDL_SetError("Couldn't send rumble packet"); + } +#endif /* __WIN32__ */ + + if ((low_frequency_rumble || high_frequency_rumble) && duration_ms) { + ctx->rumble_expiration = SDL_GetTicks() + duration_ms; + } else { + ctx->rumble_expiration = 0; + } + return 0; +} + +#ifdef __WIN32__ + /* This is the packet format for Xbox 360 and Xbox One controllers on Windows, + however with this interface there is no rumble support, no guide button, + and the left and right triggers are tied together as a single axis. + + We use XInput and Windows.Gaming.Input to make up for these shortcomings. + */ +static void +HIDAPI_DriverXbox360_HandleStatePacket(SDL_Joystick *joystick, hid_device *dev, SDL_DriverXbox360_Context *ctx, Uint8 *data, int size) +{ + Sint16 axis; + SDL_bool has_trigger_data = SDL_FALSE; + + if (ctx->last_state[10] != data[10]) { + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_A, (data[10] & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_B, (data[10] & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_X, (data[10] & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_Y, (data[10] & 0x08) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSHOULDER, (data[10] & 0x10) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSHOULDER, (data[10] & 0x20) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_BACK, (data[10] & 0x40) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_START, (data[10] & 0x80) ? SDL_PRESSED : SDL_RELEASED); + } + + if (ctx->last_state[11] != data[11]) { + SDL_bool dpad_up = SDL_FALSE; + SDL_bool dpad_down = SDL_FALSE; + SDL_bool dpad_left = SDL_FALSE; + SDL_bool dpad_right = SDL_FALSE; + + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSTICK, (data[11] & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSTICK, (data[11] & 0x02) ? SDL_PRESSED : SDL_RELEASED); + + switch (data[11] & 0x3C) { + case 4: + dpad_up = SDL_TRUE; + break; + case 8: + dpad_up = SDL_TRUE; + dpad_right = SDL_TRUE; + break; + case 12: + dpad_right = SDL_TRUE; + break; + case 16: + dpad_right = SDL_TRUE; + dpad_down = SDL_TRUE; + break; + case 20: + dpad_down = SDL_TRUE; + break; + case 24: + dpad_left = SDL_TRUE; + dpad_down = SDL_TRUE; + break; + case 28: + dpad_left = SDL_TRUE; + break; + case 32: + dpad_up = SDL_TRUE; + dpad_left = SDL_TRUE; + break; + default: + break; + } + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_DOWN, dpad_down); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_UP, dpad_up); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_RIGHT, dpad_right); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_LEFT, dpad_left); + } + + axis = (int)*(Uint16*)(&data[0]) - 0x8000; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTX, axis); + axis = (int)*(Uint16*)(&data[2]) - 0x8000; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTY, axis); + axis = (int)*(Uint16*)(&data[4]) - 0x8000; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTX, axis); + axis = (int)*(Uint16*)(&data[6]) - 0x8000; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTY, axis); + +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT + if (ctx->gamepad_statics && !ctx->gamepad) { + Uint8 buttons = 0; + + if (data[10] & 0x01) { + buttons |= (1 << SDL_CONTROLLER_BUTTON_A); + } + if (data[10] & 0x02) { + buttons |= (1 << SDL_CONTROLLER_BUTTON_B); + } + if (data[10] & 0x04) { + buttons |= (1 << SDL_CONTROLLER_BUTTON_X); + } + if (data[10] & 0x08) { + buttons |= (1 << SDL_CONTROLLER_BUTTON_Y); + } + if (buttons != 0) { + HIDAPI_DriverXbox360_GuessGamepad(ctx, buttons); + } + } + + if (ctx->gamepad) { + HRESULT hr; + struct __x_ABI_CWindows_CGaming_CInput_CGamepadReading state; + + hr = __x_ABI_CWindows_CGaming_CInput_CIGamepad_GetCurrentReading(ctx->gamepad, &state); + if (SUCCEEDED(hr)) { + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_GUIDE, (state.Buttons & 0x40000000) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERLEFT, ((int)(state.LeftTrigger * SDL_MAX_UINT16)) - 32768); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERRIGHT, ((int)(state.RightTrigger * SDL_MAX_UINT16)) - 32768); + has_trigger_data = SDL_TRUE; + } + } +#endif /* SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT */ + +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT + if (ctx->xinput_enabled) { + if (ctx->xinput_slot == XUSER_INDEX_ANY && HIDAPI_DriverXbox360_MissingXInputSlot()) { + WORD wButtons = 0; + + if (data[10] & 0x01) { + wButtons |= XINPUT_GAMEPAD_A; + } + if (data[10] & 0x02) { + wButtons |= XINPUT_GAMEPAD_B; + } + if (data[10] & 0x04) { + wButtons |= XINPUT_GAMEPAD_X; + } + if (data[10] & 0x08) { + wButtons |= XINPUT_GAMEPAD_Y; + } + if (wButtons != 0) { + Uint8 xinput_slot = HIDAPI_DriverXbox360_GuessXInputSlot(wButtons); + if (xinput_slot != XUSER_INDEX_ANY) { + HIDAPI_DriverXbox360_MarkXInputSlotUsed(xinput_slot); + ctx->xinput_slot = xinput_slot; + } + } + } + + if (!has_trigger_data && ctx->xinput_slot != XUSER_INDEX_ANY) { + XINPUT_STATE_EX xinput_state; + + if (XINPUTGETSTATE(ctx->xinput_slot, &xinput_state) == ERROR_SUCCESS) { + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_GUIDE, (xinput_state.Gamepad.wButtons & XINPUT_GAMEPAD_GUIDE) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERLEFT, ((int)xinput_state.Gamepad.bLeftTrigger * 257) - 32768); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERRIGHT, ((int)xinput_state.Gamepad.bRightTrigger * 257) - 32768); + has_trigger_data = SDL_TRUE; + } + } + } +#endif /* SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT */ + + if (!has_trigger_data) { + axis = (data[9] * 257) - 32768; + if (data[9] < 0x80) { + axis = -axis * 2 - 32769; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERRIGHT, axis); + } else if (data[9] > 0x80) { + axis = axis * 2 - 32767; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERLEFT, axis); + } else { + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERLEFT, SDL_MIN_SINT16); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERRIGHT, SDL_MIN_SINT16); + } + } + + SDL_memcpy(ctx->last_state, data, SDL_min(size, sizeof(ctx->last_state))); +} +#else + +static void +HIDAPI_DriverXbox360_HandleStatePacket(SDL_Joystick *joystick, hid_device *dev, SDL_DriverXbox360_Context *ctx, Uint8 *data, int size) +{ + Sint16 axis; +#ifdef __MACOSX__ + const SDL_bool invert_y_axes = SDL_FALSE; +#else + const SDL_bool invert_y_axes = SDL_TRUE; +#endif + + if (ctx->last_state[2] != data[2]) { + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_UP, (data[2] & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_DOWN, (data[2] & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_LEFT, (data[2] & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_RIGHT, (data[2] & 0x08) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_START, (data[2] & 0x10) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_BACK, (data[2] & 0x20) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSTICK, (data[2] & 0x40) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSTICK, (data[2] & 0x80) ? SDL_PRESSED : SDL_RELEASED); + } + + if (ctx->last_state[3] != data[3]) { + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSHOULDER, (data[3] & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSHOULDER, (data[3] & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_GUIDE, (data[3] & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_A, (data[3] & 0x10) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_B, (data[3] & 0x20) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_X, (data[3] & 0x40) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_Y, (data[3] & 0x80) ? SDL_PRESSED : SDL_RELEASED); + } + + axis = ((int)data[4] * 257) - 32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERLEFT, axis); + axis = ((int)data[5] * 257) - 32768; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERRIGHT, axis); + axis = *(Sint16*)(&data[6]); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTX, axis); + axis = *(Sint16*)(&data[8]); + if (invert_y_axes) { + axis = ~axis; + } + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTY, axis); + axis = *(Sint16*)(&data[10]); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTX, axis); + axis = *(Sint16*)(&data[12]); + if (invert_y_axes) { + axis = ~axis; + } + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTY, axis); + + SDL_memcpy(ctx->last_state, data, SDL_min(size, sizeof(ctx->last_state))); +} +#endif /* __WIN32__ */ + +#ifdef __MACOSX__ +static void +HIDAPI_DriverXboxOneS_HandleStatePacket(SDL_Joystick *joystick, hid_device *dev, SDL_DriverXbox360_Context *ctx, Uint8 *data, int size) +{ + Sint16 axis; + + if (ctx->last_state[14] != data[14]) { + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_A, (data[14] & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_B, (data[14] & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_X, (data[14] & 0x08) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_Y, (data[14] & 0x10) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSHOULDER, (data[14] & 0x40) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSHOULDER, (data[14] & 0x80) ? SDL_PRESSED : SDL_RELEASED); + } + + if (ctx->last_state[15] != data[15]) { + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_START, (data[15] & 0x08) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSTICK, (data[15] & 0x20) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSTICK, (data[15] & 0x40) ? SDL_PRESSED : SDL_RELEASED); + } + + if (ctx->last_state[16] != data[16]) { + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_BACK, (data[16] & 0x01) ? SDL_PRESSED : SDL_RELEASED); + } + + if (ctx->last_state[13] != data[13]) { + SDL_bool dpad_up = SDL_FALSE; + SDL_bool dpad_down = SDL_FALSE; + SDL_bool dpad_left = SDL_FALSE; + SDL_bool dpad_right = SDL_FALSE; + + switch (data[13]) { + case 1: + dpad_up = SDL_TRUE; + break; + case 2: + dpad_up = SDL_TRUE; + dpad_right = SDL_TRUE; + break; + case 3: + dpad_right = SDL_TRUE; + break; + case 4: + dpad_right = SDL_TRUE; + dpad_down = SDL_TRUE; + break; + case 5: + dpad_down = SDL_TRUE; + break; + case 6: + dpad_left = SDL_TRUE; + dpad_down = SDL_TRUE; + break; + case 7: + dpad_left = SDL_TRUE; + break; + case 8: + dpad_up = SDL_TRUE; + dpad_left = SDL_TRUE; + break; + default: + break; + } + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_DOWN, dpad_down); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_UP, dpad_up); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_RIGHT, dpad_right); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_LEFT, dpad_left); + } + + axis = (int)*(Uint16*)(&data[1]) - 0x8000; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTX, axis); + axis = (int)*(Uint16*)(&data[3]) - 0x8000; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTY, axis); + axis = (int)*(Uint16*)(&data[5]) - 0x8000; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTX, axis); + axis = (int)*(Uint16*)(&data[7]) - 0x8000; + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTY, axis); + + axis = ((int)*(Sint16*)(&data[9]) * 64) - 32768; + if (axis == 32704) { + axis = 32767; + } + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERLEFT, axis); + + axis = ((int)*(Sint16*)(&data[11]) * 64) - 32768; + if (axis == 32704) { + axis = 32767; + } + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERRIGHT, axis); + + SDL_memcpy(ctx->last_state, data, SDL_min(size, sizeof(ctx->last_state))); +} + +static void +HIDAPI_DriverXboxOneS_HandleGuidePacket(SDL_Joystick *joystick, hid_device *dev, SDL_DriverXbox360_Context *ctx, Uint8 *data, int size) +{ + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_GUIDE, (data[1] & 0x01) ? SDL_PRESSED : SDL_RELEASED); +} +#endif /* __MACOSX__ */ + +static SDL_bool +HIDAPI_DriverXbox360_Update(SDL_Joystick *joystick, hid_device *dev, void *context) +{ + SDL_DriverXbox360_Context *ctx = (SDL_DriverXbox360_Context *)context; + Uint8 data[USB_PACKET_LENGTH]; + int size; + + while ((size = hid_read_timeout(dev, data, sizeof(data), 0)) > 0) { +#ifdef __WIN32__ + HIDAPI_DriverXbox360_HandleStatePacket(joystick, dev, ctx, data, size); +#else + switch (data[0]) { + case 0x00: + HIDAPI_DriverXbox360_HandleStatePacket(joystick, dev, ctx, data, size); + break; +#ifdef __MACOSX__ + case 0x01: + HIDAPI_DriverXboxOneS_HandleStatePacket(joystick, dev, ctx, data, size); + break; + case 0x02: + HIDAPI_DriverXboxOneS_HandleGuidePacket(joystick, dev, ctx, data, size); + break; +#endif + default: +#ifdef DEBUG_JOYSTICK + SDL_Log("Unknown Xbox 360 packet, size = %d\n", size); + SDL_Log("%.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x\n", + data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], + data[8], data[9], data[10], data[11], data[12], data[13], data[14], data[15], data[16]); +#endif + break; + } +#endif /* __WIN32__ */ + } + + if (ctx->rumble_expiration) { + Uint32 now = SDL_GetTicks(); + if (SDL_TICKS_PASSED(now, ctx->rumble_expiration)) { + HIDAPI_DriverXbox360_Rumble(joystick, dev, context, 0, 0, 0); + } + } + + return (size >= 0); +} + +static void +HIDAPI_DriverXbox360_Quit(SDL_Joystick *joystick, hid_device *dev, void *context) +{ +#if defined(SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT) || defined(SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT) + SDL_DriverXbox360_Context *ctx = (SDL_DriverXbox360_Context *)context; +#endif + +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_XINPUT + if (ctx->xinput_enabled) { + HIDAPI_DriverXbox360_MarkXInputSlotFree(ctx->xinput_slot); + WIN_UnloadXInputDLL(); + } +#endif +#ifdef SDL_JOYSTICK_HIDAPI_WINDOWS_GAMING_INPUT + HIDAPI_DriverXbox360_InitWindowsGamingInput(ctx); +#endif + SDL_free(context); +} + +SDL_HIDAPI_DeviceDriver SDL_HIDAPI_DriverXbox360 = +{ + SDL_HINT_JOYSTICK_HIDAPI_XBOX, + SDL_TRUE, + HIDAPI_DriverXbox360_IsSupportedDevice, + HIDAPI_DriverXbox360_GetDeviceName, + HIDAPI_DriverXbox360_Init, + HIDAPI_DriverXbox360_Rumble, + HIDAPI_DriverXbox360_Update, + HIDAPI_DriverXbox360_Quit +}; + +#endif /* SDL_JOYSTICK_HIDAPI_XBOX360 */ + +#endif /* SDL_JOYSTICK_HIDAPI */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_xboxone.c b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_xboxone.c new file mode 100644 index 000000000..2cd593fb0 --- /dev/null +++ b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapi_xboxone.c @@ -0,0 +1,324 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "../../SDL_internal.h" + +#ifdef SDL_JOYSTICK_HIDAPI + +#include "SDL_hints.h" +#include "SDL_log.h" +#include "SDL_events.h" +#include "SDL_timer.h" +#include "SDL_joystick.h" +#include "SDL_gamecontroller.h" +#include "../SDL_sysjoystick.h" +#include "SDL_hidapijoystick_c.h" + + +#ifdef SDL_JOYSTICK_HIDAPI_XBOXONE + +#define USB_PACKET_LENGTH 64 + +/* + * This packet is required for all Xbox One pads with 2015 + * or later firmware installed (or present from the factory). + */ +static const Uint8 xboxone_fw2015_init[] = { + 0x05, 0x20, 0x00, 0x01, 0x00 +}; + +/* + * This packet is required for the Titanfall 2 Xbox One pads + * (0x0e6f:0x0165) to finish initialization and for Hori pads + * (0x0f0d:0x0067) to make the analog sticks work. + */ +static const Uint8 xboxone_hori_init[] = { + 0x01, 0x20, 0x00, 0x09, 0x00, 0x04, 0x20, 0x3a, + 0x00, 0x00, 0x00, 0x80, 0x00 +}; + +/* + * This packet is required for some of the PDP pads to start + * sending input reports. These pads include: (0x0e6f:0x02ab), + * (0x0e6f:0x02a4). + */ +static const Uint8 xboxone_pdp_init1[] = { + 0x0a, 0x20, 0x00, 0x03, 0x00, 0x01, 0x14 +}; + +/* + * This packet is required for some of the PDP pads to start + * sending input reports. These pads include: (0x0e6f:0x02ab), + * (0x0e6f:0x02a4). + */ +static const Uint8 xboxone_pdp_init2[] = { + 0x06, 0x20, 0x00, 0x02, 0x01, 0x00 +}; + +/* + * A specific rumble packet is required for some PowerA pads to start + * sending input reports. One of those pads is (0x24c6:0x543a). + */ +static const Uint8 xboxone_rumblebegin_init[] = { + 0x09, 0x00, 0x00, 0x09, 0x00, 0x0F, 0x00, 0x00, + 0x1D, 0x1D, 0xFF, 0x00, 0x00 +}; + +/* + * A rumble packet with zero FF intensity will immediately + * terminate the rumbling required to init PowerA pads. + * This should happen fast enough that the motors don't + * spin up to enough speed to actually vibrate the gamepad. + */ +static const Uint8 xboxone_rumbleend_init[] = { + 0x09, 0x00, 0x00, 0x09, 0x00, 0x0F, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00 +}; + +/* + * This specifies the selection of init packets that a gamepad + * will be sent on init *and* the order in which they will be + * sent. The correct sequence number will be added when the + * packet is going to be sent. + */ +typedef struct { + Uint16 vendor_id; + Uint16 product_id; + const Uint8 *data; + int size; +} SDL_DriverXboxOne_InitPacket; + +static const SDL_DriverXboxOne_InitPacket xboxone_init_packets[] = { + { 0x0e6f, 0x0165, xboxone_hori_init, sizeof(xboxone_hori_init) }, + { 0x0f0d, 0x0067, xboxone_hori_init, sizeof(xboxone_hori_init) }, + { 0x0000, 0x0000, xboxone_fw2015_init, sizeof(xboxone_fw2015_init) }, + { 0x0e6f, 0x0246, xboxone_pdp_init1, sizeof(xboxone_pdp_init1) }, + { 0x0e6f, 0x0246, xboxone_pdp_init2, sizeof(xboxone_pdp_init2) }, + { 0x0e6f, 0x02ab, xboxone_pdp_init1, sizeof(xboxone_pdp_init1) }, + { 0x0e6f, 0x02ab, xboxone_pdp_init2, sizeof(xboxone_pdp_init2) }, + { 0x0e6f, 0x02a4, xboxone_pdp_init1, sizeof(xboxone_pdp_init1) }, + { 0x0e6f, 0x02a4, xboxone_pdp_init2, sizeof(xboxone_pdp_init2) }, + { 0x24c6, 0x541a, xboxone_rumblebegin_init, sizeof(xboxone_rumblebegin_init) }, + { 0x24c6, 0x542a, xboxone_rumblebegin_init, sizeof(xboxone_rumblebegin_init) }, + { 0x24c6, 0x543a, xboxone_rumblebegin_init, sizeof(xboxone_rumblebegin_init) }, + { 0x24c6, 0x541a, xboxone_rumbleend_init, sizeof(xboxone_rumbleend_init) }, + { 0x24c6, 0x542a, xboxone_rumbleend_init, sizeof(xboxone_rumbleend_init) }, + { 0x24c6, 0x543a, xboxone_rumbleend_init, sizeof(xboxone_rumbleend_init) }, +}; + +typedef struct { + Uint8 sequence; + Uint8 last_state[USB_PACKET_LENGTH]; + Uint32 rumble_expiration; +} SDL_DriverXboxOne_Context; + + +static SDL_bool +HIDAPI_DriverXboxOne_IsSupportedDevice(Uint16 vendor_id, Uint16 product_id, Uint16 version, int interface_number) +{ + return SDL_IsJoystickXboxOne(vendor_id, product_id); +} + +static const char * +HIDAPI_DriverXboxOne_GetDeviceName(Uint16 vendor_id, Uint16 product_id) +{ + return HIDAPI_XboxControllerName(vendor_id, product_id); +} + +static SDL_bool +HIDAPI_DriverXboxOne_Init(SDL_Joystick *joystick, hid_device *dev, Uint16 vendor_id, Uint16 product_id, void **context) +{ + SDL_DriverXboxOne_Context *ctx; + int i; + Uint8 init_packet[USB_PACKET_LENGTH]; + + ctx = (SDL_DriverXboxOne_Context *)SDL_calloc(1, sizeof(*ctx)); + if (!ctx) { + SDL_OutOfMemory(); + return SDL_FALSE; + } + *context = ctx; + + /* Send the controller init data */ + for (i = 0; i < SDL_arraysize(xboxone_init_packets); ++i) { + const SDL_DriverXboxOne_InitPacket *packet = &xboxone_init_packets[i]; + if (!packet->vendor_id || (vendor_id == packet->vendor_id && product_id == packet->product_id)) { + SDL_memcpy(init_packet, packet->data, packet->size); + init_packet[2] = ctx->sequence++; + if (hid_write(dev, init_packet, packet->size) != packet->size) { + SDL_SetError("Couldn't write Xbox One initialization packet"); + SDL_free(ctx); + return SDL_FALSE; + } + } + } + + /* Initialize the joystick capabilities */ + joystick->nbuttons = SDL_CONTROLLER_BUTTON_MAX; + joystick->naxes = SDL_CONTROLLER_AXIS_MAX; + joystick->epowerlevel = SDL_JOYSTICK_POWER_WIRED; + + return SDL_TRUE; +} + +static int +HIDAPI_DriverXboxOne_Rumble(SDL_Joystick *joystick, hid_device *dev, void *context, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + SDL_DriverXboxOne_Context *ctx = (SDL_DriverXboxOne_Context *)context; + Uint8 rumble_packet[] = { 0x09, 0x00, 0x00, 0x09, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00, 0xFF }; + + /* The Rock Candy Xbox One Controller limits the range of + low frequency rumble strength in the range of [0 - 0x99] + high frequency rumble strength in the range of [0 - 0x82] + + I think the valid range of rumble at the firmware level is [0 - 0x7F] + */ + rumble_packet[2] = ctx->sequence++; + rumble_packet[8] = (low_frequency_rumble >> 9); + rumble_packet[9] = (high_frequency_rumble >> 9); + + if (hid_write(dev, rumble_packet, sizeof(rumble_packet)) != sizeof(rumble_packet)) { + return SDL_SetError("Couldn't send rumble packet"); + } + + if ((low_frequency_rumble || high_frequency_rumble) && duration_ms) { + ctx->rumble_expiration = SDL_GetTicks() + duration_ms; + } else { + ctx->rumble_expiration = 0; + } + return 0; +} + +static void +HIDAPI_DriverXboxOne_HandleStatePacket(SDL_Joystick *joystick, hid_device *dev, SDL_DriverXboxOne_Context *ctx, Uint8 *data, int size) +{ + Sint16 axis; + + if (ctx->last_state[4] != data[4]) { + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_START, (data[4] & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_BACK, (data[4] & 0x08) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_A, (data[4] & 0x10) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_B, (data[4] & 0x20) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_X, (data[4] & 0x40) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_Y, (data[4] & 0x80) ? SDL_PRESSED : SDL_RELEASED); + } + + if (ctx->last_state[5] != data[5]) { + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_UP, (data[5] & 0x01) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_DOWN, (data[5] & 0x02) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_LEFT, (data[5] & 0x04) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_DPAD_RIGHT, (data[5] & 0x08) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSHOULDER, (data[5] & 0x10) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSHOULDER, (data[5] & 0x20) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_LEFTSTICK, (data[5] & 0x40) ? SDL_PRESSED : SDL_RELEASED); + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_RIGHTSTICK, (data[5] & 0x80) ? SDL_PRESSED : SDL_RELEASED); + } + + axis = ((int)*(Sint16*)(&data[6]) * 64) - 32768; + if (axis == 32704) { + axis = 32767; + } + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERLEFT, axis); + axis = ((int)*(Sint16*)(&data[8]) * 64) - 32768; + if (axis == 32704) { + axis = 32767; + } + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_TRIGGERRIGHT, axis); + axis = *(Sint16*)(&data[10]); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTX, axis); + axis = *(Sint16*)(&data[12]); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_LEFTY, ~axis); + axis = *(Sint16*)(&data[14]); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTX, axis); + axis = *(Sint16*)(&data[16]); + SDL_PrivateJoystickAxis(joystick, SDL_CONTROLLER_AXIS_RIGHTY, ~axis); + + SDL_memcpy(ctx->last_state, data, SDL_min(size, sizeof(ctx->last_state))); +} + +static void +HIDAPI_DriverXboxOne_HandleModePacket(SDL_Joystick *joystick, hid_device *dev, SDL_DriverXboxOne_Context *ctx, Uint8 *data, int size) +{ + if (data[1] == 0x30) { + /* The Xbox One S controller needs acks for mode reports */ + const Uint8 seqnum = data[2]; + const Uint8 ack[] = { 0x01, 0x20, seqnum, 0x09, 0x00, 0x07, 0x20, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00 }; + hid_write(dev, ack, sizeof(ack)); + } + + SDL_PrivateJoystickButton(joystick, SDL_CONTROLLER_BUTTON_GUIDE, (data[4] & 0x01) ? SDL_PRESSED : SDL_RELEASED); +} + +static SDL_bool +HIDAPI_DriverXboxOne_Update(SDL_Joystick *joystick, hid_device *dev, void *context) +{ + SDL_DriverXboxOne_Context *ctx = (SDL_DriverXboxOne_Context *)context; + Uint8 data[USB_PACKET_LENGTH]; + int size; + + while ((size = hid_read_timeout(dev, data, sizeof(data), 0)) > 0) { + switch (data[0]) { + case 0x20: + HIDAPI_DriverXboxOne_HandleStatePacket(joystick, dev, ctx, data, size); + break; + case 0x07: + HIDAPI_DriverXboxOne_HandleModePacket(joystick, dev, ctx, data, size); + break; + default: +#ifdef DEBUG_JOYSTICK + SDL_Log("Unknown Xbox One packet: 0x%.2x\n", data[0]); +#endif + break; + } + } + + if (ctx->rumble_expiration) { + Uint32 now = SDL_GetTicks(); + if (SDL_TICKS_PASSED(now, ctx->rumble_expiration)) { + HIDAPI_DriverXboxOne_Rumble(joystick, dev, context, 0, 0, 0); + } + } + + return (size >= 0); +} + +static void +HIDAPI_DriverXboxOne_Quit(SDL_Joystick *joystick, hid_device *dev, void *context) +{ + SDL_free(context); +} + +SDL_HIDAPI_DeviceDriver SDL_HIDAPI_DriverXboxOne = +{ + SDL_HINT_JOYSTICK_HIDAPI_XBOX, + SDL_TRUE, + HIDAPI_DriverXboxOne_IsSupportedDevice, + HIDAPI_DriverXboxOne_GetDeviceName, + HIDAPI_DriverXboxOne_Init, + HIDAPI_DriverXboxOne_Rumble, + HIDAPI_DriverXboxOne_Update, + HIDAPI_DriverXboxOne_Quit +}; + +#endif /* SDL_JOYSTICK_HIDAPI_XBOXONE */ + +#endif /* SDL_JOYSTICK_HIDAPI */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapijoystick.c b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapijoystick.c new file mode 100644 index 000000000..064cb8204 --- /dev/null +++ b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapijoystick.c @@ -0,0 +1,1071 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "../../SDL_internal.h" + +#ifdef SDL_JOYSTICK_HIDAPI + +#include "SDL_endian.h" +#include "SDL_hints.h" +#include "SDL_log.h" +#include "SDL_mutex.h" +#include "SDL_thread.h" +#include "SDL_timer.h" +#include "SDL_joystick.h" +#include "../SDL_sysjoystick.h" +#include "SDL_hidapijoystick_c.h" + +#if defined(__WIN32__) +#include "../../core/windows/SDL_windows.h" +#endif + +#if defined(__MACOSX__) +#include +#include +#include +#include +#endif + +#if defined(__LINUX__) +#include "../../core/linux/SDL_udev.h" +#ifdef SDL_USE_LIBUDEV +#include +#endif +#endif + +struct joystick_hwdata +{ + SDL_HIDAPI_DeviceDriver *driver; + void *context; + + SDL_mutex *mutex; + hid_device *dev; +}; + +typedef struct _SDL_HIDAPI_Device +{ + SDL_JoystickID instance_id; + char *name; + char *path; + Uint16 vendor_id; + Uint16 product_id; + Uint16 version; + SDL_JoystickGUID guid; + int interface_number; /* Available on Windows and Linux */ + Uint16 usage_page; /* Available on Windows and Mac OS X */ + Uint16 usage; /* Available on Windows and Mac OS X */ + SDL_HIDAPI_DeviceDriver *driver; + + /* Used during scanning for device changes */ + SDL_bool seen; + + struct _SDL_HIDAPI_Device *next; +} SDL_HIDAPI_Device; + +static SDL_HIDAPI_DeviceDriver *SDL_HIDAPI_drivers[] = { +#ifdef SDL_JOYSTICK_HIDAPI_PS4 + &SDL_HIDAPI_DriverPS4, +#endif +#ifdef SDL_JOYSTICK_HIDAPI_STEAM + &SDL_HIDAPI_DriverSteam, +#endif +#ifdef SDL_JOYSTICK_HIDAPI_SWITCH + &SDL_HIDAPI_DriverSwitch, +#endif +#ifdef SDL_JOYSTICK_HIDAPI_XBOX360 + &SDL_HIDAPI_DriverXbox360, +#endif +#ifdef SDL_JOYSTICK_HIDAPI_XBOXONE + &SDL_HIDAPI_DriverXboxOne, +#endif +}; +static SDL_HIDAPI_Device *SDL_HIDAPI_devices; +static int SDL_HIDAPI_numjoysticks = 0; + +#if defined(SDL_USE_LIBUDEV) +static const SDL_UDEV_Symbols * usyms = NULL; +#endif + +static struct +{ + SDL_bool m_bHaveDevicesChanged; + SDL_bool m_bCanGetNotifications; + Uint32 m_unLastDetect; + +#if defined(__WIN32__) + SDL_threadID m_nThreadID; + WNDCLASSEXA m_wndClass; + HWND m_hwndMsg; + HDEVNOTIFY m_hNotify; + double m_flLastWin32MessageCheck; +#endif + +#if defined(__MACOSX__) + IONotificationPortRef m_notificationPort; + mach_port_t m_notificationMach; +#endif + +#if defined(SDL_USE_LIBUDEV) + struct udev *m_pUdev; + struct udev_monitor *m_pUdevMonitor; + int m_nUdevFd; +#endif +} SDL_HIDAPI_discovery; + + +#ifdef __WIN32__ +struct _DEV_BROADCAST_HDR +{ + DWORD dbch_size; + DWORD dbch_devicetype; + DWORD dbch_reserved; +}; + +typedef struct _DEV_BROADCAST_DEVICEINTERFACE_A +{ + DWORD dbcc_size; + DWORD dbcc_devicetype; + DWORD dbcc_reserved; + GUID dbcc_classguid; + char dbcc_name[ 1 ]; +} DEV_BROADCAST_DEVICEINTERFACE_A, *PDEV_BROADCAST_DEVICEINTERFACE_A; + +typedef struct _DEV_BROADCAST_HDR DEV_BROADCAST_HDR; +#define DBT_DEVICEARRIVAL 0x8000 /* system detected a new device */ +#define DBT_DEVICEREMOVECOMPLETE 0x8004 /* device was removed from the system */ +#define DBT_DEVTYP_DEVICEINTERFACE 0x00000005 /* device interface class */ +#define DBT_DEVNODES_CHANGED 0x0007 +#define DBT_CONFIGCHANGED 0x0018 +#define DBT_DEVICETYPESPECIFIC 0x8005 /* type specific event */ +#define DBT_DEVINSTSTARTED 0x8008 /* device installed and started */ + +#include +DEFINE_GUID(GUID_DEVINTERFACE_USB_DEVICE, 0xA5DCBF10L, 0x6530, 0x11D2, 0x90, 0x1F, 0x00, 0xC0, 0x4F, 0xB9, 0x51, 0xED); + +static LRESULT CALLBACK ControllerWndProc(HWND hwnd, UINT message, WPARAM wParam, LPARAM lParam) +{ + switch (message) { + case WM_DEVICECHANGE: + switch (wParam) { + case DBT_DEVICEARRIVAL: + case DBT_DEVICEREMOVECOMPLETE: + if (((DEV_BROADCAST_HDR*)lParam)->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) { + SDL_HIDAPI_discovery.m_bHaveDevicesChanged = SDL_TRUE; + } + break; + } + return TRUE; + } + + return DefWindowProc(hwnd, message, wParam, lParam); +} +#endif /* __WIN32__ */ + + +#if defined(__MACOSX__) +static void CallbackIOServiceFunc(void *context, io_iterator_t portIterator) +{ + /* Must drain the iterator, or we won't receive new notifications */ + io_object_t entry; + while ((entry = IOIteratorNext(portIterator)) != 0) { + IOObjectRelease(entry); + *(SDL_bool*)context = SDL_TRUE; + } +} +#endif /* __MACOSX__ */ + +static void +HIDAPI_InitializeDiscovery() +{ + SDL_HIDAPI_discovery.m_bHaveDevicesChanged = SDL_TRUE; + SDL_HIDAPI_discovery.m_bCanGetNotifications = SDL_FALSE; + SDL_HIDAPI_discovery.m_unLastDetect = 0; + +#if defined(__WIN32__) + SDL_HIDAPI_discovery.m_nThreadID = SDL_ThreadID(); + + SDL_memset(&SDL_HIDAPI_discovery.m_wndClass, 0x0, sizeof(SDL_HIDAPI_discovery.m_wndClass)); + SDL_HIDAPI_discovery.m_wndClass.hInstance = GetModuleHandle(NULL); + SDL_HIDAPI_discovery.m_wndClass.lpszClassName = "SDL_HIDAPI_DEVICE_DETECTION"; + SDL_HIDAPI_discovery.m_wndClass.lpfnWndProc = ControllerWndProc; /* This function is called by windows */ + SDL_HIDAPI_discovery.m_wndClass.cbSize = sizeof(WNDCLASSEX); + + RegisterClassExA(&SDL_HIDAPI_discovery.m_wndClass); + SDL_HIDAPI_discovery.m_hwndMsg = CreateWindowExA(0, "SDL_HIDAPI_DEVICE_DETECTION", NULL, 0, 0, 0, 0, 0, HWND_MESSAGE, NULL, NULL, NULL); + + { + DEV_BROADCAST_DEVICEINTERFACE_A devBroadcast; + SDL_memset( &devBroadcast, 0x0, sizeof( devBroadcast ) ); + + devBroadcast.dbcc_size = sizeof( devBroadcast ); + devBroadcast.dbcc_devicetype = DBT_DEVTYP_DEVICEINTERFACE; + devBroadcast.dbcc_classguid = GUID_DEVINTERFACE_USB_DEVICE; + + /* DEVICE_NOTIFY_ALL_INTERFACE_CLASSES is important, makes GUID_DEVINTERFACE_USB_DEVICE ignored, + * but that seems to be necessary to get a notice after each individual usb input device actually + * installs, rather than just as the composite device is seen. + */ + SDL_HIDAPI_discovery.m_hNotify = RegisterDeviceNotification( SDL_HIDAPI_discovery.m_hwndMsg, &devBroadcast, DEVICE_NOTIFY_WINDOW_HANDLE | DEVICE_NOTIFY_ALL_INTERFACE_CLASSES ); + SDL_HIDAPI_discovery.m_bCanGetNotifications = ( SDL_HIDAPI_discovery.m_hNotify != 0 ); + } +#endif /* __WIN32__ */ + +#if defined(__MACOSX__) + SDL_HIDAPI_discovery.m_notificationPort = IONotificationPortCreate(kIOMasterPortDefault); + if (SDL_HIDAPI_discovery.m_notificationPort) { + { + CFMutableDictionaryRef matchingDict = IOServiceMatching("IOUSBDevice"); + + /* Note: IOServiceAddMatchingNotification consumes the reference to matchingDict */ + io_iterator_t portIterator = 0; + io_object_t entry; + if (IOServiceAddMatchingNotification(SDL_HIDAPI_discovery.m_notificationPort, kIOMatchedNotification, matchingDict, CallbackIOServiceFunc, &SDL_HIDAPI_discovery.m_bHaveDevicesChanged, &portIterator) == 0) { + /* Must drain the existing iterator, or we won't receive new notifications */ + while ((entry = IOIteratorNext(portIterator)) != 0) { + IOObjectRelease(entry); + } + } else { + IONotificationPortDestroy(SDL_HIDAPI_discovery.m_notificationPort); + SDL_HIDAPI_discovery.m_notificationPort = nil; + } + } + { + CFMutableDictionaryRef matchingDict = IOServiceMatching("IOBluetoothDevice"); + + /* Note: IOServiceAddMatchingNotification consumes the reference to matchingDict */ + io_iterator_t portIterator = 0; + io_object_t entry; + if (IOServiceAddMatchingNotification(SDL_HIDAPI_discovery.m_notificationPort, kIOMatchedNotification, matchingDict, CallbackIOServiceFunc, &SDL_HIDAPI_discovery.m_bHaveDevicesChanged, &portIterator) == 0) { + /* Must drain the existing iterator, or we won't receive new notifications */ + while ((entry = IOIteratorNext(portIterator)) != 0) { + IOObjectRelease(entry); + } + } else { + IONotificationPortDestroy(SDL_HIDAPI_discovery.m_notificationPort); + SDL_HIDAPI_discovery.m_notificationPort = nil; + } + } + } + + SDL_HIDAPI_discovery.m_notificationMach = MACH_PORT_NULL; + if (SDL_HIDAPI_discovery.m_notificationPort) { + SDL_HIDAPI_discovery.m_notificationMach = IONotificationPortGetMachPort(SDL_HIDAPI_discovery.m_notificationPort); + } + + SDL_HIDAPI_discovery.m_bCanGetNotifications = (SDL_HIDAPI_discovery.m_notificationMach != MACH_PORT_NULL); + +#endif // __MACOSX__ + +#if defined(SDL_USE_LIBUDEV) + SDL_HIDAPI_discovery.m_pUdev = NULL; + SDL_HIDAPI_discovery.m_pUdevMonitor = NULL; + SDL_HIDAPI_discovery.m_nUdevFd = -1; + + usyms = SDL_UDEV_GetUdevSyms(); + if (usyms) { + SDL_HIDAPI_discovery.m_pUdev = usyms->udev_new(); + } + if (SDL_HIDAPI_discovery.m_pUdev) { + SDL_HIDAPI_discovery.m_pUdevMonitor = usyms->udev_monitor_new_from_netlink(SDL_HIDAPI_discovery.m_pUdev, "udev"); + if (SDL_HIDAPI_discovery.m_pUdevMonitor) { + usyms->udev_monitor_enable_receiving(SDL_HIDAPI_discovery.m_pUdevMonitor); + SDL_HIDAPI_discovery.m_nUdevFd = usyms->udev_monitor_get_fd(SDL_HIDAPI_discovery.m_pUdevMonitor); + SDL_HIDAPI_discovery.m_bCanGetNotifications = SDL_TRUE; + } + } + +#endif /* SDL_USE_LIBUDEV */ +} + +static void +HIDAPI_UpdateDiscovery() +{ + if (!SDL_HIDAPI_discovery.m_bCanGetNotifications) { + const Uint32 SDL_HIDAPI_DETECT_INTERVAL_MS = 3000; /* Update every 3 seconds */ + Uint32 now = SDL_GetTicks(); + if (!SDL_HIDAPI_discovery.m_unLastDetect || SDL_TICKS_PASSED(now, SDL_HIDAPI_discovery.m_unLastDetect + SDL_HIDAPI_DETECT_INTERVAL_MS)) { + SDL_HIDAPI_discovery.m_bHaveDevicesChanged = SDL_TRUE; + SDL_HIDAPI_discovery.m_unLastDetect = now; + } + return; + } + +#if defined(__WIN32__) +#if 0 /* just let the usual SDL_PumpEvents loop dispatch these, fixing bug 4286. --ryan. */ + /* We'll only get messages on the same thread that created the window */ + if (SDL_ThreadID() == SDL_HIDAPI_discovery.m_nThreadID) { + MSG msg; + while (PeekMessage(&msg, SDL_HIDAPI_discovery.m_hwndMsg, 0, 0, PM_NOREMOVE)) { + if (GetMessageA(&msg, SDL_HIDAPI_discovery.m_hwndMsg, 0, 0) != 0) { + TranslateMessage(&msg); + DispatchMessage(&msg); + } + } + } +#endif +#endif /* __WIN32__ */ + +#if defined(__MACOSX__) + if (SDL_HIDAPI_discovery.m_notificationPort) { + struct { mach_msg_header_t hdr; char payload[ 4096 ]; } msg; + while (mach_msg(&msg.hdr, MACH_RCV_MSG | MACH_RCV_TIMEOUT, 0, sizeof(msg), SDL_HIDAPI_discovery.m_notificationMach, 0, MACH_PORT_NULL) == KERN_SUCCESS) { + IODispatchCalloutFromMessage(NULL, &msg.hdr, SDL_HIDAPI_discovery.m_notificationPort); + } + } +#endif + +#if defined(SDL_USE_LIBUDEV) + if (SDL_HIDAPI_discovery.m_nUdevFd >= 0) { + /* Drain all notification events. + * We don't expect a lot of device notifications so just + * do a new discovery on any kind or number of notifications. + * This could be made more restrictive if necessary. + */ + for (;;) { + struct pollfd PollUdev; + struct udev_device *pUdevDevice; + + PollUdev.fd = SDL_HIDAPI_discovery.m_nUdevFd; + PollUdev.events = POLLIN; + if (poll(&PollUdev, 1, 0) != 1) { + break; + } + + SDL_HIDAPI_discovery.m_bHaveDevicesChanged = SDL_TRUE; + + pUdevDevice = usyms->udev_monitor_receive_device(SDL_HIDAPI_discovery.m_pUdevMonitor); + if (pUdevDevice) { + usyms->udev_device_unref(pUdevDevice); + } + } + } +#endif +} + +static void +HIDAPI_ShutdownDiscovery() +{ +#if defined(__WIN32__) + if (SDL_HIDAPI_discovery.m_hNotify) + UnregisterDeviceNotification(SDL_HIDAPI_discovery.m_hNotify); + + if (SDL_HIDAPI_discovery.m_hwndMsg) { + DestroyWindow(SDL_HIDAPI_discovery.m_hwndMsg); + } + + UnregisterClassA(SDL_HIDAPI_discovery.m_wndClass.lpszClassName, SDL_HIDAPI_discovery.m_wndClass.hInstance); +#endif + +#if defined(__MACOSX__) + if (SDL_HIDAPI_discovery.m_notificationPort) { + IONotificationPortDestroy(SDL_HIDAPI_discovery.m_notificationPort); + } +#endif + +#if defined(SDL_USE_LIBUDEV) + if (usyms) { + if (SDL_HIDAPI_discovery.m_pUdevMonitor) { + usyms->udev_monitor_unref(SDL_HIDAPI_discovery.m_pUdevMonitor); + } + if (SDL_HIDAPI_discovery.m_pUdev) { + usyms->udev_unref(SDL_HIDAPI_discovery.m_pUdev); + } + SDL_UDEV_ReleaseUdevSyms(); + usyms = NULL; + } +#endif +} + + +const char * +HIDAPI_XboxControllerName(Uint16 vendor_id, Uint16 product_id) +{ + static struct + { + Uint32 vidpid; + const char *name; + } names[] = { + { MAKE_VIDPID(0x0079, 0x18d4), "GPD Win 2 X-Box Controller" }, + { MAKE_VIDPID(0x044f, 0xb326), "Thrustmaster Gamepad GP XID" }, + { MAKE_VIDPID(0x045e, 0x028e), "Microsoft X-Box 360 pad" }, + { MAKE_VIDPID(0x045e, 0x028f), "Microsoft X-Box 360 pad v2" }, + { MAKE_VIDPID(0x045e, 0x0291), "Xbox 360 Wireless Receiver (XBOX)" }, + { MAKE_VIDPID(0x045e, 0x02d1), "Microsoft X-Box One pad" }, + { MAKE_VIDPID(0x045e, 0x02dd), "Microsoft X-Box One pad (Firmware 2015)" }, + { MAKE_VIDPID(0x045e, 0x02e3), "Microsoft X-Box One Elite pad" }, + { MAKE_VIDPID(0x045e, 0x02ea), "Microsoft X-Box One S pad" }, + { MAKE_VIDPID(0x045e, 0x02ff), "Microsoft X-Box One pad" }, + { MAKE_VIDPID(0x045e, 0x0719), "Xbox 360 Wireless Receiver" }, + { MAKE_VIDPID(0x046d, 0xc21d), "Logitech Gamepad F310" }, + { MAKE_VIDPID(0x046d, 0xc21e), "Logitech Gamepad F510" }, + { MAKE_VIDPID(0x046d, 0xc21f), "Logitech Gamepad F710" }, + { MAKE_VIDPID(0x046d, 0xc242), "Logitech Chillstream Controller" }, + { MAKE_VIDPID(0x046d, 0xcaa3), "Logitech DriveFx Racing Wheel" }, + { MAKE_VIDPID(0x056e, 0x2004), "Elecom JC-U3613M" }, + { MAKE_VIDPID(0x06a3, 0xf51a), "Saitek P3600" }, + { MAKE_VIDPID(0x0738, 0x4716), "Mad Catz Wired Xbox 360 Controller" }, + { MAKE_VIDPID(0x0738, 0x4718), "Mad Catz Street Fighter IV FightStick SE" }, + { MAKE_VIDPID(0x0738, 0x4726), "Mad Catz Xbox 360 Controller" }, + { MAKE_VIDPID(0x0738, 0x4728), "Mad Catz Street Fighter IV FightPad" }, + { MAKE_VIDPID(0x0738, 0x4736), "Mad Catz MicroCon Gamepad" }, + { MAKE_VIDPID(0x0738, 0x4738), "Mad Catz Wired Xbox 360 Controller (SFIV)" }, + { MAKE_VIDPID(0x0738, 0x4740), "Mad Catz Beat Pad" }, + { MAKE_VIDPID(0x0738, 0x4758), "Mad Catz Arcade Game Stick" }, + { MAKE_VIDPID(0x0738, 0x4a01), "Mad Catz FightStick TE 2" }, + { MAKE_VIDPID(0x0738, 0x9871), "Mad Catz Portable Drum" }, + { MAKE_VIDPID(0x0738, 0xb726), "Mad Catz Xbox controller - MW2" }, + { MAKE_VIDPID(0x0738, 0xb738), "Mad Catz MVC2TE Stick 2" }, + { MAKE_VIDPID(0x0738, 0xbeef), "Mad Catz JOYTECH NEO SE Advanced GamePad" }, + { MAKE_VIDPID(0x0738, 0xcb02), "Saitek Cyborg Rumble Pad - PC/Xbox 360" }, + { MAKE_VIDPID(0x0738, 0xcb03), "Saitek P3200 Rumble Pad - PC/Xbox 360" }, + { MAKE_VIDPID(0x0738, 0xcb29), "Saitek Aviator Stick AV8R02" }, + { MAKE_VIDPID(0x0738, 0xf738), "Super SFIV FightStick TE S" }, + { MAKE_VIDPID(0x07ff, 0xffff), "Mad Catz GamePad" }, + { MAKE_VIDPID(0x0e6f, 0x0105), "HSM3 Xbox360 dancepad" }, + { MAKE_VIDPID(0x0e6f, 0x0113), "Afterglow AX.1 Gamepad for Xbox 360" }, + { MAKE_VIDPID(0x0e6f, 0x011f), "Rock Candy Gamepad Wired Controller" }, + { MAKE_VIDPID(0x0e6f, 0x0131), "PDP EA Sports Controller" }, + { MAKE_VIDPID(0x0e6f, 0x0133), "Xbox 360 Wired Controller" }, + { MAKE_VIDPID(0x0e6f, 0x0139), "Afterglow Prismatic Wired Controller" }, + { MAKE_VIDPID(0x0e6f, 0x013a), "PDP Xbox One Controller" }, + { MAKE_VIDPID(0x0e6f, 0x0146), "Rock Candy Wired Controller for Xbox One" }, + { MAKE_VIDPID(0x0e6f, 0x0147), "PDP Marvel Xbox One Controller" }, + { MAKE_VIDPID(0x0e6f, 0x015c), "PDP Xbox One Arcade Stick" }, + { MAKE_VIDPID(0x0e6f, 0x0161), "PDP Xbox One Controller" }, + { MAKE_VIDPID(0x0e6f, 0x0162), "PDP Xbox One Controller" }, + { MAKE_VIDPID(0x0e6f, 0x0163), "PDP Xbox One Controller" }, + { MAKE_VIDPID(0x0e6f, 0x0164), "PDP Battlefield One" }, + { MAKE_VIDPID(0x0e6f, 0x0165), "PDP Titanfall 2" }, + { MAKE_VIDPID(0x0e6f, 0x0201), "Pelican PL-3601 'TSZ' Wired Xbox 360 Controller" }, + { MAKE_VIDPID(0x0e6f, 0x0213), "Afterglow Gamepad for Xbox 360" }, + { MAKE_VIDPID(0x0e6f, 0x021f), "Rock Candy Gamepad for Xbox 360" }, + { MAKE_VIDPID(0x0e6f, 0x0246), "Rock Candy Gamepad for Xbox One 2015" }, + { MAKE_VIDPID(0x0e6f, 0x02a4), "PDP Wired Controller for Xbox One - Stealth Series" }, + { MAKE_VIDPID(0x0e6f, 0x02ab), "PDP Controller for Xbox One" }, + { MAKE_VIDPID(0x0e6f, 0x0301), "Logic3 Controller" }, + { MAKE_VIDPID(0x0e6f, 0x0346), "Rock Candy Gamepad for Xbox One 2016" }, + { MAKE_VIDPID(0x0e6f, 0x0401), "Logic3 Controller" }, + { MAKE_VIDPID(0x0e6f, 0x0413), "Afterglow AX.1 Gamepad for Xbox 360" }, + { MAKE_VIDPID(0x0e6f, 0x0501), "PDP Xbox 360 Controller" }, + { MAKE_VIDPID(0x0e6f, 0xf900), "PDP Afterglow AX.1" }, + { MAKE_VIDPID(0x0f0d, 0x000a), "Hori Co. DOA4 FightStick" }, + { MAKE_VIDPID(0x0f0d, 0x000c), "Hori PadEX Turbo" }, + { MAKE_VIDPID(0x0f0d, 0x000d), "Hori Fighting Stick EX2" }, + { MAKE_VIDPID(0x0f0d, 0x0016), "Hori Real Arcade Pro.EX" }, + { MAKE_VIDPID(0x0f0d, 0x001b), "Hori Real Arcade Pro VX" }, + { MAKE_VIDPID(0x0f0d, 0x0063), "Hori Real Arcade Pro Hayabusa (USA) Xbox One" }, + { MAKE_VIDPID(0x0f0d, 0x0067), "HORIPAD ONE" }, + { MAKE_VIDPID(0x0f0d, 0x0078), "Hori Real Arcade Pro V Kai Xbox One" }, + { MAKE_VIDPID(0x11c9, 0x55f0), "Nacon GC-100XF" }, + { MAKE_VIDPID(0x12ab, 0x0004), "Honey Bee Xbox360 dancepad" }, + { MAKE_VIDPID(0x12ab, 0x0301), "PDP AFTERGLOW AX.1" }, + { MAKE_VIDPID(0x12ab, 0x0303), "Mortal Kombat Klassic FightStick" }, + { MAKE_VIDPID(0x1430, 0x4748), "RedOctane Guitar Hero X-plorer" }, + { MAKE_VIDPID(0x1430, 0xf801), "RedOctane Controller" }, + { MAKE_VIDPID(0x146b, 0x0601), "BigBen Interactive XBOX 360 Controller" }, + { MAKE_VIDPID(0x1532, 0x0037), "Razer Sabertooth" }, + { MAKE_VIDPID(0x1532, 0x0a00), "Razer Atrox Arcade Stick" }, + { MAKE_VIDPID(0x1532, 0x0a03), "Razer Wildcat" }, + { MAKE_VIDPID(0x15e4, 0x3f00), "Power A Mini Pro Elite" }, + { MAKE_VIDPID(0x15e4, 0x3f0a), "Xbox Airflo wired controller" }, + { MAKE_VIDPID(0x15e4, 0x3f10), "Batarang Xbox 360 controller" }, + { MAKE_VIDPID(0x162e, 0xbeef), "Joytech Neo-Se Take2" }, + { MAKE_VIDPID(0x1689, 0xfd00), "Razer Onza Tournament Edition" }, + { MAKE_VIDPID(0x1689, 0xfd01), "Razer Onza Classic Edition" }, + { MAKE_VIDPID(0x1689, 0xfe00), "Razer Sabertooth" }, + { MAKE_VIDPID(0x1bad, 0x0002), "Harmonix Rock Band Guitar" }, + { MAKE_VIDPID(0x1bad, 0x0003), "Harmonix Rock Band Drumkit" }, + { MAKE_VIDPID(0x1bad, 0x0130), "Ion Drum Rocker" }, + { MAKE_VIDPID(0x1bad, 0xf016), "Mad Catz Xbox 360 Controller" }, + { MAKE_VIDPID(0x1bad, 0xf018), "Mad Catz Street Fighter IV SE Fighting Stick" }, + { MAKE_VIDPID(0x1bad, 0xf019), "Mad Catz Brawlstick for Xbox 360" }, + { MAKE_VIDPID(0x1bad, 0xf021), "Mad Cats Ghost Recon FS GamePad" }, + { MAKE_VIDPID(0x1bad, 0xf023), "MLG Pro Circuit Controller (Xbox)" }, + { MAKE_VIDPID(0x1bad, 0xf025), "Mad Catz Call Of Duty" }, + { MAKE_VIDPID(0x1bad, 0xf027), "Mad Catz FPS Pro" }, + { MAKE_VIDPID(0x1bad, 0xf028), "Street Fighter IV FightPad" }, + { MAKE_VIDPID(0x1bad, 0xf02e), "Mad Catz Fightpad" }, + { MAKE_VIDPID(0x1bad, 0xf030), "Mad Catz Xbox 360 MC2 MicroCon Racing Wheel" }, + { MAKE_VIDPID(0x1bad, 0xf036), "Mad Catz MicroCon GamePad Pro" }, + { MAKE_VIDPID(0x1bad, 0xf038), "Street Fighter IV FightStick TE" }, + { MAKE_VIDPID(0x1bad, 0xf039), "Mad Catz MvC2 TE" }, + { MAKE_VIDPID(0x1bad, 0xf03a), "Mad Catz SFxT Fightstick Pro" }, + { MAKE_VIDPID(0x1bad, 0xf03d), "Street Fighter IV Arcade Stick TE - Chun Li" }, + { MAKE_VIDPID(0x1bad, 0xf03e), "Mad Catz MLG FightStick TE" }, + { MAKE_VIDPID(0x1bad, 0xf03f), "Mad Catz FightStick SoulCaliber" }, + { MAKE_VIDPID(0x1bad, 0xf042), "Mad Catz FightStick TES+" }, + { MAKE_VIDPID(0x1bad, 0xf080), "Mad Catz FightStick TE2" }, + { MAKE_VIDPID(0x1bad, 0xf501), "HoriPad EX2 Turbo" }, + { MAKE_VIDPID(0x1bad, 0xf502), "Hori Real Arcade Pro.VX SA" }, + { MAKE_VIDPID(0x1bad, 0xf503), "Hori Fighting Stick VX" }, + { MAKE_VIDPID(0x1bad, 0xf504), "Hori Real Arcade Pro. EX" }, + { MAKE_VIDPID(0x1bad, 0xf505), "Hori Fighting Stick EX2B" }, + { MAKE_VIDPID(0x1bad, 0xf506), "Hori Real Arcade Pro.EX Premium VLX" }, + { MAKE_VIDPID(0x1bad, 0xf900), "Harmonix Xbox 360 Controller" }, + { MAKE_VIDPID(0x1bad, 0xf901), "Gamestop Xbox 360 Controller" }, + { MAKE_VIDPID(0x1bad, 0xf903), "Tron Xbox 360 controller" }, + { MAKE_VIDPID(0x1bad, 0xf904), "PDP Versus Fighting Pad" }, + { MAKE_VIDPID(0x1bad, 0xf906), "MortalKombat FightStick" }, + { MAKE_VIDPID(0x1bad, 0xfa01), "MadCatz GamePad" }, + { MAKE_VIDPID(0x1bad, 0xfd00), "Razer Onza TE" }, + { MAKE_VIDPID(0x1bad, 0xfd01), "Razer Onza" }, + { MAKE_VIDPID(0x24c6, 0x5000), "Razer Atrox Arcade Stick" }, + { MAKE_VIDPID(0x24c6, 0x5300), "PowerA MINI PROEX Controller" }, + { MAKE_VIDPID(0x24c6, 0x5303), "Xbox Airflo wired controller" }, + { MAKE_VIDPID(0x24c6, 0x530a), "Xbox 360 Pro EX Controller" }, + { MAKE_VIDPID(0x24c6, 0x531a), "PowerA Pro Ex" }, + { MAKE_VIDPID(0x24c6, 0x5397), "FUS1ON Tournament Controller" }, + { MAKE_VIDPID(0x24c6, 0x541a), "PowerA Xbox One Mini Wired Controller" }, + { MAKE_VIDPID(0x24c6, 0x542a), "Xbox ONE spectra" }, + { MAKE_VIDPID(0x24c6, 0x543a), "PowerA Xbox One wired controller" }, + { MAKE_VIDPID(0x24c6, 0x5500), "Hori XBOX 360 EX 2 with Turbo" }, + { MAKE_VIDPID(0x24c6, 0x5501), "Hori Real Arcade Pro VX-SA" }, + { MAKE_VIDPID(0x24c6, 0x5502), "Hori Fighting Stick VX Alt" }, + { MAKE_VIDPID(0x24c6, 0x5503), "Hori Fighting Edge" }, + { MAKE_VIDPID(0x24c6, 0x5506), "Hori SOULCALIBUR V Stick" }, + { MAKE_VIDPID(0x24c6, 0x550d), "Hori GEM Xbox controller" }, + { MAKE_VIDPID(0x24c6, 0x550e), "Hori Real Arcade Pro V Kai 360" }, + { MAKE_VIDPID(0x24c6, 0x551a), "PowerA FUSION Pro Controller" }, + { MAKE_VIDPID(0x24c6, 0x561a), "PowerA FUSION Controller" }, + { MAKE_VIDPID(0x24c6, 0x5b00), "ThrustMaster Ferrari 458 Racing Wheel" }, + { MAKE_VIDPID(0x24c6, 0x5b02), "Thrustmaster, Inc. GPX Controller" }, + { MAKE_VIDPID(0x24c6, 0x5b03), "Thrustmaster Ferrari 458 Racing Wheel" }, + { MAKE_VIDPID(0x24c6, 0x5d04), "Razer Sabertooth" }, + { MAKE_VIDPID(0x24c6, 0xfafe), "Rock Candy Gamepad for Xbox 360" }, + }; + int i; + Uint32 vidpid = MAKE_VIDPID(vendor_id, product_id); + + for (i = 0; i < SDL_arraysize(names); ++i) { + if (vidpid == names[i].vidpid) { + return names[i].name; + } + } + return NULL; +} + +static SDL_bool +HIDAPI_IsDeviceSupported(Uint16 vendor_id, Uint16 product_id, Uint16 version) +{ + int i; + + for (i = 0; i < SDL_arraysize(SDL_HIDAPI_drivers); ++i) { + SDL_HIDAPI_DeviceDriver *driver = SDL_HIDAPI_drivers[i]; + if (driver->enabled && driver->IsSupportedDevice(vendor_id, product_id, version, -1)) { + return SDL_TRUE; + } + } + return SDL_FALSE; +} + +static SDL_HIDAPI_DeviceDriver * +HIDAPI_GetDeviceDriver(SDL_HIDAPI_Device *device) +{ + const Uint16 USAGE_PAGE_GENERIC_DESKTOP = 0x0001; + const Uint16 USAGE_JOYSTICK = 0x0004; + const Uint16 USAGE_GAMEPAD = 0x0005; + const Uint16 USAGE_MULTIAXISCONTROLLER = 0x0008; + int i; + + if (SDL_ShouldIgnoreJoystick(device->name, device->guid)) { + return NULL; + } + + if (device->usage_page && device->usage_page != USAGE_PAGE_GENERIC_DESKTOP) { + return NULL; + } + if (device->usage && device->usage != USAGE_JOYSTICK && device->usage != USAGE_GAMEPAD && device->usage != USAGE_MULTIAXISCONTROLLER) { + return NULL; + } + + for (i = 0; i < SDL_arraysize(SDL_HIDAPI_drivers); ++i) { + SDL_HIDAPI_DeviceDriver *driver = SDL_HIDAPI_drivers[i]; + if (driver->enabled && driver->IsSupportedDevice(device->vendor_id, device->product_id, device->version, device->interface_number)) { + return driver; + } + } + return NULL; +} + +static SDL_HIDAPI_Device * +HIDAPI_GetJoystickByIndex(int device_index) +{ + SDL_HIDAPI_Device *device = SDL_HIDAPI_devices; + while (device) { + if (device->driver) { + if (device_index == 0) { + break; + } + --device_index; + } + device = device->next; + } + return device; +} + +static SDL_HIDAPI_Device * +HIDAPI_GetJoystickByInfo(const char *path, Uint16 vendor_id, Uint16 product_id) +{ + SDL_HIDAPI_Device *device = SDL_HIDAPI_devices; + while (device) { + if (device->vendor_id == vendor_id && device->product_id == product_id && + SDL_strcmp(device->path, path) == 0) { + break; + } + device = device->next; + } + return device; +} + +static void SDLCALL +SDL_HIDAPIDriverHintChanged(void *userdata, const char *name, const char *oldValue, const char *hint) +{ + int i; + SDL_HIDAPI_Device *device = SDL_HIDAPI_devices; + SDL_bool enabled = (!hint || !*hint || ((*hint != '0') && (SDL_strcasecmp(hint, "false") != 0))); + + if (SDL_strcmp(name, SDL_HINT_JOYSTICK_HIDAPI) == 0) { + for (i = 0; i < SDL_arraysize(SDL_HIDAPI_drivers); ++i) { + SDL_HIDAPI_DeviceDriver *driver = SDL_HIDAPI_drivers[i]; + driver->enabled = SDL_GetHintBoolean(driver->hint, enabled); + } + } else { + for (i = 0; i < SDL_arraysize(SDL_HIDAPI_drivers); ++i) { + SDL_HIDAPI_DeviceDriver *driver = SDL_HIDAPI_drivers[i]; + if (SDL_strcmp(name, driver->hint) == 0) { + driver->enabled = enabled; + break; + } + } + } + + /* Update device list if driver availability changes */ + while (device) { + if (device->driver) { + if (!device->driver->enabled) { + device->driver = NULL; + + --SDL_HIDAPI_numjoysticks; + + SDL_PrivateJoystickRemoved(device->instance_id); + } + } else { + device->driver = HIDAPI_GetDeviceDriver(device); + if (device->driver) { + device->instance_id = SDL_GetNextJoystickInstanceID(); + + ++SDL_HIDAPI_numjoysticks; + + SDL_PrivateJoystickAdded(device->instance_id); + } + } + device = device->next; + } +} + +static void HIDAPI_JoystickDetect(void); + +static int +HIDAPI_JoystickInit(void) +{ + int i; + + if (hid_init() < 0) { + SDL_SetError("Couldn't initialize hidapi"); + return -1; + } + + for (i = 0; i < SDL_arraysize(SDL_HIDAPI_drivers); ++i) { + SDL_HIDAPI_DeviceDriver *driver = SDL_HIDAPI_drivers[i]; + SDL_AddHintCallback(driver->hint, SDL_HIDAPIDriverHintChanged, NULL); + } + SDL_AddHintCallback(SDL_HINT_JOYSTICK_HIDAPI, + SDL_HIDAPIDriverHintChanged, NULL); + HIDAPI_InitializeDiscovery(); + HIDAPI_JoystickDetect(); + return 0; +} + +static int +HIDAPI_JoystickGetCount(void) +{ + return SDL_HIDAPI_numjoysticks; +} + +static void +HIDAPI_AddDevice(struct hid_device_info *info) +{ + SDL_HIDAPI_Device *device; + SDL_HIDAPI_Device *curr, *last = NULL; + + for (curr = SDL_HIDAPI_devices, last = NULL; curr; last = curr, curr = curr->next) { + continue; + } + + device = (SDL_HIDAPI_Device *)SDL_calloc(1, sizeof(*device)); + if (!device) { + return; + } + device->instance_id = -1; + device->seen = SDL_TRUE; + device->vendor_id = info->vendor_id; + device->product_id = info->product_id; + device->version = info->release_number; + device->interface_number = info->interface_number; + device->usage_page = info->usage_page; + device->usage = info->usage; + { + /* FIXME: Is there any way to tell whether this is a Bluetooth device? */ + const Uint16 vendor = device->vendor_id; + const Uint16 product = device->product_id; + const Uint16 version = device->version; + Uint16 *guid16 = (Uint16 *)device->guid.data; + + *guid16++ = SDL_SwapLE16(SDL_HARDWARE_BUS_USB); + *guid16++ = 0; + *guid16++ = SDL_SwapLE16(vendor); + *guid16++ = 0; + *guid16++ = SDL_SwapLE16(product); + *guid16++ = 0; + *guid16++ = SDL_SwapLE16(version); + *guid16++ = 0; + + /* Note that this is a HIDAPI device for special handling elsewhere */ + device->guid.data[14] = 'h'; + device->guid.data[15] = 0; + } + + /* Need the device name before getting the driver to know whether to ignore this device */ + if (!device->name && info->manufacturer_string && info->product_string) { + char *manufacturer_string = SDL_iconv_string("UTF-8", "WCHAR_T", (char*)info->manufacturer_string, (SDL_wcslen(info->manufacturer_string)+1)*sizeof(wchar_t)); + char *product_string = SDL_iconv_string("UTF-8", "WCHAR_T", (char*)info->product_string, (SDL_wcslen(info->product_string)+1)*sizeof(wchar_t)); + if (!manufacturer_string && !product_string) { + if (sizeof(wchar_t) == sizeof(Uint16)) { + manufacturer_string = SDL_iconv_string("UTF-8", "UCS-2-INTERNAL", (char*)info->manufacturer_string, (SDL_wcslen(info->manufacturer_string)+1)*sizeof(wchar_t)); + product_string = SDL_iconv_string("UTF-8", "UCS-2-INTERNAL", (char*)info->product_string, (SDL_wcslen(info->product_string)+1)*sizeof(wchar_t)); + } else if (sizeof(wchar_t) == sizeof(Uint32)) { + manufacturer_string = SDL_iconv_string("UTF-8", "UCS-4-INTERNAL", (char*)info->manufacturer_string, (SDL_wcslen(info->manufacturer_string)+1)*sizeof(wchar_t)); + product_string = SDL_iconv_string("UTF-8", "UCS-4-INTERNAL", (char*)info->product_string, (SDL_wcslen(info->product_string)+1)*sizeof(wchar_t)); + } + } + if (manufacturer_string && product_string) { + size_t name_size = (SDL_strlen(manufacturer_string) + 1 + SDL_strlen(product_string) + 1); + device->name = (char *)SDL_malloc(name_size); + if (device->name) { + SDL_snprintf(device->name, name_size, "%s %s", manufacturer_string, product_string); + } + } + if (manufacturer_string) { + SDL_free(manufacturer_string); + } + if (product_string) { + SDL_free(product_string); + } + } + if (!device->name) { + size_t name_size = (6 + 1 + 6 + 1); + device->name = (char *)SDL_malloc(name_size); + if (!device->name) { + SDL_free(device); + return; + } + SDL_snprintf(device->name, name_size, "0x%.4x/0x%.4x", info->vendor_id, info->product_id); + } + + device->driver = HIDAPI_GetDeviceDriver(device); + + if (device->driver) { + const char *name = device->driver->GetDeviceName(device->vendor_id, device->product_id); + if (name) { + SDL_free(device->name); + device->name = SDL_strdup(name); + } + } + + device->path = SDL_strdup(info->path); + if (!device->path) { + SDL_free(device->name); + SDL_free(device); + return; + } + +#ifdef DEBUG_HIDAPI + SDL_Log("Adding HIDAPI device '%s' VID 0x%.4x, PID 0x%.4x, version %d, interface %d, usage page 0x%.4x, usage 0x%.4x, driver = %s\n", device->name, device->vendor_id, device->product_id, device->version, device->interface_number, device->usage_page, device->usage, device->driver ? device->driver->hint : "NONE"); +#endif + + /* Add it to the list */ + if (last) { + last->next = device; + } else { + SDL_HIDAPI_devices = device; + } + + if (device->driver) { + /* It's a joystick! */ + device->instance_id = SDL_GetNextJoystickInstanceID(); + + ++SDL_HIDAPI_numjoysticks; + + SDL_PrivateJoystickAdded(device->instance_id); + } +} + + +static void +HIDAPI_DelDevice(SDL_HIDAPI_Device *device, SDL_bool send_event) +{ + SDL_HIDAPI_Device *curr, *last; + for (curr = SDL_HIDAPI_devices, last = NULL; curr; last = curr, curr = curr->next) { + if (curr == device) { + if (last) { + last->next = curr->next; + } else { + SDL_HIDAPI_devices = curr->next; + } + + if (device->driver && send_event) { + /* Need to decrement the joystick count before we post the event */ + --SDL_HIDAPI_numjoysticks; + + SDL_PrivateJoystickRemoved(device->instance_id); + } + + SDL_free(device->name); + SDL_free(device->path); + SDL_free(device); + return; + } + } +} + +static void +HIDAPI_UpdateDeviceList(void) +{ + SDL_HIDAPI_Device *device; + struct hid_device_info *devs, *info; + + /* Prepare the existing device list */ + device = SDL_HIDAPI_devices; + while (device) { + device->seen = SDL_FALSE; + device = device->next; + } + + /* Enumerate the devices */ + devs = hid_enumerate(0, 0); + if (devs) { + for (info = devs; info; info = info->next) { + device = HIDAPI_GetJoystickByInfo(info->path, info->vendor_id, info->product_id); + if (device) { + device->seen = SDL_TRUE; + } else { + HIDAPI_AddDevice(info); + } + } + hid_free_enumeration(devs); + } + + /* Remove any devices that weren't seen */ + device = SDL_HIDAPI_devices; + while (device) { + SDL_HIDAPI_Device *next = device->next; + + if (!device->seen) { + HIDAPI_DelDevice(device, SDL_TRUE); + } + device = next; + } +} + +SDL_bool +HIDAPI_IsDevicePresent(Uint16 vendor_id, Uint16 product_id, Uint16 version) +{ + SDL_HIDAPI_Device *device; + + /* Don't update the device list for devices we know aren't supported */ + if (!HIDAPI_IsDeviceSupported(vendor_id, product_id, version)) { + return SDL_FALSE; + } + + /* Make sure the device list is completely up to date when we check for device presence */ + HIDAPI_UpdateDeviceList(); + + device = SDL_HIDAPI_devices; + while (device) { + if (device->vendor_id == vendor_id && device->product_id == product_id && device->driver) { + return SDL_TRUE; + } + device = device->next; + } + return SDL_FALSE; +} + +static void +HIDAPI_JoystickDetect(void) +{ + HIDAPI_UpdateDiscovery(); + if (SDL_HIDAPI_discovery.m_bHaveDevicesChanged) { + /* FIXME: We probably need to schedule an update in a few seconds as well */ + HIDAPI_UpdateDeviceList(); + SDL_HIDAPI_discovery.m_bHaveDevicesChanged = SDL_FALSE; + } +} + +static const char * +HIDAPI_JoystickGetDeviceName(int device_index) +{ + return HIDAPI_GetJoystickByIndex(device_index)->name; +} + +static int +HIDAPI_JoystickGetDevicePlayerIndex(int device_index) +{ + return -1; +} + +static SDL_JoystickGUID +HIDAPI_JoystickGetDeviceGUID(int device_index) +{ + return HIDAPI_GetJoystickByIndex(device_index)->guid; +} + +static SDL_JoystickID +HIDAPI_JoystickGetDeviceInstanceID(int device_index) +{ + return HIDAPI_GetJoystickByIndex(device_index)->instance_id; +} + +static int +HIDAPI_JoystickOpen(SDL_Joystick * joystick, int device_index) +{ + SDL_HIDAPI_Device *device = HIDAPI_GetJoystickByIndex(device_index); + struct joystick_hwdata *hwdata; + + hwdata = (struct joystick_hwdata *)SDL_calloc(1, sizeof(*hwdata)); + if (!hwdata) { + return SDL_OutOfMemory(); + } + + hwdata->driver = device->driver; + hwdata->dev = hid_open_path(device->path, 0); + if (!hwdata->dev) { + SDL_free(hwdata); + return SDL_SetError("Couldn't open HID device %s", device->path); + } + hwdata->mutex = SDL_CreateMutex(); + + if (!device->driver->Init(joystick, hwdata->dev, device->vendor_id, device->product_id, &hwdata->context)) { + hid_close(hwdata->dev); + SDL_free(hwdata); + return -1; + } + + joystick->hwdata = hwdata; + return 0; +} + +static int +HIDAPI_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + struct joystick_hwdata *hwdata = joystick->hwdata; + SDL_HIDAPI_DeviceDriver *driver = hwdata->driver; + int result; + + SDL_LockMutex(hwdata->mutex); + result = driver->Rumble(joystick, hwdata->dev, hwdata->context, low_frequency_rumble, high_frequency_rumble, duration_ms); + SDL_UnlockMutex(hwdata->mutex); + return result; +} + +static void +HIDAPI_JoystickUpdate(SDL_Joystick * joystick) +{ + struct joystick_hwdata *hwdata = joystick->hwdata; + SDL_HIDAPI_DeviceDriver *driver = hwdata->driver; + SDL_bool succeeded; + + SDL_LockMutex(hwdata->mutex); + succeeded = driver->Update(joystick, hwdata->dev, hwdata->context); + SDL_UnlockMutex(hwdata->mutex); + + if (!succeeded) { + SDL_HIDAPI_Device *device; + for (device = SDL_HIDAPI_devices; device; device = device->next) { + if (device->instance_id == joystick->instance_id) { + HIDAPI_DelDevice(device, SDL_TRUE); + break; + } + } + } +} + +static void +HIDAPI_JoystickClose(SDL_Joystick * joystick) +{ + struct joystick_hwdata *hwdata = joystick->hwdata; + SDL_HIDAPI_DeviceDriver *driver = hwdata->driver; + driver->Quit(joystick, hwdata->dev, hwdata->context); + + hid_close(hwdata->dev); + SDL_DestroyMutex(hwdata->mutex); + SDL_free(hwdata); + joystick->hwdata = NULL; +} + +static void +HIDAPI_JoystickQuit(void) +{ + int i; + + HIDAPI_ShutdownDiscovery(); + + while (SDL_HIDAPI_devices) { + HIDAPI_DelDevice(SDL_HIDAPI_devices, SDL_FALSE); + } + for (i = 0; i < SDL_arraysize(SDL_HIDAPI_drivers); ++i) { + SDL_HIDAPI_DeviceDriver *driver = SDL_HIDAPI_drivers[i]; + SDL_DelHintCallback(driver->hint, SDL_HIDAPIDriverHintChanged, NULL); + } + SDL_DelHintCallback(SDL_HINT_JOYSTICK_HIDAPI, + SDL_HIDAPIDriverHintChanged, NULL); + SDL_HIDAPI_numjoysticks = 0; + + hid_exit(); +} + +SDL_JoystickDriver SDL_HIDAPI_JoystickDriver = +{ + HIDAPI_JoystickInit, + HIDAPI_JoystickGetCount, + HIDAPI_JoystickDetect, + HIDAPI_JoystickGetDeviceName, + HIDAPI_JoystickGetDevicePlayerIndex, + HIDAPI_JoystickGetDeviceGUID, + HIDAPI_JoystickGetDeviceInstanceID, + HIDAPI_JoystickOpen, + HIDAPI_JoystickRumble, + HIDAPI_JoystickUpdate, + HIDAPI_JoystickClose, + HIDAPI_JoystickQuit, +}; + +#endif /* SDL_JOYSTICK_HIDAPI */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapijoystick_c.h b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapijoystick_c.h new file mode 100644 index 000000000..18a448339 --- /dev/null +++ b/Engine/lib/sdl/src/joystick/hidapi/SDL_hidapijoystick_c.h @@ -0,0 +1,74 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "../../SDL_internal.h" + +#ifndef SDL_JOYSTICK_HIDAPI_H +#define SDL_JOYSTICK_HIDAPI_H + +#include "../../hidapi/hidapi/hidapi.h" + +/* This is the full set of HIDAPI drivers available */ +#define SDL_JOYSTICK_HIDAPI_PS4 +#define SDL_JOYSTICK_HIDAPI_SWITCH +#define SDL_JOYSTICK_HIDAPI_XBOX360 +#define SDL_JOYSTICK_HIDAPI_XBOXONE + +#ifdef __WINDOWS__ +/* On Windows, Xbox One controllers are handled by the Xbox 360 driver */ +#undef SDL_JOYSTICK_HIDAPI_XBOXONE +/* It turns out HIDAPI for Xbox controllers doesn't allow background input */ +#undef SDL_JOYSTICK_HIDAPI_XBOX360 +#endif + +#ifdef __MACOSX__ +/* On Mac OS X, Xbox One controllers are handled by the Xbox 360 driver */ +#undef SDL_JOYSTICK_HIDAPI_XBOXONE +#endif + +typedef struct _SDL_HIDAPI_DeviceDriver +{ + const char *hint; + SDL_bool enabled; + SDL_bool (*IsSupportedDevice)(Uint16 vendor_id, Uint16 product_id, Uint16 version, int interface_number); + const char *(*GetDeviceName)(Uint16 vendor_id, Uint16 product_id); + SDL_bool (*Init)(SDL_Joystick *joystick, hid_device *dev, Uint16 vendor_id, Uint16 product_id, void **context); + int (*Rumble)(SDL_Joystick *joystick, hid_device *dev, void *context, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms); + SDL_bool (*Update)(SDL_Joystick *joystick, hid_device *dev, void *context); + void (*Quit)(SDL_Joystick *joystick, hid_device *dev, void *context); + +} SDL_HIDAPI_DeviceDriver; + +/* HIDAPI device support */ +extern SDL_HIDAPI_DeviceDriver SDL_HIDAPI_DriverPS4; +extern SDL_HIDAPI_DeviceDriver SDL_HIDAPI_DriverSteam; +extern SDL_HIDAPI_DeviceDriver SDL_HIDAPI_DriverSwitch; +extern SDL_HIDAPI_DeviceDriver SDL_HIDAPI_DriverXbox360; +extern SDL_HIDAPI_DeviceDriver SDL_HIDAPI_DriverXboxOne; + +/* Return true if a HID device is present and supported as a joystick */ +extern SDL_bool HIDAPI_IsDevicePresent(Uint16 vendor_id, Uint16 product_id, Uint16 version); + +/* Return the name of an Xbox 360 or Xbox One controller */ +extern const char *HIDAPI_XboxControllerName(Uint16 vendor_id, Uint16 product_id); + +#endif /* SDL_JOYSTICK_HIDAPI_H */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/iphoneos/SDL_sysjoystick.m b/Engine/lib/sdl/src/joystick/iphoneos/SDL_sysjoystick.m index d6014980c..d85efad33 100644 --- a/Engine/lib/sdl/src/joystick/iphoneos/SDL_sysjoystick.m +++ b/Engine/lib/sdl/src/joystick/iphoneos/SDL_sysjoystick.m @@ -33,7 +33,6 @@ #include "SDL_stdinc.h" #include "../SDL_sysjoystick.h" #include "../SDL_joystick_c.h" -#include "../steam/SDL_steamcontroller.h" #if !SDL_EVENTS_DISABLED @@ -59,7 +58,6 @@ static CMMotionManager *motionManager = nil; static SDL_JoystickDeviceItem *deviceList = NULL; static int numjoysticks = 0; -static SDL_JoystickID instancecounter = 0; int SDL_AppleTVRemoteOpenedAsJoystick = 0; static SDL_JoystickDeviceItem * @@ -80,9 +78,16 @@ GetDeviceForIndex(int device_index) } static void -SDL_SYS_AddMFIJoystickDevice(SDL_JoystickDeviceItem *device, GCController *controller) +IOS_AddMFIJoystickDevice(SDL_JoystickDeviceItem *device, GCController *controller) { #ifdef SDL_JOYSTICK_MFI + const Uint16 VENDOR_APPLE = 0x05AC; + Uint16 *guid16 = (Uint16 *)device->guid.data; + Uint16 vendor = 0; + Uint16 product = 0; + Uint16 version = 0; + Uint8 subtype = 0; + const char *name = NULL; /* Explicitly retain the controller because SDL_JoystickDeviceItem is a * struct, and ARC doesn't work with structs. */ @@ -98,40 +103,26 @@ SDL_SYS_AddMFIJoystickDevice(SDL_JoystickDeviceItem *device, GCController *contr device->name = SDL_strdup(name); - device->guid.data[0] = 'M'; - device->guid.data[1] = 'F'; - device->guid.data[2] = 'i'; - device->guid.data[3] = 'G'; - device->guid.data[4] = 'a'; - device->guid.data[5] = 'm'; - device->guid.data[6] = 'e'; - device->guid.data[7] = 'p'; - device->guid.data[8] = 'a'; - device->guid.data[9] = 'd'; - - if (controller.extendedGamepad) { - device->guid.data[10] = 1; - } else if (controller.gamepad) { - device->guid.data[10] = 2; - } -#if TARGET_OS_TV - else if (controller.microGamepad) { - device->guid.data[10] = 3; - device->remote = SDL_TRUE; - } -#endif /* TARGET_OS_TV */ - if (controller.extendedGamepad) { + vendor = VENDOR_APPLE; + product = 1; + subtype = 1; device->naxes = 6; /* 2 thumbsticks and 2 triggers */ device->nhats = 1; /* d-pad */ device->nbuttons = 7; /* ABXY, shoulder buttons, pause button */ } else if (controller.gamepad) { + vendor = VENDOR_APPLE; + product = 2; + subtype = 2; device->naxes = 0; /* no traditional analog inputs */ device->nhats = 1; /* d-pad */ device->nbuttons = 7; /* ABXY, shoulder buttons, pause button */ } #if TARGET_OS_TV else if (controller.microGamepad) { + vendor = VENDOR_APPLE; + product = 3; + subtype = 3; device->naxes = 2; /* treat the touch surface as two axes */ device->nhats = 0; /* apparently the touch surface-as-dpad is buggy */ device->nbuttons = 3; /* AX, pause button */ @@ -140,6 +131,21 @@ SDL_SYS_AddMFIJoystickDevice(SDL_JoystickDeviceItem *device, GCController *contr } #endif /* TARGET_OS_TV */ + /* We only need 16 bits for each of these; space them out to fill 128. */ + /* Byteswap so devices get same GUID on little/big endian platforms. */ + *guid16++ = SDL_SwapLE16(SDL_HARDWARE_BUS_BLUETOOTH); + *guid16++ = 0; + *guid16++ = SDL_SwapLE16(vendor); + *guid16++ = 0; + *guid16++ = SDL_SwapLE16(product); + *guid16++ = 0; + *guid16++ = SDL_SwapLE16(version); + *guid16++ = 0; + + /* Note that this is an MFI controller and what subtype it is */ + device->guid.data[14] = 'm'; + device->guid.data[15] = subtype; + /* This will be set when the first button press of the controller is * detected. */ controller.playerIndex = -1; @@ -148,7 +154,7 @@ SDL_SYS_AddMFIJoystickDevice(SDL_JoystickDeviceItem *device, GCController *contr } static void -SDL_SYS_AddJoystickDevice(GCController *controller, SDL_bool accelerometer) +IOS_AddJoystickDevice(GCController *controller, SDL_bool accelerometer) { SDL_JoystickDeviceItem *device = deviceList; @@ -174,7 +180,7 @@ SDL_SYS_AddJoystickDevice(GCController *controller, SDL_bool accelerometer) } device->accelerometer = accelerometer; - device->instance_id = instancecounter++; + device->instance_id = SDL_GetNextJoystickInstanceID(); if (accelerometer) { #if TARGET_OS_TV @@ -190,7 +196,7 @@ SDL_SYS_AddJoystickDevice(GCController *controller, SDL_bool accelerometer) SDL_memcpy(&device->guid.data, device->name, SDL_min(sizeof(SDL_JoystickGUID), SDL_strlen(device->name))); #endif /* TARGET_OS_TV */ } else if (controller) { - SDL_SYS_AddMFIJoystickDevice(device, controller); + IOS_AddMFIJoystickDevice(device, controller); } if (deviceList == NULL) { @@ -205,11 +211,11 @@ SDL_SYS_AddJoystickDevice(GCController *controller, SDL_bool accelerometer) ++numjoysticks; - SDL_PrivateJoystickAdded(numjoysticks - 1); + SDL_PrivateJoystickAdded(device->instance_id); } static SDL_JoystickDeviceItem * -SDL_SYS_RemoveJoystickDevice(SDL_JoystickDeviceItem *device) +IOS_RemoveJoystickDevice(SDL_JoystickDeviceItem *device) { SDL_JoystickDeviceItem *prev = NULL; SDL_JoystickDeviceItem *next = NULL; @@ -278,78 +284,27 @@ SDL_AppleTVRemoteRotationHintChanged(void *udata, const char *name, const char * } #endif /* TARGET_OS_TV */ -static SDL_bool SteamControllerConnectedCallback(const char *name, SDL_JoystickGUID guid, int *device_instance) -{ - SDL_JoystickDeviceItem *device = (SDL_JoystickDeviceItem *)SDL_calloc(1, sizeof(SDL_JoystickDeviceItem)); - if (device == NULL) { - return SDL_FALSE; - } - - *device_instance = device->instance_id = instancecounter++; - device->name = SDL_strdup(name); - device->guid = guid; - SDL_GetSteamControllerInputs(&device->nbuttons, - &device->naxes, - &device->nhats); - device->m_bSteamController = SDL_TRUE; - - if (deviceList == NULL) { - deviceList = device; - } else { - SDL_JoystickDeviceItem *lastdevice = deviceList; - while (lastdevice->next != NULL) { - lastdevice = lastdevice->next; - } - lastdevice->next = device; - } - - ++numjoysticks; - - SDL_PrivateJoystickAdded(numjoysticks - 1); - - return SDL_TRUE; -} - -static void SteamControllerDisconnectedCallback(int device_instance) -{ - SDL_JoystickDeviceItem *item; - - for (item = deviceList; item; item = item->next) { - if (item->instance_id == device_instance) { - SDL_SYS_RemoveJoystickDevice(item); - break; - } - } -} - -/* Function to scan the system for joysticks. - * Joystick 0 should be the system default joystick. - * It should return 0, or -1 on an unrecoverable fatal error. - */ -int -SDL_SYS_JoystickInit(void) +static int +IOS_JoystickInit(void) { @autoreleasepool { NSNotificationCenter *center = [NSNotificationCenter defaultCenter]; - SDL_InitSteamControllers(SteamControllerConnectedCallback, - SteamControllerDisconnectedCallback); - #if !TARGET_OS_TV if (SDL_GetHintBoolean(SDL_HINT_ACCELEROMETER_AS_JOYSTICK, SDL_TRUE)) { /* Default behavior, accelerometer as joystick */ - SDL_SYS_AddJoystickDevice(nil, SDL_TRUE); + IOS_AddJoystickDevice(nil, SDL_TRUE); } #endif /* !TARGET_OS_TV */ #ifdef SDL_JOYSTICK_MFI /* GameController.framework was added in iOS 7. */ if (![GCController class]) { - return numjoysticks; + return 0; } for (GCController *controller in [GCController controllers]) { - SDL_SYS_AddJoystickDevice(controller, SDL_FALSE); + IOS_AddJoystickDevice(controller, SDL_FALSE); } #if TARGET_OS_TV @@ -362,7 +317,7 @@ SDL_SYS_JoystickInit(void) queue:nil usingBlock:^(NSNotification *note) { GCController *controller = note.object; - SDL_SYS_AddJoystickDevice(controller, SDL_FALSE); + IOS_AddJoystickDevice(controller, SDL_FALSE); }]; disconnectObserver = [center addObserverForName:GCControllerDidDisconnectNotification @@ -373,7 +328,7 @@ SDL_SYS_JoystickInit(void) SDL_JoystickDeviceItem *device = deviceList; while (device != NULL) { if (device->controller == controller) { - SDL_SYS_RemoveJoystickDevice(device); + IOS_RemoveJoystickDevice(device); break; } device = device->next; @@ -382,43 +337,55 @@ SDL_SYS_JoystickInit(void) #endif /* SDL_JOYSTICK_MFI */ } - return numjoysticks; + return 0; } -int -SDL_SYS_NumJoysticks(void) +static int +IOS_JoystickGetCount(void) { return numjoysticks; } -void -SDL_SYS_JoystickDetect(void) +static void +IOS_JoystickDetect(void) { - SDL_UpdateSteamControllers(); } -/* Function to get the device-dependent name of a joystick */ -const char * -SDL_SYS_JoystickNameForDeviceIndex(int device_index) +static const char * +IOS_JoystickGetDeviceName(int device_index) { SDL_JoystickDeviceItem *device = GetDeviceForIndex(device_index); return device ? device->name : "Unknown"; } -/* Function to perform the mapping from device index to the instance id for this index */ -SDL_JoystickID SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) +static int +IOS_JoystickGetDevicePlayerIndex(int device_index) { - SDL_JoystickDeviceItem *device = GetDeviceForIndex(device_index); - return device ? device->instance_id : 0; + return -1; } -/* Function to open a joystick for use. - The joystick to open is specified by the device index. - This should fill the nbuttons and naxes fields of the joystick structure. - It returns 0, or -1 if there is an error. - */ -int -SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) +static SDL_JoystickGUID +IOS_JoystickGetDeviceGUID( int device_index ) +{ + SDL_JoystickDeviceItem *device = GetDeviceForIndex(device_index); + SDL_JoystickGUID guid; + if (device) { + guid = device->guid; + } else { + SDL_zero(guid); + } + return guid; +} + +static SDL_JoystickID +IOS_JoystickGetDeviceInstanceID(int device_index) +{ + SDL_JoystickDeviceItem *device = GetDeviceForIndex(device_index); + return device ? device->instance_id : -1; +} + +static int +IOS_JoystickOpen(SDL_Joystick * joystick, int device_index) { SDL_JoystickDeviceItem *device = GetDeviceForIndex(device_index); if (device == NULL) { @@ -464,15 +431,8 @@ SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) return 0; } -/* Function to determine if this joystick is attached to the system right now */ -SDL_bool -SDL_SYS_JoystickAttached(SDL_Joystick *joystick) -{ - return joystick->hwdata != NULL; -} - static void -SDL_SYS_AccelerometerUpdate(SDL_Joystick * joystick) +IOS_AccelerometerUpdate(SDL_Joystick * joystick) { #if !TARGET_OS_TV const float maxgforce = SDL_IPHONE_MAX_GFORCE; @@ -517,7 +477,7 @@ SDL_SYS_AccelerometerUpdate(SDL_Joystick * joystick) #ifdef SDL_JOYSTICK_MFI static Uint8 -SDL_SYS_MFIJoystickHatStateForDPad(GCControllerDirectionPad *dpad) +IOS_MFIJoystickHatStateForDPad(GCControllerDirectionPad *dpad) { Uint8 hat = 0; @@ -542,7 +502,7 @@ SDL_SYS_MFIJoystickHatStateForDPad(GCControllerDirectionPad *dpad) #endif static void -SDL_SYS_MFIJoystickUpdate(SDL_Joystick * joystick) +IOS_MFIJoystickUpdate(SDL_Joystick * joystick) { #if SDL_JOYSTICK_MFI @autoreleasepool { @@ -572,7 +532,7 @@ SDL_SYS_MFIJoystickUpdate(SDL_Joystick * joystick) gamepad.rightShoulder.isPressed, }; - hatstate = SDL_SYS_MFIJoystickHatStateForDPad(gamepad.dpad); + hatstate = IOS_MFIJoystickHatStateForDPad(gamepad.dpad); for (i = 0; i < SDL_arraysize(axes); i++) { /* The triggers (axes 2 and 5) are resting at -32768 but SDL @@ -599,7 +559,7 @@ SDL_SYS_MFIJoystickUpdate(SDL_Joystick * joystick) gamepad.rightShoulder.isPressed, }; - hatstate = SDL_SYS_MFIJoystickHatStateForDPad(gamepad.dpad); + hatstate = IOS_MFIJoystickHatStateForDPad(gamepad.dpad); for (i = 0; i < SDL_arraysize(buttons); i++) { updateplayerindex |= (joystick->buttons[i] != buttons[i]); @@ -638,15 +598,11 @@ SDL_SYS_MFIJoystickUpdate(SDL_Joystick * joystick) } for (i = 0; i < joystick->hwdata->num_pause_presses; i++) { - /* The pause button is always last. */ - Uint8 pausebutton = joystick->nbuttons - 1; - + const Uint8 pausebutton = joystick->nbuttons - 1; /* The pause button is always last. */ SDL_PrivateJoystickButton(joystick, pausebutton, SDL_PRESSED); SDL_PrivateJoystickButton(joystick, pausebutton, SDL_RELEASED); - updateplayerindex = YES; } - joystick->hwdata->num_pause_presses = 0; if (updateplayerindex && controller.playerIndex == -1) { @@ -673,13 +629,14 @@ SDL_SYS_MFIJoystickUpdate(SDL_Joystick * joystick) #endif /* SDL_JOYSTICK_MFI */ } -/* Function to update the state of a joystick - called as a device poll. - * This function shouldn't update the joystick structure directly, - * but instead should call SDL_PrivateJoystick*() to deliver events - * and update joystick device state. - */ -void -SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) +static int +IOS_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + return SDL_Unsupported(); +} + +static void +IOS_JoystickUpdate(SDL_Joystick * joystick) { SDL_JoystickDeviceItem *device = joystick->hwdata; @@ -687,21 +644,15 @@ SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) return; } - if (device->m_bSteamController) { - SDL_UpdateSteamController(joystick); - return; - } - if (device->accelerometer) { - SDL_SYS_AccelerometerUpdate(joystick); + IOS_AccelerometerUpdate(joystick); } else if (device->controller) { - SDL_SYS_MFIJoystickUpdate(joystick); + IOS_MFIJoystickUpdate(joystick); } } -/* Function to close a joystick after use */ -void -SDL_SYS_JoystickClose(SDL_Joystick * joystick) +static void +IOS_JoystickClose(SDL_Joystick * joystick) { SDL_JoystickDeviceItem *device = joystick->hwdata; @@ -729,9 +680,8 @@ SDL_SYS_JoystickClose(SDL_Joystick * joystick) } } -/* Function to perform any system-specific joystick related cleanup */ -void -SDL_SYS_JoystickQuit(void) +static void +IOS_JoystickQuit(void) { @autoreleasepool { #ifdef SDL_JOYSTICK_MFI @@ -754,7 +704,7 @@ SDL_SYS_JoystickQuit(void) #endif /* SDL_JOYSTICK_MFI */ while (deviceList != NULL) { - SDL_SYS_RemoveJoystickDevice(deviceList); + IOS_RemoveJoystickDevice(deviceList); } #if !TARGET_OS_TV @@ -762,34 +712,23 @@ SDL_SYS_JoystickQuit(void) #endif /* !TARGET_OS_TV */ } - SDL_QuitSteamControllers(); - numjoysticks = 0; } -SDL_JoystickGUID -SDL_SYS_JoystickGetDeviceGUID( int device_index ) +SDL_JoystickDriver SDL_IOS_JoystickDriver = { - SDL_JoystickDeviceItem *device = GetDeviceForIndex(device_index); - SDL_JoystickGUID guid; - if (device) { - guid = device->guid; - } else { - SDL_zero(guid); - } - return guid; -} - -SDL_JoystickGUID -SDL_SYS_JoystickGetGUID(SDL_Joystick * joystick) -{ - SDL_JoystickGUID guid; - if (joystick->hwdata) { - guid = joystick->hwdata->guid; - } else { - SDL_zero(guid); - } - return guid; -} + IOS_JoystickInit, + IOS_JoystickGetCount, + IOS_JoystickDetect, + IOS_JoystickGetDeviceName, + IOS_JoystickGetDevicePlayerIndex, + IOS_JoystickGetDeviceGUID, + IOS_JoystickGetDeviceInstanceID, + IOS_JoystickOpen, + IOS_JoystickRumble, + IOS_JoystickUpdate, + IOS_JoystickClose, + IOS_JoystickQuit, +}; /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/iphoneos/SDL_sysjoystick_c.h b/Engine/lib/sdl/src/joystick/iphoneos/SDL_sysjoystick_c.h index 7be5b04a5..12aa296b1 100644 --- a/Engine/lib/sdl/src/joystick/iphoneos/SDL_sysjoystick_c.h +++ b/Engine/lib/sdl/src/joystick/iphoneos/SDL_sysjoystick_c.h @@ -35,6 +35,7 @@ typedef struct joystick_hwdata GCController __unsafe_unretained *controller; int num_pause_presses; + Uint32 pause_button_down_time; char *name; SDL_Joystick *joystick; @@ -45,9 +46,6 @@ typedef struct joystick_hwdata int nbuttons; int nhats; - /* Steam Controller support */ - SDL_bool m_bSteamController; - struct joystick_hwdata *next; } joystick_hwdata; diff --git a/Engine/lib/sdl/src/joystick/linux/SDL_sysjoystick.c b/Engine/lib/sdl/src/joystick/linux/SDL_sysjoystick.c index 457c4b85b..06a2d9a21 100644 --- a/Engine/lib/sdl/src/joystick/linux/SDL_sysjoystick.c +++ b/Engine/lib/sdl/src/joystick/linux/SDL_sysjoystick.c @@ -29,10 +29,11 @@ /* This is the Linux implementation of the SDL joystick API */ #include -#include +#include /* errno, strerror */ #include -#include #include /* For the definition of PATH_MAX */ +#include +#include #include #include "SDL_assert.h" @@ -43,6 +44,7 @@ #include "../SDL_joystick_c.h" #include "../steam/SDL_steamcontroller.h" #include "SDL_sysjoystick_c.h" +#include "../hidapi/SDL_hidapijoystick_c.h" /* This isn't defined in older Linux kernel headers */ #ifndef SYN_DROPPED @@ -76,7 +78,6 @@ typedef struct SDL_joylist_item static SDL_joylist_item *SDL_joylist = NULL; static SDL_joylist_item *SDL_joylist_tail = NULL; static int numjoysticks = 0; -static int instance_counter = 0; #define test_bit(nr, addr) \ @@ -209,6 +210,13 @@ IsJoystick(int fd, char *namebuf, const size_t namebuflen, SDL_JoystickGUID *gui return 0; } +#ifdef SDL_JOYSTICK_HIDAPI + if (HIDAPI_IsDevicePresent(inpid.vendor, inpid.product, inpid.version)) { + /* The HIDAPI driver is taking care of this device */ + return 0; + } +#endif + /* Check the joystick blacklist */ id = MAKE_VIDPID(inpid.vendor, inpid.product); for (i = 0; i < SDL_arraysize(joystick_blacklist); ++i) { @@ -239,8 +247,7 @@ IsJoystick(int fd, char *namebuf, const size_t namebuflen, SDL_JoystickGUID *gui SDL_strlcpy((char*)guid16, namebuf, sizeof(guid->data) - 4); } - if (SDL_IsGameControllerNameAndGUID(namebuf, *guid) && - SDL_ShouldIgnoreGameController(namebuf, *guid)) { + if (SDL_ShouldIgnoreJoystick(namebuf, *guid)) { return 0; } return 1; @@ -325,14 +332,14 @@ MaybeAddDevice(const char *path) item->name = SDL_strdup(namebuf); item->guid = guid; - if ( (item->path == NULL) || (item->name == NULL) ) { + if ((item->path == NULL) || (item->name == NULL)) { SDL_free(item->path); SDL_free(item->name); SDL_free(item); return -1; } - item->device_instance = instance_counter++; + item->device_instance = SDL_GetNextJoystickInstanceID(); if (SDL_joylist_tail == NULL) { SDL_joylist = SDL_joylist_tail = item; } else { @@ -343,7 +350,7 @@ MaybeAddDevice(const char *path) /* Need to increment the joystick count before we post the event */ ++numjoysticks; - SDL_PrivateJoystickAdded(numjoysticks - 1); + SDL_PrivateJoystickAdded(item->device_instance); return numjoysticks; } @@ -409,7 +416,7 @@ JoystickInitWithoutUdev(void) MaybeAddDevice(path); } - return numjoysticks; + return 0; } #endif @@ -430,7 +437,7 @@ JoystickInitWithUdev(void) /* Force a scan to build the initial device list */ SDL_UDEV_Scan(); - return numjoysticks; + return 0; } #endif @@ -455,7 +462,7 @@ static SDL_bool SteamControllerConnectedCallback(const char *name, SDL_JoystickG return SDL_FALSE; } - *device_instance = item->device_instance = instance_counter++; + *device_instance = item->device_instance = SDL_GetNextJoystickInstanceID(); if (SDL_joylist_tail == NULL) { SDL_joylist = SDL_joylist_tail = item; } else { @@ -466,7 +473,7 @@ static SDL_bool SteamControllerConnectedCallback(const char *name, SDL_JoystickG /* Need to increment the joystick count before we post the event */ ++numjoysticks; - SDL_PrivateJoystickAdded(numjoysticks - 1); + SDL_PrivateJoystickAdded(item->device_instance); return SDL_TRUE; } @@ -505,8 +512,8 @@ static void SteamControllerDisconnectedCallback(int device_instance) } } -int -SDL_SYS_JoystickInit(void) +static int +LINUX_JoystickInit(void) { /* First see if the user specified one or more joysticks to use */ if (SDL_getenv("SDL_JOYSTICK_DEVICE") != NULL) { @@ -534,14 +541,14 @@ SDL_SYS_JoystickInit(void) #endif } -int -SDL_SYS_NumJoysticks(void) +static int +LINUX_JoystickGetCount(void) { return numjoysticks; } -void -SDL_SYS_JoystickDetect(void) +static void +LINUX_JoystickDetect(void) { #if SDL_USE_LIBUDEV SDL_UDEV_Poll(); @@ -569,14 +576,27 @@ JoystickByDevIndex(int device_index) } /* Function to get the device-dependent name of a joystick */ -const char * -SDL_SYS_JoystickNameForDeviceIndex(int device_index) +static const char * +LINUX_JoystickGetDeviceName(int device_index) { return JoystickByDevIndex(device_index)->name; } +static int +LINUX_JoystickGetDevicePlayerIndex(int device_index) +{ + return -1; +} + +static SDL_JoystickGUID +LINUX_JoystickGetDeviceGUID( int device_index ) +{ + return JoystickByDevIndex(device_index)->guid; +} + /* Function to perform the mapping from device index to the instance id for this index */ -SDL_JoystickID SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) +static SDL_JoystickID +LINUX_JoystickGetDeviceInstanceID(int device_index) { return JoystickByDevIndex(device_index)->device_instance; } @@ -624,6 +644,7 @@ ConfigJoystick(SDL_Joystick * joystick, int fd) unsigned long keybit[NBITS(KEY_MAX)] = { 0 }; unsigned long absbit[NBITS(ABS_MAX)] = { 0 }; unsigned long relbit[NBITS(REL_MAX)] = { 0 }; + unsigned long ffbit[NBITS(FF_MAX)] = { 0 }; /* See if this device uses the new unified event API */ if ((ioctl(fd, EVIOCGBIT(EV_KEY, sizeof(keybit)), keybit) >= 0) && @@ -719,6 +740,15 @@ ConfigJoystick(SDL_Joystick * joystick, int fd) } } } + + if (ioctl(fd, EVIOCGBIT(EV_FF, sizeof(ffbit)), ffbit) >= 0) { + if (test_bit(FF_RUMBLE, ffbit)) { + joystick->hwdata->ff_rumble = SDL_TRUE; + } + if (test_bit(FF_SINE, ffbit)) { + joystick->hwdata->ff_sine = SDL_TRUE; + } + } } @@ -727,8 +757,8 @@ ConfigJoystick(SDL_Joystick * joystick, int fd) This should fill the nbuttons and naxes fields of the joystick structure. It returns 0, or -1 if there is an error. */ -int -SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) +static int +LINUX_JoystickOpen(SDL_Joystick * joystick, int device_index) { SDL_joylist_item *item = JoystickByDevIndex(device_index); @@ -744,6 +774,7 @@ SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) } joystick->hwdata->item = item; joystick->hwdata->guid = item->guid; + joystick->hwdata->effect.id = -1; joystick->hwdata->m_bSteamController = item->m_bSteamController; if (item->m_bSteamController) { @@ -752,7 +783,7 @@ SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) &joystick->naxes, &joystick->nhats); } else { - int fd = open(item->path, O_RDONLY, 0); + int fd = open(item->path, O_RDWR, 0); if (fd < 0) { SDL_free(joystick->hwdata); joystick->hwdata = NULL; @@ -784,10 +815,42 @@ SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) return (0); } -/* Function to determine if this joystick is attached to the system right now */ -SDL_bool SDL_SYS_JoystickAttached(SDL_Joystick *joystick) +static int +LINUX_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) { - return joystick->hwdata->item != NULL; + struct input_event event; + + if (joystick->hwdata->ff_rumble) { + struct ff_effect *effect = &joystick->hwdata->effect; + + effect->type = FF_RUMBLE; + effect->replay.length = SDL_min(duration_ms, 32767); + effect->u.rumble.strong_magnitude = low_frequency_rumble; + effect->u.rumble.weak_magnitude = high_frequency_rumble; + } else if (joystick->hwdata->ff_sine) { + /* Scale and average the two rumble strengths */ + Sint16 magnitude = (Sint16)(((low_frequency_rumble / 2) + (high_frequency_rumble / 2)) / 2); + struct ff_effect *effect = &joystick->hwdata->effect; + + effect->type = FF_PERIODIC; + effect->replay.length = SDL_min(duration_ms, 32767); + effect->u.periodic.waveform = FF_SINE; + effect->u.periodic.magnitude = magnitude; + } else { + return SDL_Unsupported(); + } + + if (ioctl(joystick->hwdata->fd, EVIOCSFF, &joystick->hwdata->effect) < 0) { + return SDL_SetError("Couldn't update rumble effect: %s", strerror(errno)); + } + + event.type = EV_FF; + event.code = joystick->hwdata->effect.id; + event.value = 1; + if (write(joystick->hwdata->fd, &event, sizeof(event)) < 0) { + return SDL_SetError("Couldn't start rumble effect: %s", strerror(errno)); + } + return 0; } static SDL_INLINE void @@ -963,8 +1026,8 @@ HandleInputEvents(SDL_Joystick * joystick) } } -void -SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) +static void +LINUX_JoystickUpdate(SDL_Joystick * joystick) { int i; @@ -990,10 +1053,14 @@ SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) } /* Function to close a joystick after use */ -void -SDL_SYS_JoystickClose(SDL_Joystick * joystick) +static void +LINUX_JoystickClose(SDL_Joystick * joystick) { if (joystick->hwdata) { + if (joystick->hwdata->effect.id >= 0) { + ioctl(joystick->hwdata->fd, EVIOCRMFF, joystick->hwdata->effect.id); + joystick->hwdata->effect.id = -1; + } if (joystick->hwdata->fd >= 0) { close(joystick->hwdata->fd); } @@ -1008,8 +1075,8 @@ SDL_SYS_JoystickClose(SDL_Joystick * joystick) } /* Function to perform any system-specific joystick related cleanup */ -void -SDL_SYS_JoystickQuit(void) +static void +LINUX_JoystickQuit(void) { SDL_joylist_item *item = NULL; SDL_joylist_item *next = NULL; @@ -1024,7 +1091,6 @@ SDL_SYS_JoystickQuit(void) SDL_joylist = SDL_joylist_tail = NULL; numjoysticks = 0; - instance_counter = 0; #if SDL_USE_LIBUDEV SDL_UDEV_DelCallback(joystick_udev_callback); @@ -1034,15 +1100,21 @@ SDL_SYS_JoystickQuit(void) SDL_QuitSteamControllers(); } -SDL_JoystickGUID SDL_SYS_JoystickGetDeviceGUID( int device_index ) +SDL_JoystickDriver SDL_LINUX_JoystickDriver = { - return JoystickByDevIndex(device_index)->guid; -} - -SDL_JoystickGUID SDL_SYS_JoystickGetGUID(SDL_Joystick * joystick) -{ - return joystick->hwdata->guid; -} + LINUX_JoystickInit, + LINUX_JoystickGetCount, + LINUX_JoystickDetect, + LINUX_JoystickGetDeviceName, + LINUX_JoystickGetDevicePlayerIndex, + LINUX_JoystickGetDeviceGUID, + LINUX_JoystickGetDeviceInstanceID, + LINUX_JoystickOpen, + LINUX_JoystickRumble, + LINUX_JoystickUpdate, + LINUX_JoystickClose, + LINUX_JoystickQuit, +}; #endif /* SDL_JOYSTICK_LINUX */ diff --git a/Engine/lib/sdl/src/joystick/linux/SDL_sysjoystick_c.h b/Engine/lib/sdl/src/joystick/linux/SDL_sysjoystick_c.h index d06b387ee..83d593760 100644 --- a/Engine/lib/sdl/src/joystick/linux/SDL_sysjoystick_c.h +++ b/Engine/lib/sdl/src/joystick/linux/SDL_sysjoystick_c.h @@ -19,6 +19,9 @@ 3. This notice may not be removed or altered from any source distribution. */ +#ifndef SDL_sysjoystick_c_h_ +#define SDL_sysjoystick_c_h_ + #include struct SDL_joylist_item; @@ -31,6 +34,10 @@ struct joystick_hwdata SDL_JoystickGUID guid; char *fname; /* Used in haptic subsystem */ + SDL_bool ff_rumble; + SDL_bool ff_sine; + struct ff_effect effect; + /* The current Linux joystick driver maps hats to two axes */ struct hwdata_hat { @@ -57,4 +64,6 @@ struct joystick_hwdata SDL_bool m_bSteamController; }; +#endif /* SDL_sysjoystick_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/psp/SDL_sysjoystick.c b/Engine/lib/sdl/src/joystick/psp/SDL_sysjoystick.c index 228cbb209..262da858e 100644 --- a/Engine/lib/sdl/src/joystick/psp/SDL_sysjoystick.c +++ b/Engine/lib/sdl/src/joystick/psp/SDL_sysjoystick.c @@ -177,12 +177,6 @@ int SDL_SYS_JoystickOpen(SDL_Joystick *joystick, int device_index) return 0; } -/* Function to determine if this joystick is attached to the system right now */ -SDL_bool SDL_SYS_JoystickAttached(SDL_Joystick *joystick) -{ - return SDL_TRUE; -} - /* Function to update the state of a joystick - called as a device poll. * This function shouldn't update the joystick structure directly, * but instead should call SDL_PrivateJoystick*() to deliver events diff --git a/Engine/lib/sdl/src/joystick/sort_controllers.py b/Engine/lib/sdl/src/joystick/sort_controllers.py index 47213c220..32f065a9e 100755 --- a/Engine/lib/sdl/src/joystick/sort_controllers.py +++ b/Engine/lib/sdl/src/joystick/sort_controllers.py @@ -27,16 +27,28 @@ def save_controller(line): def write_controllers(): global controllers global controller_guids - for entry in sorted(controllers, key=lambda entry: entry[2]): + # Check for duplicates + for entry in controllers: + if (entry[1] in controller_guids): + current_name = entry[2] + existing_name = controller_guids[entry[1]][2] + print("Warning: entry '%s' is duplicate of entry '%s'" % (current_name, existing_name)) + + if (not current_name.startswith("(DUPE)")): + entry[2] = "(DUPE) " + current_name + + if (not existing_name.startswith("(DUPE)")): + controller_guids[entry[1]][2] = "(DUPE) " + existing_name + + controller_guids[entry[1]] = entry + + for entry in sorted(controllers, key=lambda entry: entry[2]+"-"+entry[1]): line = "".join(entry) + "\n" line = line.replace("\t", " ") if not line.endswith(",\n") and not line.endswith("*/\n"): print("Warning: '%s' is missing a comma at the end of the line" % (line)) - if (entry[1] in controller_guids): - print("Warning: entry '%s' is duplicate of entry '%s'" % (entry[2], controller_guids[entry[1]][2])) - controller_guids[entry[1]] = entry - output.write(line) + controllers = [] controller_guids = {} diff --git a/Engine/lib/sdl/src/joystick/steam/SDL_steamcontroller.h b/Engine/lib/sdl/src/joystick/steam/SDL_steamcontroller.h index ce37b4de5..81b887973 100644 --- a/Engine/lib/sdl/src/joystick/steam/SDL_steamcontroller.h +++ b/Engine/lib/sdl/src/joystick/steam/SDL_steamcontroller.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_steamcontroller_h_ +#define SDL_steamcontroller_h_ + #include "../../SDL_internal.h" typedef SDL_bool (*SteamControllerConnectedCallback_t)(const char *name, SDL_JoystickGUID guid, int *device_instance); @@ -30,4 +34,6 @@ void SDL_UpdateSteamControllers(void); void SDL_UpdateSteamController(SDL_Joystick *joystick); void SDL_QuitSteamControllers(void); +#endif /* SDL_steamcontroller_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/windows/SDL_dinputjoystick.c b/Engine/lib/sdl/src/joystick/windows/SDL_dinputjoystick.c index cff868b28..67d7d25cd 100644 --- a/Engine/lib/sdl/src/joystick/windows/SDL_dinputjoystick.c +++ b/Engine/lib/sdl/src/joystick/windows/SDL_dinputjoystick.c @@ -27,6 +27,7 @@ #include "SDL_windowsjoystick_c.h" #include "SDL_dinputjoystick_c.h" #include "SDL_xinputjoystick_c.h" +#include "../hidapi/SDL_hidapijoystick_c.h" #ifndef DIDFT_OPTIONAL #define DIDFT_OPTIONAL 0x80000000 @@ -35,6 +36,8 @@ #define INPUT_QSIZE 32 /* Buffer up to 32 input messages */ #define JOY_AXIS_THRESHOLD (((SDL_JOYSTICK_AXIS_MAX)-(SDL_JOYSTICK_AXIS_MIN))/100) /* 1% motion */ +#define CONVERT_MAGNITUDE(x) (((x)*10000) / 0x7FFF) + /* external variables referenced. */ extern HWND SDL_HelperWindow; @@ -46,170 +49,170 @@ static UINT SDL_RawDevListCount = 0; /* Taken from Wine - Thanks! */ static DIOBJECTDATAFORMAT dfDIJoystick2[] = { - { &GUID_XAxis, DIJOFS_X, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_YAxis, DIJOFS_Y, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_ZAxis, DIJOFS_Z, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RxAxis, DIJOFS_RX, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RyAxis, DIJOFS_RY, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RzAxis, DIJOFS_RZ, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_Slider, DIJOFS_SLIDER(0), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_Slider, DIJOFS_SLIDER(1), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_POV, DIJOFS_POV(0), DIDFT_OPTIONAL | DIDFT_POV | DIDFT_ANYINSTANCE, 0 }, - { &GUID_POV, DIJOFS_POV(1), DIDFT_OPTIONAL | DIDFT_POV | DIDFT_ANYINSTANCE, 0 }, - { &GUID_POV, DIJOFS_POV(2), DIDFT_OPTIONAL | DIDFT_POV | DIDFT_ANYINSTANCE, 0 }, - { &GUID_POV, DIJOFS_POV(3), DIDFT_OPTIONAL | DIDFT_POV | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(0), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(1), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(2), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(3), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(4), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(5), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(6), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(7), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(8), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(9), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(10), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(11), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(12), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(13), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(14), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(15), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(16), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(17), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(18), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(19), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(20), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(21), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(22), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(23), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(24), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(25), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(26), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(27), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(28), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(29), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(30), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(31), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(32), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(33), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(34), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(35), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(36), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(37), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(38), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(39), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(40), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(41), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(42), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(43), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(44), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(45), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(46), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(47), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(48), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(49), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(50), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(51), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(52), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(53), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(54), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(55), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(56), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(57), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(58), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(59), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(60), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(61), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(62), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(63), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(64), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(65), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(66), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(67), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(68), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(69), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(70), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(71), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(72), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(73), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(74), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(75), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(76), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(77), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(78), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(79), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(80), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(81), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(82), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(83), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(84), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(85), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(86), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(87), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(88), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(89), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(90), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(91), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(92), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(93), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(94), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(95), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(96), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(97), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(98), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(99), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(100), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(101), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(102), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(103), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(104), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(105), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(106), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(107), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(108), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(109), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(110), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(111), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(112), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(113), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(114), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(115), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(116), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(117), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(118), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(119), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(120), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(121), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(122), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(123), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(124), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(125), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(126), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { NULL, DIJOFS_BUTTON(127), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, - { &GUID_XAxis, FIELD_OFFSET(DIJOYSTATE2, lVX), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_YAxis, FIELD_OFFSET(DIJOYSTATE2, lVY), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_ZAxis, FIELD_OFFSET(DIJOYSTATE2, lVZ), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RxAxis, FIELD_OFFSET(DIJOYSTATE2, lVRx), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RyAxis, FIELD_OFFSET(DIJOYSTATE2, lVRy), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RzAxis, FIELD_OFFSET(DIJOYSTATE2, lVRz), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglVSlider[0]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglVSlider[1]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_XAxis, FIELD_OFFSET(DIJOYSTATE2, lAX), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_YAxis, FIELD_OFFSET(DIJOYSTATE2, lAY), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_ZAxis, FIELD_OFFSET(DIJOYSTATE2, lAZ), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RxAxis, FIELD_OFFSET(DIJOYSTATE2, lARx), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RyAxis, FIELD_OFFSET(DIJOYSTATE2, lARy), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RzAxis, FIELD_OFFSET(DIJOYSTATE2, lARz), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglASlider[0]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglASlider[1]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_XAxis, FIELD_OFFSET(DIJOYSTATE2, lFX), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_YAxis, FIELD_OFFSET(DIJOYSTATE2, lFY), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_ZAxis, FIELD_OFFSET(DIJOYSTATE2, lFZ), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RxAxis, FIELD_OFFSET(DIJOYSTATE2, lFRx), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RyAxis, FIELD_OFFSET(DIJOYSTATE2, lFRy), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_RzAxis, FIELD_OFFSET(DIJOYSTATE2, lFRz), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglFSlider[0]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, - { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglFSlider[1]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_XAxis, DIJOFS_X, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_YAxis, DIJOFS_Y, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_ZAxis, DIJOFS_Z, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RxAxis, DIJOFS_RX, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RyAxis, DIJOFS_RY, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RzAxis, DIJOFS_RZ, DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_Slider, DIJOFS_SLIDER(0), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_Slider, DIJOFS_SLIDER(1), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_POV, DIJOFS_POV(0), DIDFT_OPTIONAL | DIDFT_POV | DIDFT_ANYINSTANCE, 0 }, + { &GUID_POV, DIJOFS_POV(1), DIDFT_OPTIONAL | DIDFT_POV | DIDFT_ANYINSTANCE, 0 }, + { &GUID_POV, DIJOFS_POV(2), DIDFT_OPTIONAL | DIDFT_POV | DIDFT_ANYINSTANCE, 0 }, + { &GUID_POV, DIJOFS_POV(3), DIDFT_OPTIONAL | DIDFT_POV | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(0), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(1), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(2), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(3), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(4), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(5), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(6), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(7), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(8), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(9), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(10), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(11), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(12), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(13), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(14), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(15), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(16), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(17), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(18), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(19), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(20), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(21), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(22), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(23), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(24), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(25), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(26), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(27), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(28), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(29), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(30), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(31), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(32), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(33), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(34), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(35), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(36), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(37), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(38), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(39), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(40), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(41), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(42), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(43), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(44), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(45), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(46), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(47), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(48), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(49), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(50), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(51), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(52), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(53), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(54), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(55), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(56), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(57), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(58), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(59), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(60), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(61), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(62), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(63), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(64), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(65), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(66), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(67), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(68), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(69), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(70), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(71), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(72), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(73), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(74), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(75), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(76), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(77), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(78), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(79), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(80), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(81), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(82), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(83), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(84), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(85), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(86), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(87), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(88), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(89), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(90), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(91), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(92), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(93), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(94), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(95), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(96), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(97), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(98), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(99), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(100), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(101), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(102), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(103), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(104), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(105), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(106), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(107), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(108), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(109), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(110), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(111), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(112), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(113), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(114), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(115), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(116), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(117), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(118), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(119), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(120), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(121), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(122), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(123), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(124), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(125), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(126), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { NULL, DIJOFS_BUTTON(127), DIDFT_OPTIONAL | DIDFT_BUTTON | DIDFT_ANYINSTANCE, 0 }, + { &GUID_XAxis, FIELD_OFFSET(DIJOYSTATE2, lVX), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_YAxis, FIELD_OFFSET(DIJOYSTATE2, lVY), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_ZAxis, FIELD_OFFSET(DIJOYSTATE2, lVZ), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RxAxis, FIELD_OFFSET(DIJOYSTATE2, lVRx), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RyAxis, FIELD_OFFSET(DIJOYSTATE2, lVRy), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RzAxis, FIELD_OFFSET(DIJOYSTATE2, lVRz), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglVSlider[0]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglVSlider[1]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_XAxis, FIELD_OFFSET(DIJOYSTATE2, lAX), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_YAxis, FIELD_OFFSET(DIJOYSTATE2, lAY), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_ZAxis, FIELD_OFFSET(DIJOYSTATE2, lAZ), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RxAxis, FIELD_OFFSET(DIJOYSTATE2, lARx), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RyAxis, FIELD_OFFSET(DIJOYSTATE2, lARy), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RzAxis, FIELD_OFFSET(DIJOYSTATE2, lARz), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglASlider[0]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglASlider[1]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_XAxis, FIELD_OFFSET(DIJOYSTATE2, lFX), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_YAxis, FIELD_OFFSET(DIJOYSTATE2, lFY), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_ZAxis, FIELD_OFFSET(DIJOYSTATE2, lFZ), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RxAxis, FIELD_OFFSET(DIJOYSTATE2, lFRx), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RyAxis, FIELD_OFFSET(DIJOYSTATE2, lFRy), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_RzAxis, FIELD_OFFSET(DIJOYSTATE2, lFRz), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglFSlider[0]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, + { &GUID_Slider, FIELD_OFFSET(DIJOYSTATE2, rglFSlider[1]), DIDFT_OPTIONAL | DIDFT_AXIS | DIDFT_ANYINSTANCE, 0 }, }; const DIDATAFORMAT SDL_c_dfDIJoystick2 = { @@ -311,6 +314,61 @@ SDL_IsXInputDevice(const GUID* pGuidProductFromDirectInput) return SDL_FALSE; } +void FreeRumbleEffectData(DIEFFECT *effect) +{ + if (!effect) { + return; + } + SDL_free(effect->rgdwAxes); + SDL_free(effect->rglDirection); + SDL_free(effect->lpvTypeSpecificParams); + SDL_free(effect); +} + +DIEFFECT *CreateRumbleEffectData(Sint16 magnitude, Uint32 duration_ms) +{ + DIEFFECT *effect; + DIPERIODIC *periodic; + + /* Create the effect */ + effect = (DIEFFECT *)SDL_calloc(1, sizeof(*effect)); + if (!effect) { + return NULL; + } + effect->dwSize = sizeof(*effect); + effect->dwGain = 10000; + effect->dwFlags = DIEFF_OBJECTOFFSETS; + effect->dwDuration = duration_ms * 1000; /* In microseconds. */ + effect->dwTriggerButton = DIEB_NOTRIGGER; + + effect->cAxes = 2; + effect->rgdwAxes = (DWORD *)SDL_calloc(effect->cAxes, sizeof(DWORD)); + if (!effect->rgdwAxes) { + FreeRumbleEffectData(effect); + return NULL; + } + + effect->rglDirection = (LONG *)SDL_calloc(effect->cAxes, sizeof(LONG)); + if (!effect->rglDirection) { + FreeRumbleEffectData(effect); + return NULL; + } + effect->dwFlags |= DIEFF_CARTESIAN; + + periodic = (DIPERIODIC *)SDL_calloc(1, sizeof(*periodic)); + if (!periodic) { + FreeRumbleEffectData(effect); + return NULL; + } + periodic->dwMagnitude = CONVERT_MAGNITUDE(magnitude); + periodic->dwPeriod = 1000000; + + effect->cbTypeSpecificParams = sizeof(*periodic); + effect->lpvTypeSpecificParams = periodic; + + return effect; +} + int SDL_DINPUT_JoystickInit(void) { @@ -348,28 +406,29 @@ SDL_DINPUT_JoystickInit(void) static BOOL CALLBACK EnumJoysticksCallback(const DIDEVICEINSTANCE * pdidInstance, VOID * pContext) { - const Uint16 BUS_USB = 0x03; - const Uint16 BUS_BLUETOOTH = 0x05; JoyStick_DeviceData *pNewJoystick; JoyStick_DeviceData *pPrevJoystick = NULL; const DWORD devtype = (pdidInstance->dwDevType & 0xFF); Uint16 *guid16; + Uint16 vendor = 0; + Uint16 product = 0; + Uint16 version = 0; WCHAR hidPath[MAX_PATH]; if (devtype == DI8DEVTYPE_SUPPLEMENTAL) { /* Add any supplemental devices that should be ignored here */ -#define MAKE_TABLE_ENTRY(VID, PID) ((((DWORD)PID)<<16)|VID) - static DWORD ignored_devices[] = { - MAKE_TABLE_ENTRY(0, 0) - }; +#define MAKE_TABLE_ENTRY(VID, PID) ((((DWORD)PID)<<16)|VID) + static DWORD ignored_devices[] = { + MAKE_TABLE_ENTRY(0, 0) + }; #undef MAKE_TABLE_ENTRY - unsigned int i; + unsigned int i; - for (i = 0; i < SDL_arraysize(ignored_devices); ++i) { - if (pdidInstance->guidProduct.Data1 == ignored_devices[i]) { - return DIENUM_CONTINUE; - } - } + for (i = 0; i < SDL_arraysize(ignored_devices); ++i) { + if (pdidInstance->guidProduct.Data1 == ignored_devices[i]) { + return DIENUM_CONTINUE; + } + } } if (SDL_IsXInputDevice(&pdidInstance->guidProduct)) { @@ -452,27 +511,38 @@ EnumJoysticksCallback(const DIDEVICEINSTANCE * pdidInstance, VOID * pContext) guid16 = (Uint16 *)pNewJoystick->guid.data; if (SDL_memcmp(&pdidInstance->guidProduct.Data4[2], "PIDVID", 6) == 0) { - *guid16++ = SDL_SwapLE16(BUS_USB); + vendor = (Uint16)LOWORD(pdidInstance->guidProduct.Data1); + product = (Uint16)HIWORD(pdidInstance->guidProduct.Data1); + version = 0; + + *guid16++ = SDL_SwapLE16(SDL_HARDWARE_BUS_USB); *guid16++ = 0; - *guid16++ = SDL_SwapLE16((Uint16)LOWORD(pdidInstance->guidProduct.Data1)); /* vendor */ + *guid16++ = SDL_SwapLE16(vendor); *guid16++ = 0; - *guid16++ = SDL_SwapLE16((Uint16)HIWORD(pdidInstance->guidProduct.Data1)); /* product */ + *guid16++ = SDL_SwapLE16(product); *guid16++ = 0; - *guid16++ = 0; /* version */ + *guid16++ = SDL_SwapLE16(version); *guid16++ = 0; } else { - *guid16++ = SDL_SwapLE16(BUS_BLUETOOTH); + *guid16++ = SDL_SwapLE16(SDL_HARDWARE_BUS_BLUETOOTH); *guid16++ = 0; SDL_strlcpy((char*)guid16, pNewJoystick->joystickname, sizeof(pNewJoystick->guid.data) - 4); } - if (SDL_IsGameControllerNameAndGUID(pNewJoystick->joystickname, pNewJoystick->guid) && - SDL_ShouldIgnoreGameController(pNewJoystick->joystickname, pNewJoystick->guid)) { + if (SDL_ShouldIgnoreJoystick(pNewJoystick->joystickname, pNewJoystick->guid)) { SDL_free(pNewJoystick); return DIENUM_CONTINUE; } - SDL_SYS_AddJoystickDevice(pNewJoystick); +#ifdef SDL_JOYSTICK_HIDAPI + if (HIDAPI_IsDevicePresent(vendor, product, 0)) { + /* The HIDAPI driver is taking care of this device */ + SDL_free(pNewJoystick); + return DIENUM_CONTINUE; + } +#endif + + WINDOWS_AddJoystickDevice(pNewJoystick); return DIENUM_CONTINUE; /* get next device, please */ } @@ -683,7 +753,6 @@ SDL_DINPUT_JoystickOpen(SDL_Joystick * joystick, JoyStick_DeviceData *joystickde /* Force capable? */ if (joystick->hwdata->Capabilities.dwFlags & DIDC_FORCEFEEDBACK) { - result = IDirectInputDevice8_Acquire(joystick->hwdata->InputDevice); if (FAILED(result)) { return SetDIerror("IDirectInputDevice8::Acquire", result); @@ -752,6 +821,89 @@ SDL_DINPUT_JoystickOpen(SDL_Joystick * joystick, JoyStick_DeviceData *joystickde return 0; } +static int +SDL_DINPUT_JoystickInitRumble(SDL_Joystick * joystick, Sint16 magnitude, Uint32 duration_ms) +{ + HRESULT result; + + /* Reset and then enable actuators */ + result = IDirectInputDevice8_SendForceFeedbackCommand(joystick->hwdata->InputDevice, DISFFC_RESET); + if (result == DIERR_INPUTLOST || result == DIERR_NOTEXCLUSIVEACQUIRED) { + result = IDirectInputDevice8_Acquire(joystick->hwdata->InputDevice); + if (SUCCEEDED(result)) { + result = IDirectInputDevice8_SendForceFeedbackCommand(joystick->hwdata->InputDevice, DISFFC_RESET); + } + } + if (FAILED(result)) { + return SetDIerror("IDirectInputDevice8::SendForceFeedbackCommand(DISFFC_RESET)", result); + } + + result = IDirectInputDevice8_SendForceFeedbackCommand(joystick->hwdata->InputDevice, DISFFC_SETACTUATORSON); + if (FAILED(result)) { + return SetDIerror("IDirectInputDevice8::SendForceFeedbackCommand(DISFFC_SETACTUATORSON)", result); + } + + /* Create the effect */ + joystick->hwdata->ffeffect = CreateRumbleEffectData(magnitude, duration_ms); + if (!joystick->hwdata->ffeffect) { + return SDL_OutOfMemory(); + } + + result = IDirectInputDevice8_CreateEffect(joystick->hwdata->InputDevice, &GUID_Sine, + joystick->hwdata->ffeffect, &joystick->hwdata->ffeffect_ref, NULL); + if (FAILED(result)) { + return SetDIerror("IDirectInputDevice8::CreateEffect", result); + } + return 0; +} + +int +SDL_DINPUT_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + HRESULT result; + + /* Scale and average the two rumble strengths */ + Sint16 magnitude = (Sint16)(((low_frequency_rumble / 2) + (high_frequency_rumble / 2)) / 2); + + if (!(joystick->hwdata->Capabilities.dwFlags & DIDC_FORCEFEEDBACK)) { + return SDL_Unsupported(); + } + + if (joystick->hwdata->ff_initialized) { + DIPERIODIC *periodic = ((DIPERIODIC *)joystick->hwdata->ffeffect->lpvTypeSpecificParams); + joystick->hwdata->ffeffect->dwDuration = duration_ms * 1000; /* In microseconds. */ + periodic->dwMagnitude = CONVERT_MAGNITUDE(magnitude); + + result = IDirectInputEffect_SetParameters(joystick->hwdata->ffeffect_ref, joystick->hwdata->ffeffect, (DIEP_DURATION | DIEP_TYPESPECIFICPARAMS)); + if (result == DIERR_INPUTLOST) { + result = IDirectInputDevice8_Acquire(joystick->hwdata->InputDevice); + if (SUCCEEDED(result)) { + result = IDirectInputEffect_SetParameters(joystick->hwdata->ffeffect_ref, joystick->hwdata->ffeffect, (DIEP_DURATION | DIEP_TYPESPECIFICPARAMS)); + } + } + if (FAILED(result)) { + return SetDIerror("IDirectInputDevice8::SetParameters", result); + } + } else { + if (SDL_DINPUT_JoystickInitRumble(joystick, magnitude, duration_ms) < 0) { + return -1; + } + joystick->hwdata->ff_initialized = SDL_TRUE; + } + + result = IDirectInputEffect_Start(joystick->hwdata->ffeffect_ref, 1, 0); + if (result == DIERR_INPUTLOST || result == DIERR_NOTEXCLUSIVEACQUIRED) { + result = IDirectInputDevice8_Acquire(joystick->hwdata->InputDevice); + if (SUCCEEDED(result)) { + result = IDirectInputEffect_Start(joystick->hwdata->ffeffect_ref, 1, 0); + } + } + if (FAILED(result)) { + return SetDIerror("IDirectInputDevice8::Start", result); + } + return 0; +} + static Uint8 TranslatePOV(DWORD value) { @@ -803,8 +955,6 @@ UpdateDINPUTJoystickState_Buffered(SDL_Joystick * joystick) /* Handle the events or punt */ if (FAILED(result)) { - joystick->hwdata->send_remove_event = SDL_TRUE; - joystick->hwdata->removed = SDL_TRUE; return; } @@ -859,8 +1009,6 @@ UpdateDINPUTJoystickState_Polled(SDL_Joystick * joystick) } if (result != DI_OK) { - joystick->hwdata->send_remove_event = SDL_TRUE; - joystick->hwdata->removed = SDL_TRUE; return; } @@ -933,8 +1081,17 @@ SDL_DINPUT_JoystickUpdate(SDL_Joystick * joystick) void SDL_DINPUT_JoystickClose(SDL_Joystick * joystick) { + if (joystick->hwdata->ffeffect_ref) { + IDirectInputEffect_Unload(joystick->hwdata->ffeffect_ref); + joystick->hwdata->ffeffect_ref = NULL; + } + if (joystick->hwdata->ffeffect) { + FreeRumbleEffectData(joystick->hwdata->ffeffect); + joystick->hwdata->ffeffect = NULL; + } IDirectInputDevice8_Unacquire(joystick->hwdata->InputDevice); IDirectInputDevice8_Release(joystick->hwdata->InputDevice); + joystick->hwdata->ff_initialized = SDL_FALSE; } void @@ -972,6 +1129,12 @@ SDL_DINPUT_JoystickOpen(SDL_Joystick * joystick, JoyStick_DeviceData *joystickde return SDL_Unsupported(); } +int +SDL_DINPUT_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + return SDL_Unsupported(); +} + void SDL_DINPUT_JoystickUpdate(SDL_Joystick * joystick) { diff --git a/Engine/lib/sdl/src/joystick/windows/SDL_dinputjoystick_c.h b/Engine/lib/sdl/src/joystick/windows/SDL_dinputjoystick_c.h index 5cc18903b..9f29fc751 100644 --- a/Engine/lib/sdl/src/joystick/windows/SDL_dinputjoystick_c.h +++ b/Engine/lib/sdl/src/joystick/windows/SDL_dinputjoystick_c.h @@ -23,6 +23,7 @@ extern int SDL_DINPUT_JoystickInit(void); extern void SDL_DINPUT_JoystickDetect(JoyStick_DeviceData **pContext); extern int SDL_DINPUT_JoystickOpen(SDL_Joystick * joystick, JoyStick_DeviceData *joystickdevice); +extern int SDL_DINPUT_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms); extern void SDL_DINPUT_JoystickUpdate(SDL_Joystick * joystick); extern void SDL_DINPUT_JoystickClose(SDL_Joystick * joystick); extern void SDL_DINPUT_JoystickQuit(void); diff --git a/Engine/lib/sdl/src/joystick/windows/SDL_mmjoystick.c b/Engine/lib/sdl/src/joystick/windows/SDL_mmjoystick.c index 9fa86656f..60e3fcb31 100644 --- a/Engine/lib/sdl/src/joystick/windows/SDL_mmjoystick.c +++ b/Engine/lib/sdl/src/joystick/windows/SDL_mmjoystick.c @@ -279,12 +279,6 @@ SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) return (0); } -/* Function to determine if this joystick is attached to the system right now */ -SDL_bool SDL_SYS_JoystickAttached(SDL_Joystick *joystick) -{ - return SDL_TRUE; -} - static Uint8 TranslatePOV(DWORD value) { @@ -366,10 +360,7 @@ SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) /* joystick hat events */ if (joyinfo.dwFlags & JOY_RETURNPOV) { - Uint8 pos; - - pos = TranslatePOV(joyinfo.dwPOV); - SDL_PrivateJoystickHat(joystick, 0, pos); + SDL_PrivateJoystickHat(joystick, 0, TranslatePOV(joyinfo.dwPOV)); } } diff --git a/Engine/lib/sdl/src/joystick/windows/SDL_windowsjoystick.c b/Engine/lib/sdl/src/joystick/windows/SDL_windowsjoystick.c index 45cbea688..71b72e610 100644 --- a/Engine/lib/sdl/src/joystick/windows/SDL_windowsjoystick.c +++ b/Engine/lib/sdl/src/joystick/windows/SDL_windowsjoystick.c @@ -61,7 +61,6 @@ /* local variables */ static SDL_bool s_bDeviceAdded = SDL_FALSE; static SDL_bool s_bDeviceRemoved = SDL_FALSE; -static SDL_JoystickID s_nInstanceID = -1; static SDL_cond *s_condJoystickThread = NULL; static SDL_mutex *s_mutexJoyStickEnum = NULL; static SDL_Thread *s_threadJoystick = NULL; @@ -256,7 +255,7 @@ SDL_JoystickThread(void *_data) /* WM_DEVICECHANGE not working, no XINPUT, no point in keeping thread alive */ break; #endif /* SDL_JOYSTICK_XINPUT */ - } + } if (s_bWindowsDeviceChanged || bXInputChanged) { s_bDeviceRemoved = SDL_TRUE; @@ -271,30 +270,33 @@ SDL_JoystickThread(void *_data) return 1; } -void SDL_SYS_AddJoystickDevice(JoyStick_DeviceData *device) +void WINDOWS_AddJoystickDevice(JoyStick_DeviceData *device) { device->send_add_event = SDL_TRUE; - device->nInstanceID = ++s_nInstanceID; + device->nInstanceID = SDL_GetNextJoystickInstanceID(); device->pNext = SYS_Joystick; SYS_Joystick = device; s_bDeviceAdded = SDL_TRUE; } +static void WINDOWS_JoystickDetect(void); +static void WINDOWS_JoystickQuit(void); + /* Function to scan the system for joysticks. * Joystick 0 should be the system default joystick. * It should return 0, or -1 on an unrecoverable fatal error. */ -int -SDL_SYS_JoystickInit(void) +static int +WINDOWS_JoystickInit(void) { if (SDL_DINPUT_JoystickInit() < 0) { - SDL_SYS_JoystickQuit(); + WINDOWS_JoystickQuit(); return -1; } if (SDL_XINPUT_JoystickInit() < 0) { - SDL_SYS_JoystickQuit(); + WINDOWS_JoystickQuit(); return -1; } @@ -302,19 +304,19 @@ SDL_SYS_JoystickInit(void) s_condJoystickThread = SDL_CreateCond(); s_bDeviceAdded = SDL_TRUE; /* force a scan of the system for joysticks this first time */ - SDL_SYS_JoystickDetect(); + WINDOWS_JoystickDetect(); if (!s_threadJoystick) { /* spin up the thread to detect hotplug of devices */ s_bJoystickThreadQuit = SDL_FALSE; s_threadJoystick = SDL_CreateThreadInternal(SDL_JoystickThread, "SDL_joystick", 64 * 1024, NULL); } - return SDL_SYS_NumJoysticks(); + return 0; } /* return the number of joysticks that are connected right now */ -int -SDL_SYS_NumJoysticks(void) +static int +WINDOWS_JoystickGetCount(void) { int nJoysticks = 0; JoyStick_DeviceData *device = SYS_Joystick; @@ -327,8 +329,8 @@ SDL_SYS_NumJoysticks(void) } /* detect any new joysticks being inserted into the system */ -void -SDL_SYS_JoystickDetect(void) +static void +WINDOWS_JoystickDetect(void) { JoyStick_DeviceData *pCurList = NULL; @@ -383,7 +385,7 @@ SDL_SYS_JoystickDetect(void) SDL_DINPUT_MaybeAddDevice(&pNewJoystick->dxdevice); } - SDL_PrivateJoystickAdded(device_index); + SDL_PrivateJoystickAdded(pNewJoystick->nInstanceID); pNewJoystick->send_add_event = SDL_FALSE; } @@ -394,8 +396,8 @@ SDL_SYS_JoystickDetect(void) } /* Function to get the device-dependent name of a joystick */ -const char * -SDL_SYS_JoystickNameForDeviceIndex(int device_index) +static const char * +WINDOWS_JoystickGetDeviceName(int device_index) { JoyStick_DeviceData *device = SYS_Joystick; @@ -405,9 +407,34 @@ SDL_SYS_JoystickNameForDeviceIndex(int device_index) return device->joystickname; } +static int +WINDOWS_JoystickGetDevicePlayerIndex(int device_index) +{ + JoyStick_DeviceData *device = SYS_Joystick; + int index; + + for (index = device_index; index > 0; index--) + device = device->pNext; + + return device->bXInputDevice ? (int)device->XInputUserId : -1; +} + +/* return the stable device guid for this device index */ +static SDL_JoystickGUID +WINDOWS_JoystickGetDeviceGUID(int device_index) +{ + JoyStick_DeviceData *device = SYS_Joystick; + int index; + + for (index = device_index; index > 0; index--) + device = device->pNext; + + return device->guid; +} + /* Function to perform the mapping between current device instance and this joysticks instance id */ -SDL_JoystickID -SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) +static SDL_JoystickID +WINDOWS_JoystickGetDeviceInstanceID(int device_index) { JoyStick_DeviceData *device = SYS_Joystick; int index; @@ -423,8 +450,8 @@ SDL_SYS_GetInstanceIdOfDeviceIndex(int device_index) This should fill the nbuttons and naxes fields of the joystick structure. It returns 0, or -1 if there is an error. */ -int -SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) +static int +WINDOWS_JoystickOpen(SDL_Joystick * joystick, int device_index) { JoyStick_DeviceData *joystickdevice = SYS_Joystick; @@ -448,17 +475,20 @@ SDL_SYS_JoystickOpen(SDL_Joystick * joystick, int device_index) } } -/* return true if this joystick is plugged in right now */ -SDL_bool -SDL_SYS_JoystickAttached(SDL_Joystick * joystick) +static int +WINDOWS_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) { - return joystick->hwdata && !joystick->hwdata->removed; + if (joystick->hwdata->bXInputDevice) { + return SDL_XINPUT_JoystickRumble(joystick, low_frequency_rumble, high_frequency_rumble, duration_ms); + } else { + return SDL_DINPUT_JoystickRumble(joystick, low_frequency_rumble, high_frequency_rumble, duration_ms); + } } -void -SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) +static void +WINDOWS_JoystickUpdate(SDL_Joystick * joystick) { - if (!joystick->hwdata || joystick->hwdata->removed) { + if (!joystick->hwdata) { return; } @@ -467,15 +497,11 @@ SDL_SYS_JoystickUpdate(SDL_Joystick * joystick) } else { SDL_DINPUT_JoystickUpdate(joystick); } - - if (joystick->hwdata->removed) { - joystick->force_recentering = SDL_TRUE; - } } /* Function to close a joystick after use */ -void -SDL_SYS_JoystickClose(SDL_Joystick * joystick) +static void +WINDOWS_JoystickClose(SDL_Joystick * joystick) { if (joystick->hwdata->bXInputDevice) { SDL_XINPUT_JoystickClose(joystick); @@ -487,8 +513,8 @@ SDL_SYS_JoystickClose(SDL_Joystick * joystick) } /* Function to perform any system-specific joystick related cleanup */ -void -SDL_SYS_JoystickQuit(void) +static void +WINDOWS_JoystickQuit(void) { JoyStick_DeviceData *device = SYS_Joystick; @@ -524,24 +550,21 @@ SDL_SYS_JoystickQuit(void) s_bDeviceRemoved = SDL_FALSE; } -/* return the stable device guid for this device index */ -SDL_JoystickGUID -SDL_SYS_JoystickGetDeviceGUID(int device_index) +SDL_JoystickDriver SDL_WINDOWS_JoystickDriver = { - JoyStick_DeviceData *device = SYS_Joystick; - int index; - - for (index = device_index; index > 0; index--) - device = device->pNext; - - return device->guid; -} - -SDL_JoystickGUID -SDL_SYS_JoystickGetGUID(SDL_Joystick * joystick) -{ - return joystick->hwdata->guid; -} + WINDOWS_JoystickInit, + WINDOWS_JoystickGetCount, + WINDOWS_JoystickDetect, + WINDOWS_JoystickGetDeviceName, + WINDOWS_JoystickGetDevicePlayerIndex, + WINDOWS_JoystickGetDeviceGUID, + WINDOWS_JoystickGetDeviceInstanceID, + WINDOWS_JoystickOpen, + WINDOWS_JoystickRumble, + WINDOWS_JoystickUpdate, + WINDOWS_JoystickClose, + WINDOWS_JoystickQuit, +}; #endif /* SDL_JOYSTICK_DINPUT || SDL_JOYSTICK_XINPUT */ diff --git a/Engine/lib/sdl/src/joystick/windows/SDL_windowsjoystick_c.h b/Engine/lib/sdl/src/joystick/windows/SDL_windowsjoystick_c.h index 01b8b3ab1..611f7e620 100644 --- a/Engine/lib/sdl/src/joystick/windows/SDL_windowsjoystick_c.h +++ b/Engine/lib/sdl/src/joystick/windows/SDL_windowsjoystick_c.h @@ -66,8 +66,7 @@ typedef struct input_t struct joystick_hwdata { SDL_JoystickGUID guid; - SDL_bool removed; - SDL_bool send_remove_event; + Uint32 rumble_expiration; #if SDL_JOYSTICK_DINPUT LPDIRECTINPUTDEVICE8 InputDevice; @@ -76,6 +75,9 @@ struct joystick_hwdata input_t Inputs[MAX_INPUTS]; int NumInputs; int NumSliders; + SDL_bool ff_initialized; + DIEFFECT *ffeffect; + LPDIRECTINPUTEFFECT ffeffect_ref; #endif SDL_bool bXInputDevice; /* SDL_TRUE if this device supports using the xinput API rather than DirectInput */ @@ -88,6 +90,6 @@ struct joystick_hwdata extern const DIDATAFORMAT SDL_c_dfDIJoystick2; #endif -extern void SDL_SYS_AddJoystickDevice(JoyStick_DeviceData *device); +extern void WINDOWS_AddJoystickDevice(JoyStick_DeviceData *device); /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/joystick/windows/SDL_xinputjoystick.c b/Engine/lib/sdl/src/joystick/windows/SDL_xinputjoystick.c index 823e7674f..6bbe47575 100644 --- a/Engine/lib/sdl/src/joystick/windows/SDL_xinputjoystick.c +++ b/Engine/lib/sdl/src/joystick/windows/SDL_xinputjoystick.c @@ -26,8 +26,10 @@ #include "SDL_assert.h" #include "SDL_hints.h" +#include "SDL_timer.h" #include "SDL_windowsjoystick_c.h" #include "SDL_xinputjoystick_c.h" +#include "../hidapi/SDL_hidapijoystick_c.h" /* * Internal stuff. @@ -186,6 +188,9 @@ GuessXInputDevice(Uint8 userid, Uint16 *pVID, Uint16 *pPID, Uint16 *pVersion) static void AddXInputDevice(Uint8 userid, BYTE SubType, JoyStick_DeviceData **pContext) { + Uint16 vendor = 0; + Uint16 product = 0; + Uint16 version = 0; JoyStick_DeviceData *pPrevJoystick = NULL; JoyStick_DeviceData *pNewJoystick = *pContext; @@ -229,15 +234,11 @@ AddXInputDevice(Uint8 userid, BYTE SubType, JoyStick_DeviceData **pContext) if (SDL_XInputUseOldJoystickMapping()) { SDL_zero(pNewJoystick->guid); } else { - const Uint16 BUS_USB = 0x03; - Uint16 vendor = 0; - Uint16 product = 0; - Uint16 version = 0; Uint16 *guid16 = (Uint16 *)pNewJoystick->guid.data; GuessXInputDevice(userid, &vendor, &product, &version); - *guid16++ = SDL_SwapLE16(BUS_USB); + *guid16++ = SDL_SwapLE16(SDL_HARDWARE_BUS_USB); *guid16++ = 0; *guid16++ = SDL_SwapLE16(vendor); *guid16++ = 0; @@ -253,12 +254,29 @@ AddXInputDevice(Uint8 userid, BYTE SubType, JoyStick_DeviceData **pContext) pNewJoystick->SubType = SubType; pNewJoystick->XInputUserId = userid; - if (SDL_ShouldIgnoreGameController(pNewJoystick->joystickname, pNewJoystick->guid)) { + if (SDL_ShouldIgnoreJoystick(pNewJoystick->joystickname, pNewJoystick->guid)) { SDL_free(pNewJoystick); return; } - SDL_SYS_AddJoystickDevice(pNewJoystick); +#ifdef SDL_JOYSTICK_HIDAPI + if (HIDAPI_IsDevicePresent(vendor, product, version)) { + /* The HIDAPI driver is taking care of this device */ + SDL_free(pNewJoystick); + return; + } +#endif + + WINDOWS_AddJoystickDevice(pNewJoystick); +} + +static void +DelXInputDevice(Uint8 userid) +{ + if (s_arrXInputDevicePath[userid]) { + SDL_free(s_arrXInputDevicePath[userid]); + s_arrXInputDevicePath[userid] = NULL; + } } void @@ -276,6 +294,8 @@ SDL_XINPUT_JoystickDetect(JoyStick_DeviceData **pContext) XINPUT_CAPABILITIES capabilities; if (XINPUTGETCAPABILITIES(userid, XINPUT_FLAG_GAMEPAD, &capabilities) == ERROR_SUCCESS) { AddXInputDevice(userid, capabilities.SubType, pContext); + } else { + DelXInputDevice(userid); } } } @@ -292,6 +312,8 @@ SDL_XINPUT_JoystickOpen(SDL_Joystick * joystick, JoyStick_DeviceData *joystickde SDL_assert(XINPUTSETSTATE); SDL_assert(userId < XUSER_MAX_COUNT); + joystick->player_index = userId; + joystick->hwdata->bXInputDevice = SDL_TRUE; if (XINPUTGETCAPABILITIES(userId, XINPUT_FLAG_GAMEPAD, &capabilities) != ERROR_SUCCESS) { @@ -384,12 +406,12 @@ UpdateXInputJoystickState(SDL_Joystick * joystick, XINPUT_STATE_EX *pXInputState Uint8 button; Uint8 hat = SDL_HAT_CENTERED; - SDL_PrivateJoystickAxis(joystick, 0, (Sint16)pXInputState->Gamepad.sThumbLX); - SDL_PrivateJoystickAxis(joystick, 1, (Sint16)(-SDL_max(-32767, pXInputState->Gamepad.sThumbLY))); - SDL_PrivateJoystickAxis(joystick, 2, (Sint16)(((int)pXInputState->Gamepad.bLeftTrigger * 65535 / 255) - 32768)); - SDL_PrivateJoystickAxis(joystick, 3, (Sint16)pXInputState->Gamepad.sThumbRX); - SDL_PrivateJoystickAxis(joystick, 4, (Sint16)(-SDL_max(-32767, pXInputState->Gamepad.sThumbRY))); - SDL_PrivateJoystickAxis(joystick, 5, (Sint16)(((int)pXInputState->Gamepad.bRightTrigger * 65535 / 255) - 32768)); + SDL_PrivateJoystickAxis(joystick, 0, pXInputState->Gamepad.sThumbLX); + SDL_PrivateJoystickAxis(joystick, 1, ~pXInputState->Gamepad.sThumbLY); + SDL_PrivateJoystickAxis(joystick, 2, ((int)pXInputState->Gamepad.bLeftTrigger * 257) - 32768); + SDL_PrivateJoystickAxis(joystick, 3, pXInputState->Gamepad.sThumbRX); + SDL_PrivateJoystickAxis(joystick, 4, ~pXInputState->Gamepad.sThumbRY); + SDL_PrivateJoystickAxis(joystick, 5, ((int)pXInputState->Gamepad.bRightTrigger * 257) - 32768); for (button = 0; button < SDL_arraysize(s_XInputButtons); ++button) { SDL_PrivateJoystickButton(joystick, button, (wButtons & s_XInputButtons[button]) ? SDL_PRESSED : SDL_RELEASED); @@ -412,6 +434,29 @@ UpdateXInputJoystickState(SDL_Joystick * joystick, XINPUT_STATE_EX *pXInputState UpdateXInputJoystickBatteryInformation(joystick, pBatteryInformation); } +int +SDL_XINPUT_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + XINPUT_VIBRATION XVibration; + + if (!XINPUTSETSTATE) { + return SDL_Unsupported(); + } + + XVibration.wLeftMotorSpeed = low_frequency_rumble; + XVibration.wRightMotorSpeed = high_frequency_rumble; + if (XINPUTSETSTATE(joystick->hwdata->userid, &XVibration) != ERROR_SUCCESS) { + return SDL_SetError("XInputSetState() failed"); + } + + if ((low_frequency_rumble || high_frequency_rumble) && duration_ms) { + joystick->hwdata->rumble_expiration = SDL_GetTicks() + duration_ms; + } else { + joystick->hwdata->rumble_expiration = 0; + } + return 0; +} + void SDL_XINPUT_JoystickUpdate(SDL_Joystick * joystick) { @@ -424,14 +469,6 @@ SDL_XINPUT_JoystickUpdate(SDL_Joystick * joystick) result = XINPUTGETSTATE(joystick->hwdata->userid, &XInputState); if (result == ERROR_DEVICE_NOT_CONNECTED) { - Uint8 userid = joystick->hwdata->userid; - - joystick->hwdata->send_remove_event = SDL_TRUE; - joystick->hwdata->removed = SDL_TRUE; - if (s_arrXInputDevicePath[userid]) { - SDL_free(s_arrXInputDevicePath[userid]); - s_arrXInputDevicePath[userid] = NULL; - } return; } @@ -449,6 +486,13 @@ SDL_XINPUT_JoystickUpdate(SDL_Joystick * joystick) } joystick->hwdata->dwPacketNumber = XInputState.dwPacketNumber; } + + if (joystick->hwdata->rumble_expiration) { + Uint32 now = SDL_GetTicks(); + if (SDL_TICKS_PASSED(now, joystick->hwdata->rumble_expiration)) { + SDL_XINPUT_JoystickRumble(joystick, 0, 0, 0); + } + } } void @@ -464,18 +508,6 @@ SDL_XINPUT_JoystickQuit(void) } } -SDL_bool -SDL_SYS_IsXInputGamepad_DeviceIndex(int device_index) -{ - JoyStick_DeviceData *device = SYS_Joystick; - int index; - - for (index = device_index; index > 0; index--) - device = device->pNext; - - return device->bXInputDevice; -} - #else /* !SDL_JOYSTICK_XINPUT */ typedef struct JoyStick_DeviceData JoyStick_DeviceData; @@ -502,6 +534,12 @@ SDL_XINPUT_JoystickOpen(SDL_Joystick * joystick, JoyStick_DeviceData *joystickde return SDL_Unsupported(); } +int +SDL_XINPUT_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms) +{ + return SDL_Unsupported(); +} + void SDL_XINPUT_JoystickUpdate(SDL_Joystick * joystick) { diff --git a/Engine/lib/sdl/src/joystick/windows/SDL_xinputjoystick_c.h b/Engine/lib/sdl/src/joystick/windows/SDL_xinputjoystick_c.h index 63616ee30..8d57b62f6 100644 --- a/Engine/lib/sdl/src/joystick/windows/SDL_xinputjoystick_c.h +++ b/Engine/lib/sdl/src/joystick/windows/SDL_xinputjoystick_c.h @@ -26,6 +26,7 @@ extern SDL_bool SDL_XINPUT_Enabled(void); extern int SDL_XINPUT_JoystickInit(void); extern void SDL_XINPUT_JoystickDetect(JoyStick_DeviceData **pContext); extern int SDL_XINPUT_JoystickOpen(SDL_Joystick * joystick, JoyStick_DeviceData *joystickdevice); +extern int SDL_XINPUT_JoystickRumble(SDL_Joystick * joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble, Uint32 duration_ms); extern void SDL_XINPUT_JoystickUpdate(SDL_Joystick * joystick); extern void SDL_XINPUT_JoystickClose(SDL_Joystick * joystick); extern void SDL_XINPUT_JoystickQuit(void); diff --git a/Engine/lib/sdl/src/libm/e_exp.c b/Engine/lib/sdl/src/libm/e_exp.c new file mode 100644 index 000000000..d8cd4a47a --- /dev/null +++ b/Engine/lib/sdl/src/libm/e_exp.c @@ -0,0 +1,187 @@ +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +/* __ieee754_exp(x) + * Returns the exponential of x. + * + * Method + * 1. Argument reduction: + * Reduce x to an r so that |r| <= 0.5*ln2 ~ 0.34658. + * Given x, find r and integer k such that + * + * x = k*ln2 + r, |r| <= 0.5*ln2. + * + * Here r will be represented as r = hi-lo for better + * accuracy. + * + * 2. Approximation of exp(r) by a special rational function on + * the interval [0,0.34658]: + * Write + * R(r**2) = r*(exp(r)+1)/(exp(r)-1) = 2 + r*r/6 - r**4/360 + ... + * We use a special Reme algorithm on [0,0.34658] to generate + * a polynomial of degree 5 to approximate R. The maximum error + * of this polynomial approximation is bounded by 2**-59. In + * other words, + * R(z) ~ 2.0 + P1*z + P2*z**2 + P3*z**3 + P4*z**4 + P5*z**5 + * (where z=r*r, and the values of P1 to P5 are listed below) + * and + * | 5 | -59 + * | 2.0+P1*z+...+P5*z - R(z) | <= 2 + * | | + * The computation of exp(r) thus becomes + * 2*r + * exp(r) = 1 + ------- + * R - r + * r*R1(r) + * = 1 + r + ----------- (for better accuracy) + * 2 - R1(r) + * where + * 2 4 10 + * R1(r) = r - (P1*r + P2*r + ... + P5*r ). + * + * 3. Scale back to obtain exp(x): + * From step 1, we have + * exp(x) = 2^k * exp(r) + * + * Special cases: + * exp(INF) is INF, exp(NaN) is NaN; + * exp(-INF) is 0, and + * for finite argument, only exp(0)=1 is exact. + * + * Accuracy: + * according to an error analysis, the error is always less than + * 1 ulp (unit in the last place). + * + * Misc. info. + * For IEEE double + * if x > 7.09782712893383973096e+02 then exp(x) overflow + * if x < -7.45133219101941108420e+02 then exp(x) underflow + * + * Constants: + * The hexadecimal values are the intended ones for the following + * constants. The decimal values may be used, provided that the + * compiler will convert from decimal to binary accurately enough + * to produce the hexadecimal values shown. + */ + +#include "math_libm.h" +#include "math_private.h" + +static const double +one = 1.0, +halF[2] = {0.5,-0.5,}, +huge = 1.0e+300, +twom1000= 9.33263618503218878990e-302, /* 2**-1000=0x01700000,0*/ +o_threshold= 7.09782712893383973096e+02, /* 0x40862E42, 0xFEFA39EF */ +u_threshold= -7.45133219101941108420e+02, /* 0xc0874910, 0xD52D3051 */ +ln2HI[2] ={ 6.93147180369123816490e-01, /* 0x3fe62e42, 0xfee00000 */ + -6.93147180369123816490e-01,},/* 0xbfe62e42, 0xfee00000 */ +ln2LO[2] ={ 1.90821492927058770002e-10, /* 0x3dea39ef, 0x35793c76 */ + -1.90821492927058770002e-10,},/* 0xbdea39ef, 0x35793c76 */ +invln2 = 1.44269504088896338700e+00, /* 0x3ff71547, 0x652b82fe */ +P1 = 1.66666666666666019037e-01, /* 0x3FC55555, 0x5555553E */ +P2 = -2.77777777770155933842e-03, /* 0xBF66C16C, 0x16BEBD93 */ +P3 = 6.61375632143793436117e-05, /* 0x3F11566A, 0xAF25DE2C */ +P4 = -1.65339022054652515390e-06, /* 0xBEBBBD41, 0xC5D26BF1 */ +P5 = 4.13813679705723846039e-08; /* 0x3E663769, 0x72BEA4D0 */ + +double __ieee754_exp(double x) /* default IEEE double exp */ +{ + double y; + double hi = 0.0; + double lo = 0.0; + double c; + double t; + int32_t k=0; + int32_t xsb; + u_int32_t hx; + + GET_HIGH_WORD(hx,x); + xsb = (hx>>31)&1; /* sign bit of x */ + hx &= 0x7fffffff; /* high word of |x| */ + + /* filter out non-finite argument */ + if(hx >= 0x40862E42) { /* if |x|>=709.78... */ + if(hx>=0x7ff00000) { + u_int32_t lx; + GET_LOW_WORD(lx,x); + if(((hx&0xfffff)|lx)!=0) + return x+x; /* NaN */ + else return (xsb==0)? x:0.0; /* exp(+-inf)={inf,0} */ + } + #if 1 + if(x > o_threshold) return huge*huge; /* overflow */ + #else /* !!! FIXME: check this: "huge * huge" is a compiler warning, maybe they wanted +Inf? */ + if(x > o_threshold) return INFINITY; /* overflow */ + #endif + + if(x < u_threshold) return twom1000*twom1000; /* underflow */ + } + + /* argument reduction */ + if(hx > 0x3fd62e42) { /* if |x| > 0.5 ln2 */ + if(hx < 0x3FF0A2B2) { /* and |x| < 1.5 ln2 */ + hi = x-ln2HI[xsb]; lo=ln2LO[xsb]; k = 1-xsb-xsb; + } else { + k = (int32_t) (invln2*x+halF[xsb]); + t = k; + hi = x - t*ln2HI[0]; /* t*ln2HI is exact here */ + lo = t*ln2LO[0]; + } + x = hi - lo; + } + else if(hx < 0x3e300000) { /* when |x|<2**-28 */ + if(huge+x>one) return one+x;/* trigger inexact */ + } + else k = 0; + + /* x is now in primary range */ + t = x*x; + c = x - t*(P1+t*(P2+t*(P3+t*(P4+t*P5)))); + if(k==0) return one-((x*c)/(c-2.0)-x); + else y = one-((lo-(x*c)/(2.0-c))-hi); + if(k >= -1021) { + u_int32_t hy; + GET_HIGH_WORD(hy,y); + SET_HIGH_WORD(y,hy+(k<<20)); /* add k to y's exponent */ + return y; + } else { + u_int32_t hy; + GET_HIGH_WORD(hy,y); + SET_HIGH_WORD(y,hy+((k+1000)<<20)); /* add k to y's exponent */ + return y*twom1000; + } +} + +/* + * wrapper exp(x) + */ +#ifndef _IEEE_LIBM +double exp(double x) +{ + static const double o_threshold = 7.09782712893383973096e+02; /* 0x40862E42, 0xFEFA39EF */ + static const double u_threshold = -7.45133219101941108420e+02; /* 0xc0874910, 0xD52D3051 */ + + double z = __ieee754_exp(x); + if (_LIB_VERSION == _IEEE_) + return z; + if (isfinite(x)) { + if (x > o_threshold) + return __kernel_standard(x, x, 6); /* exp overflow */ + if (x < u_threshold) + return __kernel_standard(x, x, 7); /* exp underflow */ + } + return z; +} +#else +strong_alias(__ieee754_exp, exp) +#endif +libm_hidden_def(exp) diff --git a/Engine/lib/sdl/src/libm/e_rem_pio2.c b/Engine/lib/sdl/src/libm/e_rem_pio2.c index df7c2b823..5e055d627 100644 --- a/Engine/lib/sdl/src/libm/e_rem_pio2.c +++ b/Engine/lib/sdl/src/libm/e_rem_pio2.c @@ -154,7 +154,7 @@ int32_t attribute_hidden __ieee754_rem_pio2(double x, double *y) } tx[2] = z; nx = 3; - while(tx[nx-1]==zero) nx--; /* skip zero term */ + while((nx > 0) && tx[nx-1]==zero) nx--; /* skip zero term */ n = __kernel_rem_pio2(tx,y,e0,nx,2,two_over_pi); if(hx<0) {y[0] = -y[0]; y[1] = -y[1]; return -n;} return n; diff --git a/Engine/lib/sdl/src/libm/k_rem_pio2.c b/Engine/lib/sdl/src/libm/k_rem_pio2.c index 7b04275c6..393db541d 100644 --- a/Engine/lib/sdl/src/libm/k_rem_pio2.c +++ b/Engine/lib/sdl/src/libm/k_rem_pio2.c @@ -128,6 +128,8 @@ #include "math_libm.h" #include "math_private.h" +#include "SDL_assert.h" + static const int init_jk[] = {2,3,4,6}; /* initial value for jk */ static const double PIo2[] = { @@ -147,13 +149,19 @@ one = 1.0, two24 = 1.67772160000000000000e+07, /* 0x41700000, 0x00000000 */ twon24 = 5.96046447753906250000e-08; /* 0x3E700000, 0x00000000 */ -int attribute_hidden __kernel_rem_pio2(double *x, double *y, int e0, int nx, int prec, const int32_t *ipio2) +int32_t attribute_hidden __kernel_rem_pio2(double *x, double *y, int e0, int nx, const unsigned int prec, const int32_t *ipio2) { int32_t jz,jx,jv,jp,jk,carry,n,iq[20],i,j,k,m,q0,ih; double z,fw,f[20],fq[20],q[20]; + if (nx < 1) { + return 0; + } + /* initialize jk*/ + SDL_assert(prec < SDL_arraysize(init_jk)); jk = init_jk[prec]; + SDL_assert(jk > 0); jp = jk; /* determine jx,jv,q0, note that 3>q0 */ @@ -164,6 +172,9 @@ int attribute_hidden __kernel_rem_pio2(double *x, double *y, int e0, int nx, int /* set up f[0] to f[jx+jk] where f[jx+jk] = ipio2[jv+jk] */ j = jv-jx; m = jx+jk; for(i=0;i<=m;i++,j++) f[i] = (j<0)? zero : (double) ipio2[j]; + if ((m+1) < SDL_arraysize(f)) { + SDL_memset(&f[m+1], 0, sizeof (f) - ((m+1) * sizeof (f[0]))); + } /* compute q[0],q[1],...q[jk] */ for (i=0;i<=jk;i++) { @@ -179,6 +190,9 @@ recompute: iq[i] = (int32_t)(z-two24*fw); z = q[j-1]+fw; } + if (jz < SDL_arraysize(iq)) { + SDL_memset(&iq[jz], 0, sizeof (q) - (jz * sizeof (iq[0]))); + } /* compute n */ z = scalbn(z,q0); /* actual value of z */ @@ -238,7 +252,8 @@ recompute: /* chop off zero terms */ if(z==0.0) { jz -= 1; q0 -= 24; - while(iq[jz]==0) { jz--; q0-=24;} + SDL_assert(jz >= 0); + while(iq[jz]==0) { jz--; SDL_assert(jz >= 0); q0-=24;} } else { /* break z into 24-bit if necessary */ z = scalbn(z,-q0); if(z>=two24) { @@ -260,6 +275,9 @@ recompute: for(fw=0.0,k=0;k<=jp&&k<=jz-i;k++) fw += PIo2[k]*q[i+k]; fq[jz-i] = fw; } + if ((jz+1) < SDL_arraysize(f)) { + SDL_memset(&fq[jz+1], 0, sizeof (fq) - ((jz+1) * sizeof (fq[0]))); + } /* compress fq[] into y[] */ switch(prec) { diff --git a/Engine/lib/sdl/src/libm/math_libm.h b/Engine/lib/sdl/src/libm/math_libm.h index eb7bdd597..3c751c5ed 100644 --- a/Engine/lib/sdl/src/libm/math_libm.h +++ b/Engine/lib/sdl/src/libm/math_libm.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef math_libm_h_ +#define math_libm_h_ + #include "../SDL_internal.h" /* Math routines from uClibc: http://www.uclibc.org */ @@ -26,6 +30,7 @@ double SDL_uclibc_atan(double x); double SDL_uclibc_atan2(double y, double x); double SDL_uclibc_copysign(double x, double y); double SDL_uclibc_cos(double x); +double SDL_uclibc_exp(double x); double SDL_uclibc_fabs(double x); double SDL_uclibc_floor(double x); double SDL_uclibc_fmod(double x, double y); @@ -37,4 +42,6 @@ double SDL_uclibc_sin(double x); double SDL_uclibc_sqrt(double x); double SDL_uclibc_tan(double x); +#endif /* math_libm_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/libm/math_private.h b/Engine/lib/sdl/src/libm/math_private.h index 1c0c8a4e9..d0ef66a6d 100644 --- a/Engine/lib/sdl/src/libm/math_private.h +++ b/Engine/lib/sdl/src/libm/math_private.h @@ -35,6 +35,7 @@ typedef unsigned int u_int32_t; #define __ieee754_atan2 SDL_uclibc_atan2 #define copysign SDL_uclibc_copysign #define cos SDL_uclibc_cos +#define __ieee754_exp SDL_uclibc_exp #define fabs SDL_uclibc_fabs #define floor SDL_uclibc_floor #define __ieee754_fmod SDL_uclibc_fmod @@ -206,7 +207,7 @@ __ieee754_sqrt(double) extern double __ieee754_jn(int, double) attribute_hidden; extern double __ieee754_yn(int, double) attribute_hidden; extern double __ieee754_remainder(double, double) attribute_hidden; - extern int __ieee754_rem_pio2(double, double *) attribute_hidden; + extern int32_t __ieee754_rem_pio2(double, double *) attribute_hidden; #if defined(_SCALB_INT) extern double __ieee754_scalb(double, int) attribute_hidden; #else @@ -220,7 +221,7 @@ __ieee754_sqrt(double) extern double __kernel_sin(double, double, int) attribute_hidden; extern double __kernel_cos(double, double) attribute_hidden; extern double __kernel_tan(double, double, int) attribute_hidden; - extern int __kernel_rem_pio2(double *, double *, int, int, int, - const int *) attribute_hidden; + extern int32_t __kernel_rem_pio2(double *, double *, int, int, const unsigned int, + const int32_t *) attribute_hidden; #endif /* _MATH_PRIVATE_H_ */ diff --git a/Engine/lib/sdl/src/main/haiku/SDL_BApp.h b/Engine/lib/sdl/src/main/haiku/SDL_BApp.h index ba3f9271c..7adbd007d 100644 --- a/Engine/lib/sdl/src/main/haiku/SDL_BApp.h +++ b/Engine/lib/sdl/src/main/haiku/SDL_BApp.h @@ -198,7 +198,7 @@ public: _current_context->UnlockGL(); _current_context = newContext; if (_current_context) - _current_context->LockGL(); + _current_context->LockGL(); } #endif @@ -231,7 +231,7 @@ private: SDL_SendMouseMotion(win, 0, 0, x, y); /* Tell the application that the mouse passed over, redraw needed */ - BE_UpdateWindowFramebuffer(NULL,win,NULL,-1); + HAIKU_UpdateWindowFramebuffer(NULL,win,NULL,-1); } void _HandleMouseButton(BMessage *msg) { @@ -274,11 +274,11 @@ private: } /* Make sure this isn't a repeated event (key pressed and held) */ - if(state == SDL_PRESSED && BE_GetKeyState(scancode) == SDL_PRESSED) { + if(state == SDL_PRESSED && HAIKU_GetKeyState(scancode) == SDL_PRESSED) { return; } - BE_SetKeyState(scancode, state); - SDL_SendKeyboardKey(state, BE_GetScancodeFromBeKey(scancode)); + HAIKU_SetKeyState(scancode, state); + SDL_SendKeyboardKey(state, HAIKU_GetScancodeFromBeKey(scancode)); if (state == SDL_PRESSED && SDL_EventState(SDL_TEXTINPUT, SDL_QUERY)) { const int8 *keyUtf8; diff --git a/Engine/lib/sdl/src/main/haiku/SDL_BeApp.cc b/Engine/lib/sdl/src/main/haiku/SDL_BeApp.cc index f4ee17951..cbd21293b 100644 --- a/Engine/lib/sdl/src/main/haiku/SDL_BeApp.cc +++ b/Engine/lib/sdl/src/main/haiku/SDL_BeApp.cc @@ -31,7 +31,7 @@ #include #include -#include "SDL_BApp.h" /* SDL_BApp class definition */ +#include "SDL_BApp.h" /* SDL_BApp class definition */ #include "SDL_BeApp.h" #include "SDL_timer.h" #include "SDL_error.h" @@ -53,24 +53,24 @@ StartBeApp(void *unused) { BApplication *App; - // default application signature - const char *signature = "application/x-SDL-executable"; - // dig resources for correct signature - image_info info; - int32 cookie = 0; - if (get_next_image_info(B_CURRENT_TEAM, &cookie, &info) == B_OK) { - BFile f(info.name, O_RDONLY); - if (f.InitCheck() == B_OK) { - BAppFileInfo app_info(&f); - if (app_info.InitCheck() == B_OK) { - char sig[B_MIME_TYPE_LENGTH]; - if (app_info.GetSignature(sig) == B_OK) - signature = strndup(sig, B_MIME_TYPE_LENGTH); - } - } - } + // default application signature + const char *signature = "application/x-SDL-executable"; + // dig resources for correct signature + image_info info; + int32 cookie = 0; + if (get_next_image_info(B_CURRENT_TEAM, &cookie, &info) == B_OK) { + BFile f(info.name, O_RDONLY); + if (f.InitCheck() == B_OK) { + BAppFileInfo app_info(&f); + if (app_info.InitCheck() == B_OK) { + char sig[B_MIME_TYPE_LENGTH]; + if (app_info.GetSignature(sig) == B_OK) + signature = strndup(sig, B_MIME_TYPE_LENGTH); + } + } + } - App = new SDL_BApp(signature); + App = new SDL_BApp(signature); App->Run(); delete App; @@ -144,12 +144,12 @@ SDL_QuitBeApp(void) /* SDL_BApp functions */ void SDL_BApp::ClearID(SDL_BWin *bwin) { - _SetSDLWindow(NULL, bwin->GetID()); - int32 i = _GetNumWindowSlots() - 1; - while(i >= 0 && GetSDLWindow(i) == NULL) { - _PopBackWindow(); - --i; - } + _SetSDLWindow(NULL, bwin->GetID()); + int32 i = _GetNumWindowSlots() - 1; + while(i >= 0 && GetSDLWindow(i) == NULL) { + _PopBackWindow(); + --i; + } } #endif /* __HAIKU__ */ diff --git a/Engine/lib/sdl/src/main/windows/SDL_windows_main.c b/Engine/lib/sdl/src/main/windows/SDL_windows_main.c index 5e643a44b..32f672760 100644 --- a/Engine/lib/sdl/src/main/windows/SDL_windows_main.c +++ b/Engine/lib/sdl/src/main/windows/SDL_windows_main.c @@ -116,50 +116,66 @@ OutOfMemory(void) # endif #endif -/* WinMain, main, and wmain eventually call into here. */ -static int -main_utf8(int argc, char *argv[]) -{ - SDL_SetMainReady(); - - /* Run the application main() code */ - return SDL_main(argc, argv); -} - /* Gets the arguments with GetCommandLine, converts them to argc and argv - and calls main_utf8 */ + and calls SDL_main */ static int main_getcmdline() { char **argv; int argc; - char *cmdline; + char *cmdline = NULL; int retval = 0; + int cmdalloc = 0; + const TCHAR *text = GetCommandLine(); + const TCHAR *ptr; + int argc_guess = 2; /* space for NULL and initial argument. */ + int rc; + + /* make a rough guess of command line arguments. Overestimates if there + are quoted things. */ + for (ptr = text; *ptr; ptr++) { + if ((*ptr == ' ') || (*ptr == '\t')) { + argc_guess++; + } + } - /* Grab the command line */ - TCHAR *text = GetCommandLine(); #if UNICODE - cmdline = WIN_StringToUTF8(text); + rc = WideCharToMultiByte(CP_UTF8, 0, text, -1, NULL, 0, NULL, NULL); + if (rc > 0) { + cmdalloc = rc + (sizeof (char *) * argc_guess); + argv = (char **) VirtualAlloc(NULL, cmdalloc, MEM_RESERVE|MEM_COMMIT, PAGE_READWRITE); + if (argv) { + int rc2; + cmdline = (char *) (argv + argc_guess); + rc2 = WideCharToMultiByte(CP_UTF8, 0, text, -1, cmdline, rc, NULL, NULL); + SDL_assert(rc2 == rc); + } + } #else /* !!! FIXME: are these in the system codepage? We need to convert to UTF-8. */ - cmdline = SDL_strdup(text); + rc = ((int) SDL_strlen(text)) + 1; + cmdalloc = rc + (sizeof (char *) * argc_guess); + argv = (char **) VirtualAlloc(NULL, cmdalloc, MEM_RESERVE|MEM_COMMIT, PAGE_READWRITE); + if (argv) { + cmdline = (char *) (argv + argc_guess); + SDL_strcpy(cmdline, text); + } #endif if (cmdline == NULL) { return OutOfMemory(); } /* Parse it into argv and argc */ - argc = ParseCommandLine(cmdline, NULL); - argv = SDL_stack_alloc(char *, argc + 1); - if (argv == NULL) { - return OutOfMemory(); - } - ParseCommandLine(cmdline, argv); + SDL_assert(ParseCommandLine(cmdline, NULL) <= argc_guess); + argc = ParseCommandLine(cmdline, argv); - retval = main_utf8(argc, argv); + SDL_SetMainReady(); - SDL_stack_free(argv); - SDL_free(cmdline); + /* Run the application main() code */ + retval = SDL_main(argc, argv); + + VirtualFree(argv, cmdalloc, MEM_DECOMMIT); + VirtualFree(argv, 0, MEM_RELEASE); return retval; } @@ -177,21 +193,7 @@ console_ansi_main(int argc, char *argv[]) int console_wmain(int argc, wchar_t *wargv[], wchar_t *wenvp) { - int retval = 0; - char **argv = SDL_stack_alloc(char*, argc + 1); - int i; - - for (i = 0; i < argc; ++i) { - argv[i] = WIN_StringToUTF8(wargv[i]); - } - argv[argc] = NULL; - - retval = main_utf8(argc, argv); - - /* !!! FIXME: we are leaking all the elements of argv we allocated. */ - SDL_stack_free(argv); - - return retval; + return main_getcmdline(); } #endif diff --git a/Engine/lib/sdl/src/main/windows/version.rc b/Engine/lib/sdl/src/main/windows/version.rc index 6d16ad57d..8eb8c8ab3 100644 --- a/Engine/lib/sdl/src/main/windows/version.rc +++ b/Engine/lib/sdl/src/main/windows/version.rc @@ -9,8 +9,8 @@ LANGUAGE LANG_ENGLISH, SUBLANG_ENGLISH_US // VS_VERSION_INFO VERSIONINFO - FILEVERSION 2,0,8,0 - PRODUCTVERSION 2,0,8,0 + FILEVERSION 2,0,9,0 + PRODUCTVERSION 2,0,9,0 FILEFLAGSMASK 0x3fL FILEFLAGS 0x0L FILEOS 0x40004L @@ -23,12 +23,12 @@ BEGIN BEGIN VALUE "CompanyName", "\0" VALUE "FileDescription", "SDL\0" - VALUE "FileVersion", "2, 0, 8, 0\0" + VALUE "FileVersion", "2, 0, 9, 0\0" VALUE "InternalName", "SDL\0" VALUE "LegalCopyright", "Copyright © 2018 Sam Lantinga\0" VALUE "OriginalFilename", "SDL2.dll\0" VALUE "ProductName", "Simple DirectMedia Layer\0" - VALUE "ProductVersion", "2, 0, 8, 0\0" + VALUE "ProductVersion", "2, 0, 9, 0\0" END END BLOCK "VarFileInfo" diff --git a/Engine/lib/sdl/src/power/SDL_power.c b/Engine/lib/sdl/src/power/SDL_power.c index e09e27ba2..de77c0951 100644 --- a/Engine/lib/sdl/src/power/SDL_power.c +++ b/Engine/lib/sdl/src/power/SDL_power.c @@ -42,11 +42,8 @@ SDL_GetPowerInfo_Hardwired(SDL_PowerState * state, int *seconds, int *percent) return SDL_TRUE; } #endif -#endif - static SDL_GetPowerInfo_Impl implementations[] = { -#ifndef SDL_POWER_DISABLED #ifdef SDL_POWER_LINUX /* in order of preference. More than could work. */ SDL_GetPowerInfo_Linux_org_freedesktop_upower, SDL_GetPowerInfo_Linux_sys_class_power_supply, @@ -81,31 +78,34 @@ static SDL_GetPowerInfo_Impl implementations[] = { #ifdef SDL_POWER_HARDWIRED SDL_GetPowerInfo_Hardwired, #endif -#endif }; +#endif SDL_PowerState SDL_GetPowerInfo(int *seconds, int *percent) { +#ifndef SDL_POWER_DISABLED const int total = sizeof(implementations) / sizeof(implementations[0]); - int _seconds, _percent; SDL_PowerState retval = SDL_POWERSTATE_UNKNOWN; int i; +#endif + int _seconds, _percent; /* Make these never NULL for platform-specific implementations. */ if (seconds == NULL) { seconds = &_seconds; } - if (percent == NULL) { percent = &_percent; } +#ifndef SDL_POWER_DISABLED for (i = 0; i < total; i++) { if (implementations[i](&retval, seconds, percent)) { return retval; } } +#endif /* nothing was definitive. */ *seconds = -1; diff --git a/Engine/lib/sdl/src/power/SDL_syspower.h b/Engine/lib/sdl/src/power/SDL_syspower.h index a9bf70cfc..afd6268fb 100644 --- a/Engine/lib/sdl/src/power/SDL_syspower.h +++ b/Engine/lib/sdl/src/power/SDL_syspower.h @@ -40,7 +40,9 @@ SDL_bool SDL_GetPowerInfo_Android(SDL_PowerState *, int *, int *); SDL_bool SDL_GetPowerInfo_PSP(SDL_PowerState *, int *, int *); SDL_bool SDL_GetPowerInfo_WinRT(SDL_PowerState *, int *, int *); SDL_bool SDL_GetPowerInfo_Emscripten(SDL_PowerState *, int *, int *); -SDL_bool SDL_GetPowerInfo_Hardwired(SDL_PowerState *, int *, int *); + +/* this one is static in SDL_power.c */ +/* SDL_bool SDL_GetPowerInfo_Hardwired(SDL_PowerState *, int *, int *);*/ #endif /* SDL_syspower_h_ */ diff --git a/Engine/lib/sdl/src/power/linux/SDL_syspower.c b/Engine/lib/sdl/src/power/linux/SDL_syspower.c index 105d5fe7f..e6c0c1c3d 100644 --- a/Engine/lib/sdl/src/power/linux/SDL_syspower.c +++ b/Engine/lib/sdl/src/power/linux/SDL_syspower.c @@ -608,12 +608,12 @@ SDL_GetPowerInfo_Linux_org_freedesktop_upower(SDL_PowerState *state, int *second { SDL_bool retval = SDL_FALSE; - #if SDL_USE_LIBDBUS +#if SDL_USE_LIBDBUS SDL_DBusContext *dbus = SDL_DBus_GetContext(); char **paths = NULL; int i, numpaths = 0; - if (!SDL_DBus_CallMethodOnConnection(dbus->system_conn, UPOWER_DBUS_NODE, UPOWER_DBUS_PATH, UPOWER_DBUS_INTERFACE, "EnumerateDevices", + if (!dbus || !SDL_DBus_CallMethodOnConnection(dbus->system_conn, UPOWER_DBUS_NODE, UPOWER_DBUS_PATH, UPOWER_DBUS_INTERFACE, "EnumerateDevices", DBUS_TYPE_INVALID, DBUS_TYPE_ARRAY, DBUS_TYPE_OBJECT_PATH, &paths, &numpaths, DBUS_TYPE_INVALID)) { return SDL_FALSE; /* try a different approach than UPower. */ @@ -631,7 +631,7 @@ SDL_GetPowerInfo_Linux_org_freedesktop_upower(SDL_PowerState *state, int *second if (dbus) { dbus->free_string_array(paths); } - #endif /* SDL_USE_LIBDBUS */ +#endif /* SDL_USE_LIBDBUS */ return retval; } diff --git a/Engine/lib/sdl/src/render/SDL_render.c b/Engine/lib/sdl/src/render/SDL_render.c index 8cd3a7bc1..4985b16ba 100644 --- a/Engine/lib/sdl/src/render/SDL_render.c +++ b/Engine/lib/sdl/src/render/SDL_render.c @@ -132,6 +132,16 @@ SDL_GetRenderDriverInfo(int index, SDL_RendererInfo * info) #endif } +static void GetWindowViewportValues(SDL_Renderer *renderer, int *logical_w, int *logical_h, SDL_Rect *viewport, SDL_FPoint *scale) +{ + SDL_LockMutex(renderer->target_mutex); + *logical_w = renderer->target ? renderer->logical_w_backup : renderer->logical_w; + *logical_h = renderer->target ? renderer->logical_h_backup : renderer->logical_h; + *viewport = renderer->target ? renderer->viewport_backup : renderer->viewport; + *scale = renderer->target ? renderer->scale_backup : renderer->scale; + SDL_UnlockMutex(renderer->target_mutex); +} + static int SDLCALL SDL_RendererEventWatch(void *userdata, SDL_Event *event) { @@ -197,35 +207,51 @@ SDL_RendererEventWatch(void *userdata, SDL_Event *event) } } else if (event->type == SDL_MOUSEMOTION) { SDL_Window *window = SDL_GetWindowFromID(event->motion.windowID); - if (renderer->logical_w && window == renderer->window) { - event->motion.x -= (int)(renderer->viewport.x * renderer->dpi_scale.x); - event->motion.y -= (int)(renderer->viewport.y * renderer->dpi_scale.y); - event->motion.x = (int)(event->motion.x / (renderer->scale.x * renderer->dpi_scale.x)); - event->motion.y = (int)(event->motion.y / (renderer->scale.y * renderer->dpi_scale.y)); - if (event->motion.xrel > 0) { - event->motion.xrel = SDL_max(1, (int)(event->motion.xrel / (renderer->scale.x * renderer->dpi_scale.x))); - } else if (event->motion.xrel < 0) { - event->motion.xrel = SDL_min(-1, (int)(event->motion.xrel / (renderer->scale.x * renderer->dpi_scale.x))); - } - if (event->motion.yrel > 0) { - event->motion.yrel = SDL_max(1, (int)(event->motion.yrel / (renderer->scale.y * renderer->dpi_scale.y))); - } else if (event->motion.yrel < 0) { - event->motion.yrel = SDL_min(-1, (int)(event->motion.yrel / (renderer->scale.y * renderer->dpi_scale.y))); + if (window == renderer->window) { + int logical_w, logical_h; + SDL_Rect viewport; + SDL_FPoint scale; + GetWindowViewportValues(renderer, &logical_w, &logical_h, &viewport, &scale); + if (logical_w) { + event->motion.x -= (int)(viewport.x * renderer->dpi_scale.x); + event->motion.y -= (int)(viewport.y * renderer->dpi_scale.y); + event->motion.x = (int)(event->motion.x / (scale.x * renderer->dpi_scale.x)); + event->motion.y = (int)(event->motion.y / (scale.y * renderer->dpi_scale.y)); + if (event->motion.xrel > 0) { + event->motion.xrel = SDL_max(1, (int)(event->motion.xrel / (scale.x * renderer->dpi_scale.x))); + } else if (event->motion.xrel < 0) { + event->motion.xrel = SDL_min(-1, (int)(event->motion.xrel / (scale.x * renderer->dpi_scale.x))); + } + if (event->motion.yrel > 0) { + event->motion.yrel = SDL_max(1, (int)(event->motion.yrel / (scale.y * renderer->dpi_scale.y))); + } else if (event->motion.yrel < 0) { + event->motion.yrel = SDL_min(-1, (int)(event->motion.yrel / (scale.y * renderer->dpi_scale.y))); + } } } } else if (event->type == SDL_MOUSEBUTTONDOWN || event->type == SDL_MOUSEBUTTONUP) { SDL_Window *window = SDL_GetWindowFromID(event->button.windowID); - if (renderer->logical_w && window == renderer->window) { - event->button.x -= (int)(renderer->viewport.x * renderer->dpi_scale.x); - event->button.y -= (int)(renderer->viewport.y * renderer->dpi_scale.y); - event->button.x = (int)(event->button.x / (renderer->scale.x * renderer->dpi_scale.x)); - event->button.y = (int)(event->button.y / (renderer->scale.y * renderer->dpi_scale.y)); + if (window == renderer->window) { + int logical_w, logical_h; + SDL_Rect viewport; + SDL_FPoint scale; + GetWindowViewportValues(renderer, &logical_w, &logical_h, &viewport, &scale); + if (logical_w) { + event->button.x -= (int)(viewport.x * renderer->dpi_scale.x); + event->button.y -= (int)(viewport.y * renderer->dpi_scale.y); + event->button.x = (int)(event->button.x / (scale.x * renderer->dpi_scale.x)); + event->button.y = (int)(event->button.y / (scale.y * renderer->dpi_scale.y)); + } } } else if (event->type == SDL_FINGERDOWN || event->type == SDL_FINGERUP || event->type == SDL_FINGERMOTION) { - if (renderer->logical_w) { + int logical_w, logical_h; + SDL_Rect viewport; + SDL_FPoint scale; + GetWindowViewportValues(renderer, &logical_w, &logical_h, &viewport, &scale); + if (logical_w) { int w = 1; int h = 1; SDL_GetRendererOutputSize(renderer, &w, &h); @@ -233,18 +259,18 @@ SDL_RendererEventWatch(void *userdata, SDL_Event *event) event->tfinger.x *= (w - 1); event->tfinger.y *= (h - 1); - event->tfinger.x -= (renderer->viewport.x * renderer->dpi_scale.x); - event->tfinger.y -= (renderer->viewport.y * renderer->dpi_scale.y); - event->tfinger.x = (event->tfinger.x / (renderer->scale.x * renderer->dpi_scale.x)); - event->tfinger.y = (event->tfinger.y / (renderer->scale.y * renderer->dpi_scale.y)); + event->tfinger.x -= (viewport.x * renderer->dpi_scale.x); + event->tfinger.y -= (viewport.y * renderer->dpi_scale.y); + event->tfinger.x = (event->tfinger.x / (scale.x * renderer->dpi_scale.x)); + event->tfinger.y = (event->tfinger.y / (scale.y * renderer->dpi_scale.y)); - if (renderer->logical_w > 1) { - event->tfinger.x = event->tfinger.x / (renderer->logical_w - 1); + if (logical_w > 1) { + event->tfinger.x = event->tfinger.x / (logical_w - 1); } else { event->tfinger.x = 0.5f; } - if (renderer->logical_h > 1) { - event->tfinger.y = event->tfinger.y / (renderer->logical_h - 1); + if (logical_h > 1) { + event->tfinger.y = event->tfinger.y / (logical_h - 1); } else { event->tfinger.y = 0.5f; } @@ -345,6 +371,7 @@ SDL_CreateRenderer(SDL_Window * window, int index, Uint32 flags) if (renderer) { renderer->magic = &renderer_magic; renderer->window = window; + renderer->target_mutex = SDL_CreateMutex(); renderer->scale.x = 1.0f; renderer->scale.y = 1.0f; renderer->dpi_scale.x = 1.0f; @@ -392,6 +419,7 @@ SDL_CreateSoftwareRenderer(SDL_Surface * surface) if (renderer) { renderer->magic = &renderer_magic; + renderer->target_mutex = SDL_CreateMutex(); renderer->scale.x = 1.0f; renderer->scale.y = 1.0f; @@ -493,6 +521,22 @@ GetClosestSupportedFormat(SDL_Renderer * renderer, Uint32 format) return renderer->info.texture_formats[0]; } + +static SDL_ScaleMode SDL_GetScaleMode(void) +{ + const char *hint = SDL_GetHint(SDL_HINT_RENDER_SCALE_QUALITY); + + if (!hint || SDL_strcasecmp(hint, "nearest") == 0) { + return SDL_ScaleModeNearest; + } else if (SDL_strcasecmp(hint, "linear") == 0) { + return SDL_ScaleModeLinear; + } else if (SDL_strcasecmp(hint, "best") == 0) { + return SDL_ScaleModeBest; + } else { + return (SDL_ScaleMode)SDL_atoi(hint); + } +} + SDL_Texture * SDL_CreateTexture(SDL_Renderer * renderer, Uint32 format, int access, int w, int h) { @@ -534,6 +578,7 @@ SDL_CreateTexture(SDL_Renderer * renderer, Uint32 format, int access, int w, int texture->g = 255; texture->b = 255; texture->a = 255; + texture->scaleMode = SDL_GetScaleMode(); texture->renderer = renderer; texture->next = renderer->textures; if (renderer->textures) { @@ -605,7 +650,7 @@ SDL_CreateTextureFromSurface(SDL_Renderer * renderer, SDL_Surface * surface) /* See what the best texture format is */ fmt = surface->format; - if (fmt->Amask || SDL_GetColorKey(surface, NULL) == 0) { + if (fmt->Amask || SDL_HasColorKey(surface)) { needAlpha = SDL_TRUE; } else { needAlpha = SDL_FALSE; @@ -664,7 +709,7 @@ SDL_CreateTextureFromSurface(SDL_Renderer * renderer, SDL_Surface * surface) SDL_GetSurfaceAlphaMod(surface, &a); SDL_SetTextureAlphaMod(texture, a); - if (SDL_GetColorKey(surface, NULL) == 0) { + if (SDL_HasColorKey(surface)) { /* We converted to a texture with alpha format */ SDL_SetTextureBlendMode(texture, SDL_BLENDMODE_BLEND); } else { @@ -1187,6 +1232,8 @@ SDL_SetRenderTarget(SDL_Renderer *renderer, SDL_Texture *texture) } } + SDL_LockMutex(renderer->target_mutex); + if (texture && !renderer->target) { /* Make a backup of the viewport */ renderer->viewport_backup = renderer->viewport; @@ -1199,6 +1246,7 @@ SDL_SetRenderTarget(SDL_Renderer *renderer, SDL_Texture *texture) renderer->target = texture; if (renderer->SetRenderTarget(renderer, texture) < 0) { + SDL_UnlockMutex(renderer->target_mutex); return -1; } @@ -1221,6 +1269,9 @@ SDL_SetRenderTarget(SDL_Renderer *renderer, SDL_Texture *texture) renderer->logical_w = renderer->logical_w_backup; renderer->logical_h = renderer->logical_h_backup; } + + SDL_UnlockMutex(renderer->target_mutex); + if (renderer->UpdateViewport(renderer) < 0) { return -1; } @@ -1259,6 +1310,7 @@ UpdateLogicalSize(SDL_Renderer *renderer) hint = SDL_GetHint(SDL_HINT_RENDER_LOGICAL_SIZE_MODE); if (hint && (*hint == '1' || SDL_strcasecmp(hint, "overscan") == 0)) { +#if SDL_VIDEO_RENDER_D3D SDL_bool overscan_supported = SDL_TRUE; /* Unfortunately, Direct3D 9 doesn't support negative viewport numbers which the overscan implementation relies on. @@ -1269,6 +1321,9 @@ UpdateLogicalSize(SDL_Renderer *renderer) if (overscan_supported) { scale_policy = 1; } +#else + scale_policy = 1; +#endif } want_aspect = (float)renderer->logical_w / renderer->logical_h; @@ -2086,6 +2141,10 @@ SDL_DestroyRenderer(SDL_Renderer * renderer) /* It's no longer magical... */ renderer->magic = NULL; + /* Free the target mutex */ + SDL_DestroyMutex(renderer->target_mutex); + renderer->target_mutex = NULL; + /* Free the renderer instance */ renderer->DestroyRenderer(renderer); } diff --git a/Engine/lib/sdl/src/render/SDL_sysrender.h b/Engine/lib/sdl/src/render/SDL_sysrender.h index f0f54c883..940bebcc1 100644 --- a/Engine/lib/sdl/src/render/SDL_sysrender.h +++ b/Engine/lib/sdl/src/render/SDL_sysrender.h @@ -25,12 +25,20 @@ #include "SDL_render.h" #include "SDL_events.h" +#include "SDL_mutex.h" #include "SDL_yuv_sw_c.h" /* The SDL 2D rendering system */ typedef struct SDL_RenderDriver SDL_RenderDriver; +typedef enum +{ + SDL_ScaleModeNearest, + SDL_ScaleModeLinear, + SDL_ScaleModeBest +} SDL_ScaleMode; + typedef struct { float x; @@ -55,6 +63,7 @@ struct SDL_Texture int h; /**< The height of the texture */ int modMode; /**< The texture modulation mode */ SDL_BlendMode blendMode; /**< The texture blend mode */ + SDL_ScaleMode scaleMode; /**< The texture scale mode */ Uint8 r, g, b, a; /**< Texture modulation values */ SDL_Renderer *renderer; @@ -164,6 +173,7 @@ struct SDL_Renderer /* The list of textures */ SDL_Texture *textures; SDL_Texture *target; + SDL_mutex *target_mutex; Uint8 r, g, b, a; /**< Color for drawing operations values */ SDL_BlendMode blendMode; /**< The drawing blend mode */ diff --git a/Engine/lib/sdl/src/render/SDL_yuv_sw_c.h b/Engine/lib/sdl/src/render/SDL_yuv_sw_c.h index 0dfb5db7f..34322f24c 100644 --- a/Engine/lib/sdl/src/render/SDL_yuv_sw_c.h +++ b/Engine/lib/sdl/src/render/SDL_yuv_sw_c.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_yuv_sw_c_h_ +#define SDL_yuv_sw_c_h_ + #include "../SDL_internal.h" #include "SDL_video.h" @@ -64,4 +68,6 @@ void SDL_SW_DestroyYUVTexture(SDL_SW_YUVTexture * swdata); #define USE_MMX_ASSEMBLY 1 #endif +#endif /* SDL_yuv_sw_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/render/direct3d/SDL_render_d3d.c b/Engine/lib/sdl/src/render/direct3d/SDL_render_d3d.c index d3be57132..69a9dff54 100644 --- a/Engine/lib/sdl/src/render/direct3d/SDL_render_d3d.c +++ b/Engine/lib/sdl/src/render/direct3d/SDL_render_d3d.c @@ -664,18 +664,6 @@ D3D_SupportsBlendMode(SDL_Renderer * renderer, SDL_BlendMode blendMode) return SDL_TRUE; } -static D3DTEXTUREFILTERTYPE -GetScaleQuality(void) -{ - const char *hint = SDL_GetHint(SDL_HINT_RENDER_SCALE_QUALITY); - - if (!hint || *hint == '0' || SDL_strcasecmp(hint, "nearest") == 0) { - return D3DTEXF_POINT; - } else /* if (*hint == '1' || SDL_strcasecmp(hint, "linear") == 0) */ { - return D3DTEXF_LINEAR; - } -} - static int D3D_CreateTextureRep(IDirect3DDevice9 *device, D3D_TextureRep *texture, DWORD usage, Uint32 format, D3DFORMAT d3dfmt, int w, int h) { @@ -829,7 +817,7 @@ D3D_CreateTexture(SDL_Renderer * renderer, SDL_Texture * texture) if (!texturedata) { return SDL_OutOfMemory(); } - texturedata->scaleMode = GetScaleQuality(); + texturedata->scaleMode = (texture->scaleMode == SDL_ScaleModeNearest) ? D3DTEXF_POINT : D3DTEXF_LINEAR; texture->driverdata = texturedata; diff --git a/Engine/lib/sdl/src/render/direct3d11/SDL_render_d3d11.c b/Engine/lib/sdl/src/render/direct3d11/SDL_render_d3d11.c index e0ccfc450..7a370392f 100644 --- a/Engine/lib/sdl/src/render/direct3d11/SDL_render_d3d11.c +++ b/Engine/lib/sdl/src/render/direct3d11/SDL_render_d3d11.c @@ -1161,17 +1161,6 @@ D3D11_SupportsBlendMode(SDL_Renderer * renderer, SDL_BlendMode blendMode) return SDL_TRUE; } -static D3D11_FILTER -GetScaleQuality(void) -{ - const char *hint = SDL_GetHint(SDL_HINT_RENDER_SCALE_QUALITY); - if (!hint || *hint == '0' || SDL_strcasecmp(hint, "nearest") == 0) { - return D3D11_FILTER_MIN_MAG_MIP_POINT; - } else /* if (*hint == '1' || SDL_strcasecmp(hint, "linear") == 0) */ { - return D3D11_FILTER_MIN_MAG_MIP_LINEAR; - } -} - static int D3D11_CreateTexture(SDL_Renderer * renderer, SDL_Texture * texture) { @@ -1192,7 +1181,7 @@ D3D11_CreateTexture(SDL_Renderer * renderer, SDL_Texture * texture) SDL_OutOfMemory(); return -1; } - textureData->scaleMode = GetScaleQuality(); + textureData->scaleMode = (texture->scaleMode == SDL_ScaleModeNearest) ? D3D11_FILTER_MIN_MAG_MIP_POINT : D3D11_FILTER_MIN_MAG_MIP_LINEAR; texture->driverdata = textureData; @@ -2234,8 +2223,6 @@ static int D3D11_RenderCopy(SDL_Renderer * renderer, SDL_Texture * texture, const SDL_Rect * srcrect, const SDL_FRect * dstrect) { - D3D11_RenderData *rendererData = (D3D11_RenderData *) renderer->driverdata; - D3D11_TextureData *textureData = (D3D11_TextureData *) texture->driverdata; float minu, maxu, minv, maxv; Float4 color; VertexPositionColor vertices[4]; @@ -2307,8 +2294,6 @@ D3D11_RenderCopyEx(SDL_Renderer * renderer, SDL_Texture * texture, const SDL_Rect * srcrect, const SDL_FRect * dstrect, const double angle, const SDL_FPoint * center, const SDL_RendererFlip flip) { - D3D11_RenderData *rendererData = (D3D11_RenderData *) renderer->driverdata; - D3D11_TextureData *textureData = (D3D11_TextureData *) texture->driverdata; float minu, maxu, minv, maxv; Float4 color; Float4X4 modelMatrix; diff --git a/Engine/lib/sdl/src/render/metal/SDL_render_metal.m b/Engine/lib/sdl/src/render/metal/SDL_render_metal.m index f7af72d42..5b4d8ea27 100644 --- a/Engine/lib/sdl/src/render/metal/SDL_render_metal.m +++ b/Engine/lib/sdl/src/render/metal/SDL_render_metal.m @@ -752,8 +752,7 @@ METAL_ActivateRenderCommandEncoder(SDL_Renderer * renderer, MTLLoadAction load) static void METAL_WindowEvent(SDL_Renderer * renderer, const SDL_WindowEvent *event) { - if (event->event == SDL_WINDOWEVENT_SIZE_CHANGED || - event->event == SDL_WINDOWEVENT_SHOWN || + if (event->event == SDL_WINDOWEVENT_SHOWN || event->event == SDL_WINDOWEVENT_HIDDEN) { // !!! FIXME: write me } @@ -844,17 +843,24 @@ METAL_CreateTexture(SDL_Renderer * renderer, SDL_Texture * texture) mtltexdesc.height = (texture->h + 1) / 2; mtltexdesc.textureType = MTLTextureType2DArray; mtltexdesc.arrayLength = 2; - mtltexture_uv = [data.mtldevice newTextureWithDescriptor:mtltexdesc]; } else if (nv12) { mtltexdesc.pixelFormat = MTLPixelFormatRG8Unorm; mtltexdesc.width = (texture->w + 1) / 2; mtltexdesc.height = (texture->h + 1) / 2; + } + + if (yuv || nv12) { mtltexture_uv = [data.mtldevice newTextureWithDescriptor:mtltexdesc]; + if (mtltexture_uv == nil) { +#if !__has_feature(objc_arc) + [mtltexture release]; +#endif + return SDL_SetError("Texture allocation failed"); + } } METAL_TextureData *texturedata = [[METAL_TextureData alloc] init]; - const char *hint = SDL_GetHint(SDL_HINT_RENDER_SCALE_QUALITY); - if (!hint || *hint == '0' || SDL_strcasecmp(hint, "nearest") == 0) { + if (texture->scaleMode == SDL_ScaleModeNearest) { texturedata.mtlsampler = data.mtlsamplernearest; } else { texturedata.mtlsampler = data.mtlsamplerlinear; @@ -957,8 +963,8 @@ METAL_UpdateTextureYUV(SDL_Renderer * renderer, SDL_Texture * texture, const Uint8 *Vplane, int Vpitch) { @autoreleasepool { METAL_TextureData *texturedata = (__bridge METAL_TextureData *)texture->driverdata; - int Uslice = texture->format == SDL_PIXELFORMAT_YV12 ? 1 : 0; - int Vslice = texture->format == SDL_PIXELFORMAT_YV12 ? 0 : 1; + const int Uslice = 0; + const int Vslice = 1; /* Bail out if we're supposed to update an empty rectangle */ if (rect->w <= 0 || rect->h <= 0) { @@ -1345,10 +1351,23 @@ static int METAL_RenderReadPixels(SDL_Renderer * renderer, const SDL_Rect * rect, Uint32 pixel_format, void * pixels, int pitch) { @autoreleasepool { + METAL_RenderData *data = (__bridge METAL_RenderData *) renderer->driverdata; + + /* Make sure we have a valid MTLTexture to read from, and an active command + * buffer we can wait for. */ METAL_ActivateRenderCommandEncoder(renderer, MTLLoadActionLoad); - // !!! FIXME: this probably needs to commit the current command buffer, and probably waitUntilCompleted - METAL_RenderData *data = (__bridge METAL_RenderData *) renderer->driverdata; + /* Wait for the current command buffer to finish, so we don't read from the + * texture before the GPU finishes rendering to it. */ + if (data.mtlcmdencoder) { + [data.mtlcmdencoder endEncoding]; + [data.mtlcmdbuffer commit]; + [data.mtlcmdbuffer waitUntilCompleted]; + + data.mtlcmdencoder = nil; + data.mtlcmdbuffer = nil; + } + id mtltexture = data.mtlpassdesc.colorAttachments[0].texture; MTLRegion mtlregion = MTLRegionMake2D(rect->x, rect->y, rect->w, rect->h); @@ -1364,6 +1383,13 @@ METAL_RenderReadPixels(SDL_Renderer * renderer, const SDL_Rect * rect, const Uint32 temp_format = (mtltexture.pixelFormat == MTLPixelFormatBGRA8Unorm) ? SDL_PIXELFORMAT_ARGB8888 : SDL_PIXELFORMAT_ABGR8888; const int status = SDL_ConvertPixels(rect->w, rect->h, temp_format, temp_pixels, temp_pitch, pixel_format, pixels, pitch); SDL_free(temp_pixels); + + /* Set up an active command buffer and encoder once we're done. It will use + * the same texture that was active before (even if it's part of the swap + * chain), since we didn't clear that when waiting for the command buffer to + * complete. */ + METAL_ActivateRenderCommandEncoder(renderer, MTLLoadActionLoad); + return status; }} diff --git a/Engine/lib/sdl/src/render/opengl/SDL_render_gl.c b/Engine/lib/sdl/src/render/opengl/SDL_render_gl.c index 1c379eb25..f3e8326ac 100644 --- a/Engine/lib/sdl/src/render/opengl/SDL_render_gl.c +++ b/Engine/lib/sdl/src/render/opengl/SDL_render_gl.c @@ -703,18 +703,6 @@ convert_format(GL_RenderData *renderdata, Uint32 pixel_format, return SDL_TRUE; } -static GLenum -GetScaleQuality(void) -{ - const char *hint = SDL_GetHint(SDL_HINT_RENDER_SCALE_QUALITY); - - if (!hint || *hint == '0' || SDL_strcasecmp(hint, "nearest") == 0) { - return GL_NEAREST; - } else { - return GL_LINEAR; - } -} - static int GL_CreateTexture(SDL_Renderer * renderer, SDL_Texture * texture) { @@ -803,7 +791,7 @@ GL_CreateTexture(SDL_Renderer * renderer, SDL_Texture * texture) data->format = format; data->formattype = type; - scaleMode = GetScaleQuality(); + scaleMode = (texture->scaleMode == SDL_ScaleModeNearest) ? GL_NEAREST : GL_LINEAR; renderdata->glEnable(data->type); renderdata->glBindTexture(data->type, data->texture); renderdata->glTexParameteri(data->type, GL_TEXTURE_MIN_FILTER, scaleMode); diff --git a/Engine/lib/sdl/src/render/opengl/SDL_shaders_gl.h b/Engine/lib/sdl/src/render/opengl/SDL_shaders_gl.h index 9805c599c..36975214d 100644 --- a/Engine/lib/sdl/src/render/opengl/SDL_shaders_gl.h +++ b/Engine/lib/sdl/src/render/opengl/SDL_shaders_gl.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_shaders_gl_h_ +#define SDL_shaders_gl_h_ + #include "../../SDL_internal.h" /* OpenGL shader implementation */ @@ -44,4 +48,6 @@ extern GL_ShaderContext * GL_CreateShaderContext(void); extern void GL_SelectShader(GL_ShaderContext *ctx, GL_Shader shader); extern void GL_DestroyShaderContext(GL_ShaderContext *ctx); +#endif /* SDL_shaders_gl_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/render/opengles/SDL_render_gles.c b/Engine/lib/sdl/src/render/opengles/SDL_render_gles.c index d6bfca5d0..4007dff58 100644 --- a/Engine/lib/sdl/src/render/opengles/SDL_render_gles.c +++ b/Engine/lib/sdl/src/render/opengles/SDL_render_gles.c @@ -524,18 +524,6 @@ power_of_2(int input) return value; } -static GLenum -GetScaleQuality(void) -{ - const char *hint = SDL_GetHint(SDL_HINT_RENDER_SCALE_QUALITY); - - if (!hint || *hint == '0' || SDL_strcasecmp(hint, "nearest") == 0) { - return GL_NEAREST; - } else { - return GL_LINEAR; - } -} - static int GLES_CreateTexture(SDL_Renderer * renderer, SDL_Texture * texture) { @@ -603,7 +591,7 @@ GLES_CreateTexture(SDL_Renderer * renderer, SDL_Texture * texture) data->format = format; data->formattype = type; - scaleMode = GetScaleQuality(); + scaleMode = (texture->scaleMode == SDL_ScaleModeNearest) ? GL_NEAREST : GL_LINEAR; renderdata->glBindTexture(data->type, data->texture); renderdata->glTexParameteri(data->type, GL_TEXTURE_MIN_FILTER, scaleMode); renderdata->glTexParameteri(data->type, GL_TEXTURE_MAG_FILTER, scaleMode); diff --git a/Engine/lib/sdl/src/render/opengles2/SDL_render_gles2.c b/Engine/lib/sdl/src/render/opengles2/SDL_render_gles2.c index 0cd388c37..fe51b9aaf 100644 --- a/Engine/lib/sdl/src/render/opengles2/SDL_render_gles2.c +++ b/Engine/lib/sdl/src/render/opengles2/SDL_render_gles2.c @@ -553,18 +553,6 @@ static void GLES2_UnlockTexture(SDL_Renderer *renderer, SDL_Texture *texture); static int GLES2_SetRenderTarget(SDL_Renderer * renderer, SDL_Texture * texture); static void GLES2_DestroyTexture(SDL_Renderer *renderer, SDL_Texture *texture); -static GLenum -GetScaleQuality(void) -{ - const char *hint = SDL_GetHint(SDL_HINT_RENDER_SCALE_QUALITY); - - if (!hint || *hint == '0' || SDL_strcasecmp(hint, "nearest") == 0) { - return GL_NEAREST; - } else { - return GL_LINEAR; - } -} - static int GLES2_CreateTexture(SDL_Renderer *renderer, SDL_Texture *texture) { @@ -625,7 +613,7 @@ GLES2_CreateTexture(SDL_Renderer *renderer, SDL_Texture *texture) data->nv12 = ((texture->format == SDL_PIXELFORMAT_NV12) || (texture->format == SDL_PIXELFORMAT_NV21)); data->texture_u = 0; data->texture_v = 0; - scaleMode = GetScaleQuality(); + scaleMode = (texture->scaleMode == SDL_ScaleModeNearest) ? GL_NEAREST : GL_LINEAR; /* Allocate a blob for image renderdata */ if (texture->access == SDL_TEXTUREACCESS_STREAMING) { @@ -731,6 +719,10 @@ GLES2_TexSubImage2D(GLES2_DriverContext *data, GLenum target, GLint xoffset, GLi int src_pitch; int y; + if ((width == 0) || (height == 0) || (bpp == 0)) { + return 0; /* nothing to do */ + } + /* Reformat the texture data into a tightly packed array */ src_pitch = width * bpp; src = (Uint8 *)pixels; @@ -1542,7 +1534,7 @@ GLES2_UpdateVertexBuffer(SDL_Renderer *renderer, GLES2_Attribute attr, GLES2_DriverContext *data = (GLES2_DriverContext *)renderer->driverdata; #if !SDL_GLES2_USE_VBOS - data->glVertexAttribPointer(attr, attr == GLES2_ATTRIBUTE_ANGLE ? 1 : 2, GL_FLOAT, GL_FALSE, 0, vertexData); + data->glVertexAttribPointer(attr, 2, GL_FLOAT, GL_FALSE, 0, vertexData); #else if (!data->vertex_buffers[attr]) { data->glGenBuffers(1, &data->vertex_buffers[attr]); @@ -1557,7 +1549,7 @@ GLES2_UpdateVertexBuffer(SDL_Renderer *renderer, GLES2_Attribute attr, data->glBufferSubData(GL_ARRAY_BUFFER, 0, dataSizeInBytes, vertexData); } - data->glVertexAttribPointer(attr, attr == GLES2_ATTRIBUTE_ANGLE ? 1 : 2, GL_FLOAT, GL_FALSE, 0, 0); + data->glVertexAttribPointer(attr, 2, GL_FLOAT, GL_FALSE, 0, 0); #endif return 0; @@ -1873,8 +1865,9 @@ GLES2_RenderCopyEx(SDL_Renderer *renderer, SDL_Texture *texture, const SDL_Rect GLfloat vertices[8]; GLfloat texCoords[8]; GLfloat translate[8]; - GLfloat fAngle[4]; + GLfloat fAngle[8]; GLfloat tmp; + float radian_angle; GLES2_ActivateRenderer(renderer); @@ -1884,7 +1877,11 @@ GLES2_RenderCopyEx(SDL_Renderer *renderer, SDL_Texture *texture, const SDL_Rect data->glEnableVertexAttribArray(GLES2_ATTRIBUTE_CENTER); data->glEnableVertexAttribArray(GLES2_ATTRIBUTE_ANGLE); - fAngle[0] = fAngle[1] = fAngle[2] = fAngle[3] = (GLfloat)(360.0f - angle); + + radian_angle = (float)(M_PI * (360.0 - angle) / 180.0); + fAngle[0] = fAngle[2] = fAngle[4] = fAngle[6] = (GLfloat)SDL_sin(radian_angle); + /* render expects cos value - 1 (see GLES2_VertexSrc_Default_) */ + fAngle[1] = fAngle[3] = fAngle[5] = fAngle[7] = (GLfloat)SDL_cos(radian_angle) - 1.0f; /* Calculate the center of rotation */ translate[0] = translate[2] = translate[4] = translate[6] = (center->x + dstrect->x); translate[1] = translate[3] = translate[5] = translate[7] = (center->y + dstrect->y); @@ -1913,7 +1910,7 @@ GLES2_RenderCopyEx(SDL_Renderer *renderer, SDL_Texture *texture, const SDL_Rect data->glVertexAttribPointer(GLES2_ATTRIBUTE_CENTER, 2, GL_FLOAT, GL_FALSE, 0, translate); data->glVertexAttribPointer(GLES2_ATTRIBUTE_POSITION, 2, GL_FLOAT, GL_FALSE, 0, vertices);*/ - GLES2_UpdateVertexBuffer(renderer, GLES2_ATTRIBUTE_ANGLE, fAngle, 4 * sizeof(GLfloat)); + GLES2_UpdateVertexBuffer(renderer, GLES2_ATTRIBUTE_ANGLE, fAngle, 8 * sizeof(GLfloat)); GLES2_UpdateVertexBuffer(renderer, GLES2_ATTRIBUTE_CENTER, translate, 8 * sizeof(GLfloat)); GLES2_UpdateVertexBuffer(renderer, GLES2_ATTRIBUTE_POSITION, vertices, 8 * sizeof(GLfloat)); @@ -1940,6 +1937,7 @@ GLES2_RenderReadPixels(SDL_Renderer * renderer, const SDL_Rect * rect, { GLES2_DriverContext *data = (GLES2_DriverContext *)renderer->driverdata; Uint32 temp_format = renderer->target ? renderer->target->format : SDL_PIXELFORMAT_ABGR8888; + size_t buflen; void *temp_pixels; int temp_pitch; Uint8 *src, *dst, *tmp; @@ -1949,7 +1947,12 @@ GLES2_RenderReadPixels(SDL_Renderer * renderer, const SDL_Rect * rect, GLES2_ActivateRenderer(renderer); temp_pitch = rect->w * SDL_BYTESPERPIXEL(temp_format); - temp_pixels = SDL_malloc(rect->h * temp_pitch); + buflen = (size_t) (rect->h * temp_pitch); + if (buflen == 0) { + return 0; /* nothing to do. */ + } + + temp_pixels = SDL_malloc(buflen); if (!temp_pixels) { return SDL_OutOfMemory(); } diff --git a/Engine/lib/sdl/src/render/opengles2/SDL_shaders_gles2.c b/Engine/lib/sdl/src/render/opengles2/SDL_shaders_gles2.c index b0bcdff25..f428a4945 100644 --- a/Engine/lib/sdl/src/render/opengles2/SDL_shaders_gles2.c +++ b/Engine/lib/sdl/src/render/opengles2/SDL_shaders_gles2.c @@ -30,20 +30,24 @@ /************************************************************************************************* * Vertex/fragment shader source * *************************************************************************************************/ - +/* Notes on a_angle: + * It is a vector containing sin and cos for rotation matrix + * To get correct rotation for most cases when a_angle is disabled cos + value is decremented by 1.0 to get proper output with 0.0 which is + default value +*/ static const Uint8 GLES2_VertexSrc_Default_[] = " \ uniform mat4 u_projection; \ attribute vec2 a_position; \ attribute vec2 a_texCoord; \ - attribute float a_angle; \ + attribute vec2 a_angle; \ attribute vec2 a_center; \ varying vec2 v_texCoord; \ \ void main() \ { \ - float angle = radians(a_angle); \ - float c = cos(angle); \ - float s = sin(angle); \ + float s = a_angle[0]; \ + float c = a_angle[1] + 1.0; \ mat2 rotationMatrix = mat2(c, -s, s, c); \ vec2 position = rotationMatrix * (a_position - a_center) + a_center; \ v_texCoord = a_texCoord; \ diff --git a/Engine/lib/sdl/src/render/psp/SDL_render_psp.c b/Engine/lib/sdl/src/render/psp/SDL_render_psp.c index 38f893e8f..babc2526a 100644 --- a/Engine/lib/sdl/src/render/psp/SDL_render_psp.c +++ b/Engine/lib/sdl/src/render/psp/SDL_render_psp.c @@ -186,18 +186,6 @@ TextureNextPow2(unsigned int w) } -static int -GetScaleQuality(void) -{ - const char *hint = SDL_GetHint(SDL_HINT_RENDER_SCALE_QUALITY); - - if (!hint || *hint == '0' || SDL_strcasecmp(hint, "nearest") == 0) { - return GU_NEAREST; /* GU_NEAREST good for tile-map */ - } else { - return GU_LINEAR; /* GU_LINEAR good for scaling */ - } -} - static int PixelFormatToPSPFMT(Uint32 format) { @@ -514,7 +502,7 @@ void TextureActivate(SDL_Texture * texture) { PSP_TextureData *psp_texture = (PSP_TextureData *) texture->driverdata; - int scaleMode = GetScaleQuality(); + int scaleMode = (texture->scaleMode == SDL_ScaleModeNearest) ? GU_NEAREST : GU_LINEAR; /* Swizzling is useless with small textures. */ if (texture->w >= 16 || texture->h >= 16) diff --git a/Engine/lib/sdl/src/render/software/SDL_blendfillrect.h b/Engine/lib/sdl/src/render/software/SDL_blendfillrect.h index 262210f77..3cac83454 100644 --- a/Engine/lib/sdl/src/render/software/SDL_blendfillrect.h +++ b/Engine/lib/sdl/src/render/software/SDL_blendfillrect.h @@ -18,10 +18,16 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_blendfillrect_h_ +#define SDL_blendfillrect_h_ + #include "../../SDL_internal.h" extern int SDL_BlendFillRect(SDL_Surface * dst, const SDL_Rect * rect, SDL_BlendMode blendMode, Uint8 r, Uint8 g, Uint8 b, Uint8 a); extern int SDL_BlendFillRects(SDL_Surface * dst, const SDL_Rect * rects, int count, SDL_BlendMode blendMode, Uint8 r, Uint8 g, Uint8 b, Uint8 a); +#endif /* SDL_blendfillrect_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/render/software/SDL_blendline.h b/Engine/lib/sdl/src/render/software/SDL_blendline.h index 82072cb00..a48a4984e 100644 --- a/Engine/lib/sdl/src/render/software/SDL_blendline.h +++ b/Engine/lib/sdl/src/render/software/SDL_blendline.h @@ -18,10 +18,16 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_blendline_h_ +#define SDL_blendline_h_ + #include "../../SDL_internal.h" extern int SDL_BlendLine(SDL_Surface * dst, int x1, int y1, int x2, int y2, SDL_BlendMode blendMode, Uint8 r, Uint8 g, Uint8 b, Uint8 a); extern int SDL_BlendLines(SDL_Surface * dst, const SDL_Point * points, int count, SDL_BlendMode blendMode, Uint8 r, Uint8 g, Uint8 b, Uint8 a); +#endif /* SDL_blendline_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/render/software/SDL_blendpoint.h b/Engine/lib/sdl/src/render/software/SDL_blendpoint.h index dd9e49c1c..188557cea 100644 --- a/Engine/lib/sdl/src/render/software/SDL_blendpoint.h +++ b/Engine/lib/sdl/src/render/software/SDL_blendpoint.h @@ -18,10 +18,16 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_blendpoint_h_ +#define SDL_blendpoint_h_ + #include "../../SDL_internal.h" extern int SDL_BlendPoint(SDL_Surface * dst, int x, int y, SDL_BlendMode blendMode, Uint8 r, Uint8 g, Uint8 b, Uint8 a); extern int SDL_BlendPoints(SDL_Surface * dst, const SDL_Point * points, int count, SDL_BlendMode blendMode, Uint8 r, Uint8 g, Uint8 b, Uint8 a); +#endif /* SDL_blendpoint_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/render/software/SDL_drawline.h b/Engine/lib/sdl/src/render/software/SDL_drawline.h index 9395d5050..4e8e2bd45 100644 --- a/Engine/lib/sdl/src/render/software/SDL_drawline.h +++ b/Engine/lib/sdl/src/render/software/SDL_drawline.h @@ -18,10 +18,16 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_drawline_h_ +#define SDL_drawline_h_ + #include "../../SDL_internal.h" extern int SDL_DrawLine(SDL_Surface * dst, int x1, int y1, int x2, int y2, Uint32 color); extern int SDL_DrawLines(SDL_Surface * dst, const SDL_Point * points, int count, Uint32 color); +#endif /* SDL_drawline_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/render/software/SDL_drawpoint.h b/Engine/lib/sdl/src/render/software/SDL_drawpoint.h index c36670007..454774d06 100644 --- a/Engine/lib/sdl/src/render/software/SDL_drawpoint.h +++ b/Engine/lib/sdl/src/render/software/SDL_drawpoint.h @@ -18,10 +18,16 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_drawpoint_h_ +#define SDL_drawpoint_h_ + #include "../../SDL_internal.h" extern int SDL_DrawPoint(SDL_Surface * dst, int x, int y, Uint32 color); extern int SDL_DrawPoints(SDL_Surface * dst, const SDL_Point * points, int count, Uint32 color); +#endif /* SDL_drawpoint_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/render/software/SDL_render_sw.c b/Engine/lib/sdl/src/render/software/SDL_render_sw.c index 89e54b859..709dfe8e4 100644 --- a/Engine/lib/sdl/src/render/software/SDL_render_sw.c +++ b/Engine/lib/sdl/src/render/software/SDL_render_sw.c @@ -587,18 +587,6 @@ SW_RenderCopy(SDL_Renderer * renderer, SDL_Texture * texture, } } -static int -GetScaleQuality(void) -{ - const char *hint = SDL_GetHint(SDL_HINT_RENDER_SCALE_QUALITY); - - if (!hint || *hint == '0' || SDL_strcasecmp(hint, "nearest") == 0) { - return 0; - } else { - return 1; - } -} - static int SW_RenderCopyEx(SDL_Renderer * renderer, SDL_Texture * texture, const SDL_Rect * srcrect, const SDL_FRect * dstrect, @@ -669,6 +657,11 @@ SW_RenderCopyEx(SDL_Renderer * renderer, SDL_Texture * texture, blitRequired = SDL_TRUE; } + /* srcrect is not selecting the whole src surface, so cropping is needed */ + if (!(srcrect->w == src->w && srcrect->h == src->h && srcrect->x == 0 && srcrect->y == 0)) { + blitRequired = SDL_TRUE; + } + /* The color and alpha modulation has to be applied before the rotation when using the NONE and MOD blend modes. */ if ((blendmode == SDL_BLENDMODE_NONE || blendmode == SDL_BLENDMODE_MOD) && (alphaMod & rMod & gMod & bMod) != 255) { applyModulation = SDL_TRUE; @@ -717,7 +710,7 @@ SW_RenderCopyEx(SDL_Renderer * renderer, SDL_Texture * texture, if (!retval) { SDLgfx_rotozoomSurfaceSizeTrig(tmp_rect.w, tmp_rect.h, angle, &dstwidth, &dstheight, &cangle, &sangle); - src_rotated = SDLgfx_rotateSurface(src_clone, angle, dstwidth/2, dstheight/2, GetScaleQuality(), flip & SDL_FLIP_HORIZONTAL, flip & SDL_FLIP_VERTICAL, dstwidth, dstheight, cangle, sangle); + src_rotated = SDLgfx_rotateSurface(src_clone, angle, dstwidth/2, dstheight/2, (texture->scaleMode == SDL_ScaleModeNearest) ? 0 : 1, flip & SDL_FLIP_HORIZONTAL, flip & SDL_FLIP_VERTICAL, dstwidth, dstheight, cangle, sangle); if (src_rotated == NULL) { retval = -1; } diff --git a/Engine/lib/sdl/src/render/software/SDL_render_sw_c.h b/Engine/lib/sdl/src/render/software/SDL_render_sw_c.h index 8f065de7f..f228517c5 100644 --- a/Engine/lib/sdl/src/render/software/SDL_render_sw_c.h +++ b/Engine/lib/sdl/src/render/software/SDL_render_sw_c.h @@ -19,6 +19,11 @@ 3. This notice may not be removed or altered from any source distribution. */ +#ifndef SDL_render_sw_c_h_ +#define SDL_render_sw_c_h_ + extern SDL_Renderer * SW_CreateRendererForSurface(SDL_Surface * surface); +#endif /* SDL_render_sw_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/render/software/SDL_rotate.c b/Engine/lib/sdl/src/render/software/SDL_rotate.c index 44762048f..09e099c39 100644 --- a/Engine/lib/sdl/src/render/software/SDL_rotate.c +++ b/Engine/lib/sdl/src/render/software/SDL_rotate.c @@ -83,7 +83,9 @@ static Uint32 _colorkey(SDL_Surface *src) { Uint32 key = 0; - SDL_GetColorKey(src, &key); + if (SDL_HasColorKey(src)) { + SDL_GetColorKey(src, &key); + } return key; } @@ -424,8 +426,10 @@ SDLgfx_rotateSurface(SDL_Surface * src, double angle, int centerx, int centery, if (src == NULL) return NULL; - if (SDL_GetColorKey(src, &colorkey) == 0) { - colorKeyAvailable = SDL_TRUE; + if (SDL_HasColorKey(src)) { + if (SDL_GetColorKey(src, &colorkey) == 0) { + colorKeyAvailable = SDL_TRUE; + } } /* This function requires a 32-bit surface or 8-bit surface with a colorkey */ diff --git a/Engine/lib/sdl/src/render/software/SDL_rotate.h b/Engine/lib/sdl/src/render/software/SDL_rotate.h index 2bf2ea82e..54c0927a1 100644 --- a/Engine/lib/sdl/src/render/software/SDL_rotate.h +++ b/Engine/lib/sdl/src/render/software/SDL_rotate.h @@ -19,6 +19,9 @@ 3. This notice may not be removed or altered from any source distribution. */ +#ifndef SDL_rotate_h_ +#define SDL_rotate_h_ + #ifndef MIN #define MIN(a,b) (((a) < (b)) ? (a) : (b)) #endif @@ -26,3 +29,4 @@ extern SDL_Surface *SDLgfx_rotateSurface(SDL_Surface * src, double angle, int centerx, int centery, int smooth, int flipx, int flipy, int dstwidth, int dstheight, double cangle, double sangle); extern void SDLgfx_rotozoomSurfaceSizeTrig(int width, int height, double angle, int *dstwidth, int *dstheight, double *cangle, double *sangle); +#endif /* SDL_rotate_h_ */ diff --git a/Engine/lib/sdl/src/sensor/SDL_sensor.c b/Engine/lib/sdl/src/sensor/SDL_sensor.c new file mode 100644 index 000000000..5c7a99019 --- /dev/null +++ b/Engine/lib/sdl/src/sensor/SDL_sensor.c @@ -0,0 +1,546 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "../SDL_internal.h" + +/* This is the sensor API for Simple DirectMedia Layer */ + +#include "SDL.h" +#include "SDL_atomic.h" +#include "SDL_events.h" +#include "SDL_syssensor.h" +#include "SDL_assert.h" + +#if !SDL_EVENTS_DISABLED +#include "../events/SDL_events_c.h" +#endif + +static SDL_SensorDriver *SDL_sensor_drivers[] = { +#ifdef SDL_SENSOR_ANDROID + &SDL_ANDROID_SensorDriver, +#endif +#ifdef SDL_SENSOR_COREMOTION + &SDL_COREMOTION_SensorDriver, +#endif +#if defined(SDL_SENSOR_DUMMY) || defined(SDL_SENSOR_DISABLED) + &SDL_DUMMY_SensorDriver +#endif +}; +static SDL_Sensor *SDL_sensors = NULL; +static SDL_bool SDL_updating_sensor = SDL_FALSE; +static SDL_mutex *SDL_sensor_lock = NULL; /* This needs to support recursive locks */ +static SDL_atomic_t SDL_next_sensor_instance_id; + +static void +SDL_LockSensors(void) +{ + if (SDL_sensor_lock) { + SDL_LockMutex(SDL_sensor_lock); + } +} + +static void +SDL_UnlockSensors(void) +{ + if (SDL_sensor_lock) { + SDL_UnlockMutex(SDL_sensor_lock); + } +} + + +int +SDL_SensorInit(void) +{ + int i, status; + + /* Create the sensor list lock */ + if (!SDL_sensor_lock) { + SDL_sensor_lock = SDL_CreateMutex(); + } + +#if !SDL_EVENTS_DISABLED + if (SDL_InitSubSystem(SDL_INIT_EVENTS) < 0) { + return -1; + } +#endif /* !SDL_EVENTS_DISABLED */ + + status = -1; + for (i = 0; i < SDL_arraysize(SDL_sensor_drivers); ++i) { + if (SDL_sensor_drivers[i]->Init() >= 0) { + status = 0; + } + } + return status; +} + +/* + * Count the number of sensors attached to the system + */ +int +SDL_NumSensors(void) +{ + int i, total_sensors = 0; + SDL_LockSensors(); + for (i = 0; i < SDL_arraysize(SDL_sensor_drivers); ++i) { + total_sensors += SDL_sensor_drivers[i]->GetCount(); + } + SDL_UnlockSensors(); + return total_sensors; +} + +/* + * Return the next available sensor instance ID + * This may be called by drivers from multiple threads, unprotected by any locks + */ +SDL_SensorID SDL_GetNextSensorInstanceID() +{ + return SDL_AtomicIncRef(&SDL_next_sensor_instance_id); +} + +/* + * Get the driver and device index for an API device index + * This should be called while the sensor lock is held, to prevent another thread from updating the list + */ +static SDL_bool +SDL_GetDriverAndSensorIndex(int device_index, SDL_SensorDriver **driver, int *driver_index) +{ + int i, num_sensors, total_sensors = 0; + + if (device_index >= 0) { + for (i = 0; i < SDL_arraysize(SDL_sensor_drivers); ++i) { + num_sensors = SDL_sensor_drivers[i]->GetCount(); + if (device_index < num_sensors) { + *driver = SDL_sensor_drivers[i]; + *driver_index = device_index; + return SDL_TRUE; + } + device_index -= num_sensors; + total_sensors += num_sensors; + } + } + + SDL_SetError("There are %d sensors available", total_sensors); + return SDL_FALSE; +} + +/* + * Get the implementation dependent name of a sensor + */ +const char * +SDL_SensorGetDeviceName(int device_index) +{ + SDL_SensorDriver *driver; + const char *name = NULL; + + SDL_LockSensors(); + if (SDL_GetDriverAndSensorIndex(device_index, &driver, &device_index)) { + name = driver->GetDeviceName(device_index); + } + SDL_UnlockSensors(); + + /* FIXME: Really we should reference count this name so it doesn't go away after unlock */ + return name; +} + +SDL_SensorType +SDL_SensorGetDeviceType(int device_index) +{ + SDL_SensorDriver *driver; + SDL_SensorType type = SDL_SENSOR_INVALID; + + SDL_LockSensors(); + if (SDL_GetDriverAndSensorIndex(device_index, &driver, &device_index)) { + type = driver->GetDeviceType(device_index); + } + SDL_UnlockSensors(); + + return type; +} + +SDL_SensorType +SDL_SensorGetDeviceNonPortableType(int device_index) +{ + SDL_SensorDriver *driver; + int type = -1; + + SDL_LockSensors(); + if (SDL_GetDriverAndSensorIndex(device_index, &driver, &device_index)) { + type = driver->GetDeviceNonPortableType(device_index); + } + SDL_UnlockSensors(); + + return type; +} + +SDL_SensorID +SDL_SensorGetDeviceInstanceID(int device_index) +{ + SDL_SensorDriver *driver; + SDL_SensorID instance_id = -1; + + SDL_LockSensors(); + if (SDL_GetDriverAndSensorIndex(device_index, &driver, &device_index)) { + instance_id = driver->GetDeviceInstanceID(device_index); + } + SDL_UnlockSensors(); + + return instance_id; +} + +/* + * Open a sensor for use - the index passed as an argument refers to + * the N'th sensor on the system. This index is the value which will + * identify this sensor in future sensor events. + * + * This function returns a sensor identifier, or NULL if an error occurred. + */ +SDL_Sensor * +SDL_SensorOpen(int device_index) +{ + SDL_SensorDriver *driver; + SDL_SensorID instance_id; + SDL_Sensor *sensor; + SDL_Sensor *sensorlist; + const char *sensorname = NULL; + + SDL_LockSensors(); + + if (!SDL_GetDriverAndSensorIndex(device_index, &driver, &device_index)) { + SDL_UnlockSensors(); + return NULL; + } + + sensorlist = SDL_sensors; + /* If the sensor is already open, return it + * it is important that we have a single sensor * for each instance id + */ + instance_id = driver->GetDeviceInstanceID(device_index); + while (sensorlist) { + if (instance_id == sensorlist->instance_id) { + sensor = sensorlist; + ++sensor->ref_count; + SDL_UnlockSensors(); + return sensor; + } + sensorlist = sensorlist->next; + } + + /* Create and initialize the sensor */ + sensor = (SDL_Sensor *) SDL_calloc(sizeof(*sensor), 1); + if (sensor == NULL) { + SDL_OutOfMemory(); + SDL_UnlockSensors(); + return NULL; + } + sensor->driver = driver; + sensor->instance_id = instance_id; + sensor->type = driver->GetDeviceType(device_index); + sensor->non_portable_type = driver->GetDeviceNonPortableType(device_index); + + if (driver->Open(sensor, device_index) < 0) { + SDL_free(sensor); + SDL_UnlockSensors(); + return NULL; + } + + sensorname = driver->GetDeviceName(device_index); + if (sensorname) { + sensor->name = SDL_strdup(sensorname); + } else { + sensor->name = NULL; + } + + /* Add sensor to list */ + ++sensor->ref_count; + /* Link the sensor in the list */ + sensor->next = SDL_sensors; + SDL_sensors = sensor; + + SDL_UnlockSensors(); + + driver->Update(sensor); + + return sensor; +} + +/* + * Find the SDL_Sensor that owns this instance id + */ +SDL_Sensor * +SDL_SensorFromInstanceID(SDL_SensorID instance_id) +{ + SDL_Sensor *sensor; + + SDL_LockSensors(); + for (sensor = SDL_sensors; sensor; sensor = sensor->next) { + if (sensor->instance_id == instance_id) { + break; + } + } + SDL_UnlockSensors(); + return sensor; +} + +/* + * Checks to make sure the sensor is valid. + */ +static int +SDL_PrivateSensorValid(SDL_Sensor * sensor) +{ + int valid; + + if (sensor == NULL) { + SDL_SetError("Sensor hasn't been opened yet"); + valid = 0; + } else { + valid = 1; + } + + return valid; +} + +/* + * Get the friendly name of this sensor + */ +const char * +SDL_SensorGetName(SDL_Sensor * sensor) +{ + if (!SDL_PrivateSensorValid(sensor)) { + return NULL; + } + + return sensor->name; +} + +/* + * Get the type of this sensor + */ +SDL_SensorType +SDL_SensorGetType(SDL_Sensor * sensor) +{ + if (!SDL_PrivateSensorValid(sensor)) { + return SDL_SENSOR_INVALID; + } + + return sensor->type; +} + +/* + * Get the platform dependent type of this sensor + */ +int +SDL_SensorGetNonPortableType(SDL_Sensor * sensor) +{ + if (!SDL_PrivateSensorValid(sensor)) { + return -1; + } + + return sensor->non_portable_type; +} + +/* + * Get the instance id for this opened sensor + */ +SDL_SensorID +SDL_SensorGetInstanceID(SDL_Sensor * sensor) +{ + if (!SDL_PrivateSensorValid(sensor)) { + return -1; + } + + return sensor->instance_id; +} + +/* + * Get the current state of this sensor + */ +int +SDL_SensorGetData(SDL_Sensor * sensor, float *data, int num_values) +{ + if (!SDL_PrivateSensorValid(sensor)) { + return -1; + } + + num_values = SDL_min(num_values, SDL_arraysize(sensor->data)); + SDL_memcpy(data, sensor->data, num_values*sizeof(*data)); + return 0; +} + +/* + * Close a sensor previously opened with SDL_SensorOpen() + */ +void +SDL_SensorClose(SDL_Sensor * sensor) +{ + SDL_Sensor *sensorlist; + SDL_Sensor *sensorlistprev; + + if (!SDL_PrivateSensorValid(sensor)) { + return; + } + + SDL_LockSensors(); + + /* First decrement ref count */ + if (--sensor->ref_count > 0) { + SDL_UnlockSensors(); + return; + } + + if (SDL_updating_sensor) { + SDL_UnlockSensors(); + return; + } + + sensor->driver->Close(sensor); + sensor->hwdata = NULL; + + sensorlist = SDL_sensors; + sensorlistprev = NULL; + while (sensorlist) { + if (sensor == sensorlist) { + if (sensorlistprev) { + /* unlink this entry */ + sensorlistprev->next = sensorlist->next; + } else { + SDL_sensors = sensor->next; + } + break; + } + sensorlistprev = sensorlist; + sensorlist = sensorlist->next; + } + + SDL_free(sensor->name); + + /* Free the data associated with this sensor */ + SDL_free(sensor); + + SDL_UnlockSensors(); +} + +void +SDL_SensorQuit(void) +{ + int i; + + /* Make sure we're not getting called in the middle of updating sensors */ + SDL_assert(!SDL_updating_sensor); + + SDL_LockSensors(); + + /* Stop the event polling */ + while (SDL_sensors) { + SDL_sensors->ref_count = 1; + SDL_SensorClose(SDL_sensors); + } + + /* Quit the sensor setup */ + for (i = 0; i < SDL_arraysize(SDL_sensor_drivers); ++i) { + SDL_sensor_drivers[i]->Quit(); + } + + SDL_UnlockSensors(); + +#if !SDL_EVENTS_DISABLED + SDL_QuitSubSystem(SDL_INIT_EVENTS); +#endif + + if (SDL_sensor_lock) { + SDL_DestroyMutex(SDL_sensor_lock); + SDL_sensor_lock = NULL; + } +} + + +/* These are global for SDL_syssensor.c and SDL_events.c */ + +int +SDL_PrivateSensorUpdate(SDL_Sensor *sensor, float *data, int num_values) +{ + int posted; + + /* Allow duplicate events, for things like steps and heartbeats */ + + /* Update internal sensor state */ + num_values = SDL_min(num_values, SDL_arraysize(sensor->data)); + SDL_memcpy(sensor->data, data, num_values*sizeof(*data)); + + /* Post the event, if desired */ + posted = 0; +#if !SDL_EVENTS_DISABLED + if (SDL_GetEventState(SDL_JOYAXISMOTION) == SDL_ENABLE) { + SDL_Event event; + event.type = SDL_SENSORUPDATE; + event.sensor.which = sensor->instance_id; + num_values = SDL_min(num_values, SDL_arraysize(event.sensor.data)); + SDL_memset(event.sensor.data, 0, sizeof(event.sensor.data)); + SDL_memcpy(event.sensor.data, data, num_values*sizeof(*data)); + posted = SDL_PushEvent(&event) == 1; + } +#endif /* !SDL_EVENTS_DISABLED */ + return posted; +} + +void +SDL_SensorUpdate(void) +{ + int i; + SDL_Sensor *sensor; + + SDL_LockSensors(); + + if (SDL_updating_sensor) { + /* The sensors are already being updated */ + SDL_UnlockSensors(); + return; + } + + SDL_updating_sensor = SDL_TRUE; + + /* Make sure the list is unlocked while dispatching events to prevent application deadlocks */ + SDL_UnlockSensors(); + + for (sensor = SDL_sensors; sensor; sensor = sensor->next) { + sensor->driver->Update(sensor); + } + + SDL_LockSensors(); + + SDL_updating_sensor = SDL_FALSE; + + /* If any sensors were closed while updating, free them here */ + for (sensor = SDL_sensors; sensor; sensor = sensor->next) { + if (sensor->ref_count <= 0) { + SDL_SensorClose(sensor); + } + } + + /* this needs to happen AFTER walking the sensor list above, so that any + dangling hardware data from removed devices can be free'd + */ + for (i = 0; i < SDL_arraysize(SDL_sensor_drivers); ++i) { + SDL_sensor_drivers[i]->Detect(); + } + + SDL_UnlockSensors(); +} + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/sensor/SDL_sensor_c.h b/Engine/lib/sdl/src/sensor/SDL_sensor_c.h new file mode 100644 index 000000000..70974af59 --- /dev/null +++ b/Engine/lib/sdl/src/sensor/SDL_sensor_c.h @@ -0,0 +1,44 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 SDL_sensor_c_h_ +#define SDL_sensor_c_h_ + +#include "SDL_config.h" + +struct _SDL_SensorDriver; + +/* Useful functions and variables from SDL_sensor.c */ +#include "SDL_sensor.h" + +/* Function to get the next available sensor instance ID */ +extern SDL_SensorID SDL_GetNextSensorInstanceID(void); + +/* Initialization and shutdown functions */ +extern int SDL_SensorInit(void); +extern void SDL_SensorQuit(void); + +/* Internal event queueing functions */ +extern int SDL_PrivateSensorUpdate(SDL_Sensor *sensor, float *data, int num_values); + +#endif /* SDL_sensor_c_h_ */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/sensor/SDL_syssensor.h b/Engine/lib/sdl/src/sensor/SDL_syssensor.h new file mode 100644 index 000000000..210577a9d --- /dev/null +++ b/Engine/lib/sdl/src/sensor/SDL_syssensor.h @@ -0,0 +1,105 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 SDL_syssensor_c_h_ +#define SDL_syssensor_c_h_ + +#include "SDL_config.h" + +/* This is the system specific header for the SDL sensor API */ + +#include "SDL_sensor.h" +#include "SDL_sensor_c.h" + +/* The SDL sensor structure */ +struct _SDL_Sensor +{ + SDL_SensorID instance_id; /* Device instance, monotonically increasing from 0 */ + char *name; /* Sensor name - system dependent */ + SDL_SensorType type; /* Type of the sensor */ + int non_portable_type; /* Platform dependent type of the sensor */ + + float data[16]; /* The current state of the sensor */ + + struct _SDL_SensorDriver *driver; + + struct sensor_hwdata *hwdata; /* Driver dependent information */ + + int ref_count; /* Reference count for multiple opens */ + + struct _SDL_Sensor *next; /* pointer to next sensor we have allocated */ +}; + +typedef struct _SDL_SensorDriver +{ + /* Function to scan the system for sensors. + * sensor 0 should be the system default sensor. + * This function should return 0, or -1 on an unrecoverable fatal error. + */ + int (*Init)(void); + + /* Function to return the number of sensors available right now */ + int (*GetCount)(void); + + /* Function to check to see if the available sensors have changed */ + void (*Detect)(void); + + /* Function to get the device-dependent name of a sensor */ + const char *(*GetDeviceName)(int device_index); + + /* Function to get the type of a sensor */ + SDL_SensorType (*GetDeviceType)(int device_index); + + /* Function to get the platform dependent type of a sensor */ + int (*GetDeviceNonPortableType)(int device_index); + + /* Function to get the current instance id of the sensor located at device_index */ + SDL_SensorID (*GetDeviceInstanceID)(int device_index); + + /* Function to open a sensor for use. + The sensor to open is specified by the device index. + It returns 0, or -1 if there is an error. + */ + int (*Open)(SDL_Sensor * sensor, int device_index); + + /* Function to update the state of a sensor - called as a device poll. + * This function shouldn't update the sensor structure directly, + * but instead should call SDL_PrivateSensorUpdate() to deliver events + * and update sensor device state. + */ + void (*Update)(SDL_Sensor * sensor); + + /* Function to close a sensor after use */ + void (*Close)(SDL_Sensor * sensor); + + /* Function to perform any system-specific sensor related cleanup */ + void (*Quit)(void); + +} SDL_SensorDriver; + +/* The available sensor drivers */ +extern SDL_SensorDriver SDL_ANDROID_SensorDriver; +extern SDL_SensorDriver SDL_COREMOTION_SensorDriver; +extern SDL_SensorDriver SDL_DUMMY_SensorDriver; + +#endif /* SDL_syssensor_c_h_ */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/sensor/android/SDL_androidsensor.c b/Engine/lib/sdl/src/sensor/android/SDL_androidsensor.c new file mode 100644 index 000000000..117c18dff --- /dev/null +++ b/Engine/lib/sdl/src/sensor/android/SDL_androidsensor.c @@ -0,0 +1,211 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "SDL_config.h" + +#ifdef SDL_SENSOR_ANDROID + +/* This is the system specific header for the SDL sensor API */ +#include + +#include "SDL_error.h" +#include "SDL_sensor.h" +#include "SDL_androidsensor.h" +#include "../SDL_syssensor.h" +#include "../SDL_sensor_c.h" +//#include "../../core/android/SDL_android.h" + +#ifndef LOOPER_ID_USER +#define LOOPER_ID_USER 3 +#endif + +typedef struct +{ + ASensorRef asensor; + SDL_SensorID instance_id; +} SDL_AndroidSensor; + +static ASensorManager* SDL_sensor_manager; +static ALooper* SDL_sensor_looper; +static SDL_AndroidSensor *SDL_sensors; +static int SDL_sensors_count; + +static int +SDL_ANDROID_SensorInit(void) +{ + int i, sensors_count; + ASensorList sensors; + + SDL_sensor_manager = ASensorManager_getInstance(); + if (!SDL_sensor_manager) { + return SDL_SetError("Couldn't create sensor manager"); + } + + SDL_sensor_looper = ALooper_forThread(); + if (!SDL_sensor_looper) { + SDL_sensor_looper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS); + if (!SDL_sensor_looper) { + return SDL_SetError("Couldn't create sensor event loop"); + } + } + + /* FIXME: Is the sensor list dynamic? */ + sensors_count = ASensorManager_getSensorList(SDL_sensor_manager, &sensors); + if (sensors_count > 0) { + SDL_sensors = (SDL_AndroidSensor *)SDL_calloc(sensors_count, sizeof(*SDL_sensors)); + if (!SDL_sensors) { + return SDL_OutOfMemory(); + } + + for (i = 0; i < sensors_count; ++i) { + SDL_sensors[i].asensor = sensors[i]; + SDL_sensors[i].instance_id = SDL_GetNextSensorInstanceID(); + } + SDL_sensors_count = sensors_count; + } + return 0; +} + +static int +SDL_ANDROID_SensorGetCount(void) +{ + return SDL_sensors_count; +} + +static void +SDL_ANDROID_SensorDetect(void) +{ +} + +static const char * +SDL_ANDROID_SensorGetDeviceName(int device_index) +{ + return ASensor_getName(SDL_sensors[device_index].asensor); +} + +static SDL_SensorType +SDL_ANDROID_SensorGetDeviceType(int device_index) +{ + switch (ASensor_getType(SDL_sensors[device_index].asensor)) { + case 0x00000001: + return SDL_SENSOR_ACCEL; + case 0x00000004: + return SDL_SENSOR_GYRO; + default: + return SDL_SENSOR_UNKNOWN; + } +} + +static int +SDL_ANDROID_SensorGetDeviceNonPortableType(int device_index) +{ + return ASensor_getType(SDL_sensors[device_index].asensor); +} + +static SDL_SensorID +SDL_ANDROID_SensorGetDeviceInstanceID(int device_index) +{ + return SDL_sensors[device_index].instance_id; +} + +static int +SDL_ANDROID_SensorOpen(SDL_Sensor *sensor, int device_index) +{ + struct sensor_hwdata *hwdata; + + hwdata = (struct sensor_hwdata *)SDL_calloc(1, sizeof(*hwdata)); + if (hwdata == NULL) { + return SDL_OutOfMemory(); + } + + hwdata->asensor = SDL_sensors[device_index].asensor; + hwdata->eventqueue = ASensorManager_createEventQueue(SDL_sensor_manager, SDL_sensor_looper, LOOPER_ID_USER, NULL, NULL); + if (!hwdata->eventqueue) { + SDL_free(hwdata); + return SDL_SetError("Couldn't create sensor event queue"); + } + + if (ASensorEventQueue_enableSensor(hwdata->eventqueue, hwdata->asensor) < 0) { + ASensorManager_destroyEventQueue(SDL_sensor_manager, hwdata->eventqueue); + SDL_free(hwdata); + return SDL_SetError("Couldn't enable sensor"); + } + + /* FIXME: What rate should we set for this sensor? 60 FPS? Let's try the default rate for now... */ + + sensor->hwdata = hwdata; + return 0; +} + +static void +SDL_ANDROID_SensorUpdate(SDL_Sensor *sensor) +{ + int events; + ASensorEvent event; + struct android_poll_source* source; + + if (ALooper_pollAll(0, NULL, &events, (void**)&source) == LOOPER_ID_USER) { + SDL_zero(event); + while (ASensorEventQueue_getEvents(sensor->hwdata->eventqueue, &event, 1) > 0) { + SDL_PrivateSensorUpdate(sensor, event.data, SDL_arraysize(event.data)); + } + } +} + +static void +SDL_ANDROID_SensorClose(SDL_Sensor *sensor) +{ + if (sensor->hwdata) { + ASensorEventQueue_disableSensor(sensor->hwdata->eventqueue, sensor->hwdata->asensor); + ASensorManager_destroyEventQueue(SDL_sensor_manager, sensor->hwdata->eventqueue); + SDL_free(sensor->hwdata); + sensor->hwdata = NULL; + } +} + +static void +SDL_ANDROID_SensorQuit(void) +{ + if (SDL_sensors) { + SDL_free(SDL_sensors); + SDL_sensors = NULL; + SDL_sensors_count = 0; + } +} + +SDL_SensorDriver SDL_ANDROID_SensorDriver = +{ + SDL_ANDROID_SensorInit, + SDL_ANDROID_SensorGetCount, + SDL_ANDROID_SensorDetect, + SDL_ANDROID_SensorGetDeviceName, + SDL_ANDROID_SensorGetDeviceType, + SDL_ANDROID_SensorGetDeviceNonPortableType, + SDL_ANDROID_SensorGetDeviceInstanceID, + SDL_ANDROID_SensorOpen, + SDL_ANDROID_SensorUpdate, + SDL_ANDROID_SensorClose, + SDL_ANDROID_SensorQuit, +}; + +#endif /* SDL_SENSOR_ANDROID */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/sensor/android/SDL_androidsensor.h b/Engine/lib/sdl/src/sensor/android/SDL_androidsensor.h new file mode 100644 index 000000000..c65002edf --- /dev/null +++ b/Engine/lib/sdl/src/sensor/android/SDL_androidsensor.h @@ -0,0 +1,31 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "SDL_config.h" + +/* The private structure used to keep track of a sensor */ +struct sensor_hwdata +{ + ASensorRef asensor; + ASensorEventQueue *eventqueue; +}; + + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/sensor/coremotion/SDL_coremotionsensor.h b/Engine/lib/sdl/src/sensor/coremotion/SDL_coremotionsensor.h new file mode 100644 index 000000000..2312e84f6 --- /dev/null +++ b/Engine/lib/sdl/src/sensor/coremotion/SDL_coremotionsensor.h @@ -0,0 +1,30 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "SDL_config.h" + +/* The private structure used to keep track of a sensor */ +struct sensor_hwdata +{ + float data[3]; +}; + + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/sensor/coremotion/SDL_coremotionsensor.m b/Engine/lib/sdl/src/sensor/coremotion/SDL_coremotionsensor.m new file mode 100644 index 000000000..526cce874 --- /dev/null +++ b/Engine/lib/sdl/src/sensor/coremotion/SDL_coremotionsensor.m @@ -0,0 +1,234 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "SDL_config.h" + +#ifdef SDL_SENSOR_COREMOTION + +/* This is the system specific header for the SDL sensor API */ +#include + +#include "SDL_error.h" +#include "SDL_sensor.h" +#include "SDL_coremotionsensor.h" +#include "../SDL_syssensor.h" +#include "../SDL_sensor_c.h" + +typedef struct +{ + SDL_SensorType type; + SDL_SensorID instance_id; +} SDL_CoreMotionSensor; + +static CMMotionManager *SDL_motion_manager; +static SDL_CoreMotionSensor *SDL_sensors; +static int SDL_sensors_count; + +static int +SDL_COREMOTION_SensorInit(void) +{ + int i, sensors_count = 0; + + if (!SDL_motion_manager) { + SDL_motion_manager = [[CMMotionManager alloc] init]; + } + + if (SDL_motion_manager.accelerometerAvailable) { + ++sensors_count; + } + if (SDL_motion_manager.gyroAvailable) { + ++sensors_count; + } + + if (sensors_count > 0) { + SDL_sensors = (SDL_CoreMotionSensor *)SDL_calloc(sensors_count, sizeof(*SDL_sensors)); + if (!SDL_sensors) { + return SDL_OutOfMemory(); + } + + i = 0; + if (SDL_motion_manager.accelerometerAvailable) { + SDL_sensors[i].type = SDL_SENSOR_ACCEL; + SDL_sensors[i].instance_id = SDL_GetNextSensorInstanceID(); + ++i; + } + if (SDL_motion_manager.gyroAvailable) { + SDL_sensors[i].type = SDL_SENSOR_GYRO; + SDL_sensors[i].instance_id = SDL_GetNextSensorInstanceID(); + ++i; + } + SDL_sensors_count = sensors_count; + } + return 0; +} + +static int +SDL_COREMOTION_SensorGetCount(void) +{ + return SDL_sensors_count; +} + +static void +SDL_COREMOTION_SensorDetect(void) +{ +} + +static const char * +SDL_COREMOTION_SensorGetDeviceName(int device_index) +{ + switch (SDL_sensors[device_index].type) { + case SDL_SENSOR_ACCEL: + return "Accelerometer"; + case SDL_SENSOR_GYRO: + return "Gyro"; + default: + return "Unknown"; + } +} + +static SDL_SensorType +SDL_COREMOTION_SensorGetDeviceType(int device_index) +{ + return SDL_sensors[device_index].type; +} + +static int +SDL_COREMOTION_SensorGetDeviceNonPortableType(int device_index) +{ + return SDL_sensors[device_index].type; +} + +static SDL_SensorID +SDL_COREMOTION_SensorGetDeviceInstanceID(int device_index) +{ + return SDL_sensors[device_index].instance_id; +} + +static int +SDL_COREMOTION_SensorOpen(SDL_Sensor *sensor, int device_index) +{ + struct sensor_hwdata *hwdata; + + hwdata = (struct sensor_hwdata *)SDL_calloc(1, sizeof(*hwdata)); + if (hwdata == NULL) { + return SDL_OutOfMemory(); + } + sensor->hwdata = hwdata; + + switch (sensor->type) + { + case SDL_SENSOR_ACCEL: + [SDL_motion_manager startAccelerometerUpdates]; + break; + case SDL_SENSOR_GYRO: + [SDL_motion_manager startGyroUpdates]; + break; + default: + break; + } + return 0; +} + +static void +SDL_COREMOTION_SensorUpdate(SDL_Sensor *sensor) +{ + switch (sensor->type) + { + case SDL_SENSOR_ACCEL: + { + CMAccelerometerData *accelerometerData = SDL_motion_manager.accelerometerData; + if (accelerometerData) { + CMAcceleration acceleration = accelerometerData.acceleration; + float data[3]; + data[0] = acceleration.x * SDL_STANDARD_GRAVITY; + data[1] = acceleration.y * SDL_STANDARD_GRAVITY; + data[2] = acceleration.z * SDL_STANDARD_GRAVITY; + if (SDL_memcmp(data, sensor->hwdata->data, sizeof(data)) != 0) { + SDL_PrivateSensorUpdate(sensor, data, SDL_arraysize(data)); + SDL_memcpy(sensor->hwdata->data, data, sizeof(data)); + } + } + } + break; + case SDL_SENSOR_GYRO: + { + CMGyroData *gyroData = SDL_motion_manager.gyroData; + if (gyroData) { + CMRotationRate rotationRate = gyroData.rotationRate; + float data[3]; + data[0] = rotationRate.x; + data[1] = rotationRate.y; + data[2] = rotationRate.z; + if (SDL_memcmp(data, sensor->hwdata->data, sizeof(data)) != 0) { + SDL_PrivateSensorUpdate(sensor, data, SDL_arraysize(data)); + SDL_memcpy(sensor->hwdata->data, data, sizeof(data)); + } + } + } + break; + default: + break; + } +} + +static void +SDL_COREMOTION_SensorClose(SDL_Sensor *sensor) +{ + if (sensor->hwdata) { + switch (sensor->type) + { + case SDL_SENSOR_ACCEL: + [SDL_motion_manager stopAccelerometerUpdates]; + break; + case SDL_SENSOR_GYRO: + [SDL_motion_manager stopGyroUpdates]; + break; + default: + break; + } + SDL_free(sensor->hwdata); + sensor->hwdata = NULL; + } +} + +static void +SDL_COREMOTION_SensorQuit(void) +{ +} + +SDL_SensorDriver SDL_COREMOTION_SensorDriver = +{ + SDL_COREMOTION_SensorInit, + SDL_COREMOTION_SensorGetCount, + SDL_COREMOTION_SensorDetect, + SDL_COREMOTION_SensorGetDeviceName, + SDL_COREMOTION_SensorGetDeviceType, + SDL_COREMOTION_SensorGetDeviceNonPortableType, + SDL_COREMOTION_SensorGetDeviceInstanceID, + SDL_COREMOTION_SensorOpen, + SDL_COREMOTION_SensorUpdate, + SDL_COREMOTION_SensorClose, + SDL_COREMOTION_SensorQuit, +}; + +#endif /* SDL_SENSOR_COREMOTION */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/sensor/dummy/SDL_dummysensor.c b/Engine/lib/sdl/src/sensor/dummy/SDL_dummysensor.c new file mode 100644 index 000000000..cf040450d --- /dev/null +++ b/Engine/lib/sdl/src/sensor/dummy/SDL_dummysensor.c @@ -0,0 +1,110 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "SDL_config.h" + +#if defined(SDL_SENSOR_DUMMY) || defined(SDL_SENSOR_DISABLED) + +#include "SDL_error.h" +#include "SDL_sensor.h" +#include "SDL_dummysensor.h" +#include "../SDL_syssensor.h" + +static int +SDL_DUMMY_SensorInit(void) +{ + return 0; +} + +static int +SDL_DUMMY_SensorGetCount(void) +{ + return 0; +} + +static void +SDL_DUMMY_SensorDetect(void) +{ +} + +static const char * +SDL_DUMMY_SensorGetDeviceName(int device_index) +{ + return NULL; +} + +static SDL_SensorType +SDL_DUMMY_SensorGetDeviceType(int device_index) +{ + return SDL_SENSOR_INVALID; +} + +static int +SDL_DUMMY_SensorGetDeviceNonPortableType(int device_index) +{ + return -1; +} + +static SDL_SensorID +SDL_DUMMY_SensorGetDeviceInstanceID(int device_index) +{ + return -1; +} + +static int +SDL_DUMMY_SensorOpen(SDL_Sensor *sensor, int device_index) +{ + return SDL_Unsupported(); +} + +static void +SDL_DUMMY_SensorUpdate(SDL_Sensor *sensor) +{ +} + +static void +SDL_DUMMY_SensorClose(SDL_Sensor *sensor) +{ +} + +static void +SDL_DUMMY_SensorQuit(void) +{ +} + +SDL_SensorDriver SDL_DUMMY_SensorDriver = +{ + SDL_DUMMY_SensorInit, + SDL_DUMMY_SensorGetCount, + SDL_DUMMY_SensorDetect, + SDL_DUMMY_SensorGetDeviceName, + SDL_DUMMY_SensorGetDeviceType, + SDL_DUMMY_SensorGetDeviceNonPortableType, + SDL_DUMMY_SensorGetDeviceInstanceID, + SDL_DUMMY_SensorOpen, + SDL_DUMMY_SensorUpdate, + SDL_DUMMY_SensorClose, + SDL_DUMMY_SensorQuit, +}; + +#endif /* SDL_SENSOR_DUMMY || SDL_SENSOR_DISABLED */ + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/sensor/dummy/SDL_dummysensor.h b/Engine/lib/sdl/src/sensor/dummy/SDL_dummysensor.h new file mode 100644 index 000000000..507ee93b9 --- /dev/null +++ b/Engine/lib/sdl/src/sensor/dummy/SDL_dummysensor.h @@ -0,0 +1,23 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 "SDL_config.h" + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/stdlib/SDL_iconv.c b/Engine/lib/sdl/src/stdlib/SDL_iconv.c index f8dbc9fa7..e2e3a3f86 100644 --- a/Engine/lib/sdl/src/stdlib/SDL_iconv.c +++ b/Engine/lib/sdl/src/stdlib/SDL_iconv.c @@ -89,13 +89,13 @@ SDL_iconv(SDL_iconv_t cd, #else /* Lots of useful information on Unicode at: - http://www.cl.cam.ac.uk/~mgk25/unicode.html + http://www.cl.cam.ac.uk/~mgk25/unicode.html */ -#define UNICODE_BOM 0xFEFF +#define UNICODE_BOM 0xFEFF -#define UNKNOWN_ASCII '?' -#define UNKNOWN_UNICODE 0xFFFD +#define UNKNOWN_ASCII '?' +#define UNKNOWN_UNICODE 0xFFFD enum { @@ -115,13 +115,13 @@ enum ENCODING_UCS4LE, }; #if SDL_BYTEORDER == SDL_BIG_ENDIAN -#define ENCODING_UTF16NATIVE ENCODING_UTF16BE -#define ENCODING_UTF32NATIVE ENCODING_UTF32BE +#define ENCODING_UTF16NATIVE ENCODING_UTF16BE +#define ENCODING_UTF32NATIVE ENCODING_UTF32BE #define ENCODING_UCS2NATIVE ENCODING_UCS2BE #define ENCODING_UCS4NATIVE ENCODING_UCS4BE #else -#define ENCODING_UTF16NATIVE ENCODING_UTF16LE -#define ENCODING_UTF32NATIVE ENCODING_UTF32LE +#define ENCODING_UTF16NATIVE ENCODING_UTF16LE +#define ENCODING_UTF32NATIVE ENCODING_UTF32LE #define ENCODING_UCS2NATIVE ENCODING_UCS2LE #define ENCODING_UCS4NATIVE ENCODING_UCS4LE #endif diff --git a/Engine/lib/sdl/src/stdlib/SDL_stdlib.c b/Engine/lib/sdl/src/stdlib/SDL_stdlib.c index b36d83c79..d500bf400 100644 --- a/Engine/lib/sdl/src/stdlib/SDL_stdlib.c +++ b/Engine/lib/sdl/src/stdlib/SDL_stdlib.c @@ -200,11 +200,31 @@ SDL_cosf(float x) #endif } +double +SDL_exp(double x) +{ +#if defined(HAVE_EXP) + return exp(x); +#else + return SDL_uclibc_exp(x); +#endif +} + +float +SDL_expf(float x) +{ +#if defined(HAVE_EXPF) + return expf(x); +#else + return (float)SDL_exp((double)x); +#endif +} + double SDL_fabs(double x) { #if defined(HAVE_FABS) - return fabs(x); + return fabs(x); #else return SDL_uclibc_fabs(x); #endif @@ -214,7 +234,7 @@ float SDL_fabsf(float x) { #if defined(HAVE_FABSF) - return fabsf(x); + return fabsf(x); #else return (float)SDL_fabs((double)x); #endif diff --git a/Engine/lib/sdl/src/stdlib/SDL_string.c b/Engine/lib/sdl/src/stdlib/SDL_string.c index 444ae6d52..a563adfcc 100644 --- a/Engine/lib/sdl/src/stdlib/SDL_string.c +++ b/Engine/lib/sdl/src/stdlib/SDL_string.c @@ -271,12 +271,16 @@ SDL_memset(SDL_OUT_BYTECAP(len) void *dst, int c, size_t len) size_t left; Uint32 *dstp4; Uint8 *dstp1 = (Uint8 *) dst; - Uint32 value4 = (c | (c << 8) | (c << 16) | (c << 24)); - Uint8 value1 = (Uint8) c; + Uint8 value1; + Uint32 value4; + + /* The value used in memset() is a byte, passed as an int */ + c &= 0xff; /* The destination pointer needs to be aligned on a 4-byte boundary to * execute a 32-bit set. Set first bytes manually if needed until it is * aligned. */ + value1 = (Uint8)c; while ((intptr_t)dstp1 & 0x3) { if (len--) { *dstp1++ = value1; @@ -285,6 +289,7 @@ SDL_memset(SDL_OUT_BYTECAP(len) void *dst, int c, size_t len) } } + value4 = (c | (c << 8) | (c << 16) | (c << 24)); dstp4 = (Uint32 *) dstp1; left = (len % 4); len /= 4; @@ -416,6 +421,17 @@ SDL_strlen(const char *string) #endif /* HAVE_STRLEN */ } +wchar_t * +SDL_wcsdup(const wchar_t *string) +{ + size_t len = ((SDL_wcslen(string) + 1) * sizeof(wchar_t)); + wchar_t *newstr = (wchar_t *)SDL_malloc(len); + if (newstr) { + SDL_memcpy(newstr, string, len); + } + return newstr; +} + size_t SDL_wcslen(const wchar_t * string) { @@ -562,7 +578,7 @@ char * SDL_strdup(const char *string) { size_t len = SDL_strlen(string) + 1; - char *newstr = SDL_malloc(len); + char *newstr = (char *)SDL_malloc(len); if (newstr) { SDL_memcpy(newstr, string, len); } @@ -1319,7 +1335,18 @@ SDL_snprintf(SDL_OUT_Z_CAP(maxlen) char *text, size_t maxlen, SDL_PRINTF_FORMAT_ return retval; } -#ifdef HAVE_VSNPRINTF +#if defined(HAVE_LIBC) && defined(__WATCOMC__) +/* _vsnprintf() doesn't ensure nul termination */ +int SDL_vsnprintf(SDL_OUT_Z_CAP(maxlen) char *text, size_t maxlen, const char *fmt, va_list ap) +{ + int retval; + if (!fmt) fmt = ""; + retval = _vsnprintf(text, maxlen, fmt, ap); + if (maxlen > 0) text[maxlen-1] = '\0'; + if (retval < 0) retval = (int) maxlen; + return retval; +} +#elif defined(HAVE_VSNPRINTF) int SDL_vsnprintf(SDL_OUT_Z_CAP(maxlen) char *text, size_t maxlen, const char *fmt, va_list ap) { if (!fmt) { @@ -1338,9 +1365,9 @@ typedef enum typedef struct { - SDL_bool left_justify; + SDL_bool left_justify; /* for now: ignored. */ SDL_bool force_sign; - SDL_bool force_type; + SDL_bool force_type; /* for now: used only by float printer, ignored otherwise. */ SDL_bool pad_zeroes; SDL_letter_case force_case; int width; @@ -1352,15 +1379,18 @@ static size_t SDL_PrintString(char *text, size_t maxlen, SDL_FormatInfo *info, const char *string) { size_t length = 0; - size_t slen; + size_t slen, sz; if (string == NULL) { string = "(null)"; } - if (info && info->width && (size_t)info->width > SDL_strlen(string)) { + sz = SDL_strlen(string); + if (info && info->width > 0 && (size_t)info->width > sz) { char fill = info->pad_zeroes ? '0' : ' '; - size_t width = info->width - SDL_strlen(string); + size_t width = info->width - sz; + if (info->precision >= 0 && (size_t)info->precision < sz) + width += sz - (size_t)info->precision; while (width-- > 0 && maxlen > 0) { *text++ = fill; ++length; @@ -1372,6 +1402,13 @@ SDL_PrintString(char *text, size_t maxlen, SDL_FormatInfo *info, const char *str length += SDL_min(slen, maxlen); if (info) { + if (info->precision >= 0 && (size_t)info->precision < sz) { + slen = (size_t)info->precision; + if (slen < maxlen) { + text[slen] = 0; + length -= (sz - slen); + } + } if (info->force_case == SDL_CASE_LOWER) { SDL_strlwr(text); } else if (info->force_case == SDL_CASE_UPPER) { @@ -1381,12 +1418,54 @@ SDL_PrintString(char *text, size_t maxlen, SDL_FormatInfo *info, const char *str return length; } +static void +SDL_IntPrecisionAdjust(char *num, size_t maxlen, SDL_FormatInfo *info) +{/* left-pad num with zeroes. */ + size_t sz, pad, have_sign; + + if (!info) + return; + + have_sign = 0; + if (*num == '-' || *num == '+') { + have_sign = 1; + ++num; + --maxlen; + } + sz = SDL_strlen(num); + if (info->precision > 0 && sz < (size_t)info->precision) { + pad = (size_t)info->precision - sz; + if (pad + sz + 1 <= maxlen) { /* otherwise ignore the precision */ + SDL_memmove(num + pad, num, sz + 1); + SDL_memset(num, '0', pad); + } + } + info->precision = -1;/* so that SDL_PrintString() doesn't make a mess. */ + + if (info->pad_zeroes && info->width > 0 && (size_t)info->width > sz + have_sign) { + /* handle here: spaces are added before the sign + but zeroes must be placed _after_ the sign. */ + /* sz hasn't changed: we ignore pad_zeroes if a precision is given. */ + pad = (size_t)info->width - sz - have_sign; + if (pad + sz + 1 <= maxlen) { + SDL_memmove(num + pad, num, sz + 1); + SDL_memset(num, '0', pad); + } + info->width = 0; /* so that SDL_PrintString() doesn't make a mess. */ + } +} + static size_t SDL_PrintLong(char *text, size_t maxlen, SDL_FormatInfo *info, long value) { - char num[130]; + char num[130], *p = num; - SDL_ltoa(value, num, info ? info->radix : 10); + if (info->force_sign && value >= 0L) { + *p++ = '+'; + } + + SDL_ltoa(value, p, info ? info->radix : 10); + SDL_IntPrecisionAdjust(num, maxlen, info); return SDL_PrintString(text, maxlen, info, num); } @@ -1396,15 +1475,21 @@ SDL_PrintUnsignedLong(char *text, size_t maxlen, SDL_FormatInfo *info, unsigned char num[130]; SDL_ultoa(value, num, info ? info->radix : 10); + SDL_IntPrecisionAdjust(num, maxlen, info); return SDL_PrintString(text, maxlen, info, num); } static size_t SDL_PrintLongLong(char *text, size_t maxlen, SDL_FormatInfo *info, Sint64 value) { - char num[130]; + char num[130], *p = num; - SDL_lltoa(value, num, info ? info->radix : 10); + if (info->force_sign && value >= (Sint64)0) { + *p++ = '+'; + } + + SDL_lltoa(value, p, info ? info->radix : 10); + SDL_IntPrecisionAdjust(num, maxlen, info); return SDL_PrintString(text, maxlen, info, num); } @@ -1414,6 +1499,7 @@ SDL_PrintUnsignedLongLong(char *text, size_t maxlen, SDL_FormatInfo *info, Uint6 char num[130]; SDL_ulltoa(value, num, info ? info->radix : 10); + SDL_IntPrecisionAdjust(num, maxlen, info); return SDL_PrintString(text, maxlen, info, num); } @@ -1571,14 +1657,24 @@ SDL_vsnprintf(SDL_OUT_Z_CAP(maxlen) char *text, size_t maxlen, const char *fmt, if (*fmt >= '0' && *fmt <= '9') { info.width = SDL_strtol(fmt, (char **)&fmt, 0); } + else if (*fmt == '*') { + ++fmt; + info.width = va_arg(ap, int); + } if (*fmt == '.') { ++fmt; if (*fmt >= '0' && *fmt <= '9') { info.precision = SDL_strtol(fmt, (char **)&fmt, 0); + } else if (*fmt == '*') { + ++fmt; + info.precision = va_arg(ap, int); } else { info.precision = 0; } + if (info.precision < 0) { + info.precision = 0; + } } while (!done) { @@ -1614,6 +1710,9 @@ SDL_vsnprintf(SDL_OUT_Z_CAP(maxlen) char *text, size_t maxlen, const char *fmt, break; case 'i': case 'd': + if (info.precision >= 0) { + info.pad_zeroes = SDL_FALSE; + } switch (inttype) { case DO_INT: len = SDL_PrintLong(text, left, &info, @@ -1651,7 +1750,10 @@ SDL_vsnprintf(SDL_OUT_Z_CAP(maxlen) char *text, size_t maxlen, const char *fmt, } /* Fall through to unsigned handling */ case 'u': - info.pad_zeroes = SDL_TRUE; + info.force_sign = SDL_FALSE; + if (info.precision >= 0) { + info.pad_zeroes = SDL_FALSE; + } switch (inttype) { case DO_INT: len = SDL_PrintUnsignedLong(text, left, &info, @@ -1678,12 +1780,14 @@ SDL_vsnprintf(SDL_OUT_Z_CAP(maxlen) char *text, size_t maxlen, const char *fmt, /* In practice this is used on Windows for WCHAR strings */ wchar_t *wide_arg = va_arg(ap, wchar_t *); char *arg = SDL_iconv_string("UTF-8", "UTF-16LE", (char *)(wide_arg), (SDL_wcslen(wide_arg)+1)*sizeof(*wide_arg)); + info.pad_zeroes = SDL_FALSE; len = SDL_PrintString(text, left, &info, arg); SDL_free(arg); done = SDL_TRUE; } break; case 's': + info.pad_zeroes = SDL_FALSE; len = SDL_PrintString(text, left, &info, va_arg(ap, char *)); done = SDL_TRUE; break; diff --git a/Engine/lib/sdl/src/test/SDL_test_common.c b/Engine/lib/sdl/src/test/SDL_test_common.c index b7e294af7..81dd39eb1 100644 --- a/Engine/lib/sdl/src/test/SDL_test_common.c +++ b/Engine/lib/sdl/src/test/SDL_test_common.c @@ -1048,6 +1048,22 @@ SDLTest_CommonInit(SDLTest_CommonState * state) return SDL_TRUE; } +static const char * +DisplayOrientationName(int orientation) +{ + switch (orientation) + { +#define CASE(X) case SDL_ORIENTATION_##X: return #X + CASE(UNKNOWN); + CASE(LANDSCAPE); + CASE(LANDSCAPE_FLIPPED); + CASE(PORTRAIT); + CASE(PORTRAIT_FLIPPED); +#undef CASE +default: return "???"; + } +} + static const char * ControllerAxisName(const SDL_GameControllerAxis axis) { @@ -1102,6 +1118,17 @@ SDLTest_PrintEvent(SDL_Event * event) } switch (event->type) { + case SDL_DISPLAYEVENT: + switch (event->display.event) { + case SDL_DISPLAYEVENT_ORIENTATION: + SDL_Log("SDL EVENT: Display %d changed orientation to %s", event->display.display, DisplayOrientationName(event->display.data1)); + break; + default: + SDL_Log("SDL EVENT: Display %d got unknown event 0x%4.4x", + event->display.display, event->display.event); + break; + } + break; case SDL_WINDOWEVENT: switch (event->window.event) { case SDL_WINDOWEVENT_SHOWN: @@ -1349,7 +1376,18 @@ SDLTest_PrintEvent(SDL_Event * event) case SDL_APP_DIDENTERFOREGROUND: SDL_Log("SDL EVENT: App entered the foreground"); break; - + case SDL_DROPBEGIN: + SDL_Log("SDL EVENT: Drag and drop beginning"); + break; + case SDL_DROPFILE: + SDL_Log("SDL EVENT: Drag and drop file: '%s'", event->drop.file); + break; + case SDL_DROPTEXT: + SDL_Log("SDL EVENT: Drag and drop text: '%s'", event->drop.file); + break; + case SDL_DROPCOMPLETE: + SDL_Log("SDL EVENT: Drag and drop ending"); + break; case SDL_QUIT: SDL_Log("SDL EVENT: Quit requested"); break; @@ -1744,6 +1782,11 @@ SDLTest_CommonEvent(SDLTest_CommonState * state, SDL_Event * event, int *done) case SDL_MOUSEMOTION: lastEvent = event->motion; break; + + case SDL_DROPFILE: + case SDL_DROPTEXT: + SDL_free(event->drop.file); + break; } } diff --git a/Engine/lib/sdl/src/test/SDL_test_harness.c b/Engine/lib/sdl/src/test/SDL_test_harness.c index 15021c6fd..80b07940d 100644 --- a/Engine/lib/sdl/src/test/SDL_test_harness.c +++ b/Engine/lib/sdl/src/test/SDL_test_harness.c @@ -206,6 +206,9 @@ SDLTest_SetTestTimeout(int timeout, void (*callback)()) /** * \brief Timeout handler. Aborts test run and exits harness process. */ +#if defined(__WATCOMC__) +#pragma aux SDLTest_BailOut aborts; +#endif static SDL_NORETURN void SDLTest_BailOut() { diff --git a/Engine/lib/sdl/src/thread/SDL_thread.c b/Engine/lib/sdl/src/thread/SDL_thread.c index 84b72d577..5570adbff 100644 --- a/Engine/lib/sdl/src/thread/SDL_thread.c +++ b/Engine/lib/sdl/src/thread/SDL_thread.c @@ -299,19 +299,21 @@ SDL_RunThread(void *data) #ifdef SDL_CreateThread #undef SDL_CreateThread +#undef SDL_CreateThreadWithStackSize #endif #if SDL_DYNAMIC_API #define SDL_CreateThread SDL_CreateThread_REAL +#define SDL_CreateThreadWithStackSize SDL_CreateThreadWithStackSize_REAL #endif #ifdef SDL_PASSED_BEGINTHREAD_ENDTHREAD -static SDL_Thread * +SDL_Thread * SDL_CreateThreadWithStackSize(int (SDLCALL * fn) (void *), const char *name, const size_t stacksize, void *data, pfnSDL_CurrentBeginThread pfnBeginThread, pfnSDL_CurrentEndThread pfnEndThread) #else -static SDL_Thread * +SDL_Thread * SDL_CreateThreadWithStackSize(int (SDLCALL * fn) (void *), const char *name, const size_t stacksize, void *data) #endif diff --git a/Engine/lib/sdl/src/thread/psp/SDL_systhread.c b/Engine/lib/sdl/src/thread/psp/SDL_systhread.c index 9286be59e..284f182a6 100644 --- a/Engine/lib/sdl/src/thread/psp/SDL_systhread.c +++ b/Engine/lib/sdl/src/thread/psp/SDL_systhread.c @@ -97,6 +97,8 @@ int SDL_SYS_SetThreadPriority(SDL_ThreadPriority priority) if (priority == SDL_THREAD_PRIORITY_LOW) { value = 19; } else if (priority == SDL_THREAD_PRIORITY_HIGH) { + value = -10; + } else if (priority == SDL_THREAD_PRIORITY_TIME_CRITICAL) { value = -20; } else { value = 0; diff --git a/Engine/lib/sdl/src/thread/pthread/SDL_sysmutex.c b/Engine/lib/sdl/src/thread/pthread/SDL_sysmutex.c index e7b5b5cec..e514778a5 100644 --- a/Engine/lib/sdl/src/thread/pthread/SDL_sysmutex.c +++ b/Engine/lib/sdl/src/thread/pthread/SDL_sysmutex.c @@ -116,6 +116,7 @@ int SDL_TryLockMutex(SDL_mutex * mutex) { int retval; + int result; #if FAKE_RECURSIVE_MUTEX pthread_t this_thread; #endif @@ -134,18 +135,20 @@ SDL_TryLockMutex(SDL_mutex * mutex) We set the locking thread id after we obtain the lock so unlocks from other threads will fail. */ - if (pthread_mutex_trylock(&mutex->id) == 0) { + result = pthread_mutex_trylock(&mutex->id); + if (result == 0) { mutex->owner = this_thread; mutex->recursive = 0; - } else if (errno == EBUSY) { + } else if (result == EBUSY) { retval = SDL_MUTEX_TIMEDOUT; } else { retval = SDL_SetError("pthread_mutex_trylock() failed"); } } #else - if (pthread_mutex_trylock(&mutex->id) != 0) { - if (errno == EBUSY) { + result = pthread_mutex_trylock(&mutex->id); + if (result != 0) { + if (result == EBUSY) { retval = SDL_MUTEX_TIMEDOUT; } else { retval = SDL_SetError("pthread_mutex_trylock() failed"); diff --git a/Engine/lib/sdl/src/thread/pthread/SDL_systhread.c b/Engine/lib/sdl/src/thread/pthread/SDL_systhread.c index 035484085..ec329379e 100644 --- a/Engine/lib/sdl/src/thread/pthread/SDL_systhread.c +++ b/Engine/lib/sdl/src/thread/pthread/SDL_systhread.c @@ -34,6 +34,9 @@ #include #include #include +#include + +#include "../../core/linux/SDL_dbus.h" #endif /* __LINUX__ */ #if defined(__LINUX__) || defined(__MACOSX__) || defined(__IPHONEOS__) @@ -43,6 +46,7 @@ #endif #endif +#include "SDL_log.h" #include "SDL_platform.h" #include "SDL_thread.h" #include "../SDL_thread_c.h" @@ -180,6 +184,84 @@ SDL_ThreadID(void) return ((SDL_threadID) pthread_self()); } +#if __LINUX__ +/* d-bus queries to org.freedesktop.RealtimeKit1. */ +#if SDL_USE_LIBDBUS + +#define RTKIT_DBUS_NODE "org.freedesktop.RealtimeKit1" +#define RTKIT_DBUS_PATH "/org/freedesktop/RealtimeKit1" +#define RTKIT_DBUS_INTERFACE "org.freedesktop.RealtimeKit1" + +static pthread_once_t rtkit_initialize_once = PTHREAD_ONCE_INIT; +static Sint32 rtkit_min_nice_level = -20; + +static void +rtkit_initialize() +{ + SDL_DBusContext *dbus = SDL_DBus_GetContext(); + + /* Try getting minimum nice level: this is often greater than PRIO_MIN (-20). */ + if (!dbus || !SDL_DBus_QueryPropertyOnConnection(dbus->system_conn, RTKIT_DBUS_NODE, RTKIT_DBUS_PATH, RTKIT_DBUS_INTERFACE, "MinNiceLevel", + DBUS_TYPE_INT32, &rtkit_min_nice_level)) { + rtkit_min_nice_level = -20; + } +} + +static SDL_bool +rtkit_setpriority(pid_t thread, int nice_level) +{ + Uint64 ui64 = (Uint64)thread; + Sint32 si32 = (Sint32)nice_level; + SDL_DBusContext *dbus = SDL_DBus_GetContext(); + + pthread_once(&rtkit_initialize_once, rtkit_initialize); + + if (si32 < rtkit_min_nice_level) + si32 = rtkit_min_nice_level; + + if (!dbus || !SDL_DBus_CallMethodOnConnection(dbus->system_conn, + RTKIT_DBUS_NODE, RTKIT_DBUS_PATH, RTKIT_DBUS_INTERFACE, "MakeThreadHighPriority", + DBUS_TYPE_UINT64, &ui64, DBUS_TYPE_INT32, &si32, DBUS_TYPE_INVALID, + DBUS_TYPE_INVALID)) { + return SDL_FALSE; + } + return SDL_TRUE; +} + +#else + +static SDL_bool +rtkit_setpriority(pid_t thread, int nice_level) +{ + return SDL_FALSE; +} + +#endif /* !SDL_USE_LIBDBUS */ + +int +SDL_LinuxSetThreadPriority(Sint64 threadID, int priority) +{ + if (setpriority(PRIO_PROCESS, (id_t)threadID, priority) < 0) { + /* Note that this fails if you're trying to set high priority + and you don't have root permission. BUT DON'T RUN AS ROOT! + + You can grant the ability to increase thread priority by + running the following command on your application binary: + sudo setcap 'cap_sys_nice=eip' + + Let's try setting priority with RealtimeKit... + + README and sample code at: + http://git.0pointer.net/rtkit.git + */ + if (rtkit_setpriority((pid_t)threadID, priority) == SDL_FALSE) { + return SDL_SetError("setpriority() failed"); + } + } + return 0; +} +#endif /* __LINUX__ */ + int SDL_SYS_SetThreadPriority(SDL_ThreadPriority priority) { @@ -188,25 +270,18 @@ SDL_SYS_SetThreadPriority(SDL_ThreadPriority priority) return 0; #elif __LINUX__ int value; + pid_t thread = syscall(SYS_gettid); if (priority == SDL_THREAD_PRIORITY_LOW) { value = 19; } else if (priority == SDL_THREAD_PRIORITY_HIGH) { + value = -10; + } else if (priority == SDL_THREAD_PRIORITY_TIME_CRITICAL) { value = -20; } else { value = 0; } - if (setpriority(PRIO_PROCESS, syscall(SYS_gettid), value) < 0) { - /* Note that this fails if you're trying to set high priority - and you don't have root permission. BUT DON'T RUN AS ROOT! - - You can grant the ability to increase thread priority by - running the following command on your application binary: - sudo setcap 'cap_sys_nice=eip' - */ - return SDL_SetError("setpriority() failed"); - } - return 0; + return SDL_LinuxSetThreadPriority(thread, value); #else struct sched_param sched; int policy; @@ -217,12 +292,15 @@ SDL_SYS_SetThreadPriority(SDL_ThreadPriority priority) } if (priority == SDL_THREAD_PRIORITY_LOW) { sched.sched_priority = sched_get_priority_min(policy); - } else if (priority == SDL_THREAD_PRIORITY_HIGH) { + } else if (priority == SDL_THREAD_PRIORITY_TIME_CRITICAL) { sched.sched_priority = sched_get_priority_max(policy); } else { int min_priority = sched_get_priority_min(policy); int max_priority = sched_get_priority_max(policy); sched.sched_priority = (min_priority + (max_priority - min_priority) / 2); + if (priority == SDL_THREAD_PRIORITY_HIGH) { + sched.sched_priority += ((max_priority - min_priority) / 4); + } } if (pthread_setschedparam(thread, policy, &sched) != 0) { return SDL_SetError("pthread_setschedparam() failed"); diff --git a/Engine/lib/sdl/src/thread/windows/SDL_systhread.c b/Engine/lib/sdl/src/thread/windows/SDL_systhread.c index 90036c920..251510d48 100644 --- a/Engine/lib/sdl/src/thread/windows/SDL_systhread.c +++ b/Engine/lib/sdl/src/thread/windows/SDL_systhread.c @@ -231,6 +231,8 @@ SDL_SYS_SetThreadPriority(SDL_ThreadPriority priority) value = THREAD_PRIORITY_LOWEST; } else if (priority == SDL_THREAD_PRIORITY_HIGH) { value = THREAD_PRIORITY_HIGHEST; + } else if (priority == SDL_THREAD_PRIORITY_TIME_CRITICAL) { + value = THREAD_PRIORITY_TIME_CRITICAL; } else { value = THREAD_PRIORITY_NORMAL; } diff --git a/Engine/lib/sdl/src/timer/SDL_timer_c.h b/Engine/lib/sdl/src/timer/SDL_timer_c.h index f83bdded0..3ea350ffb 100644 --- a/Engine/lib/sdl/src/timer/SDL_timer_c.h +++ b/Engine/lib/sdl/src/timer/SDL_timer_c.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_timer_c_h_ +#define SDL_timer_c_h_ + #include "../SDL_internal.h" /* Useful functions and variables from SDL_timer.c */ @@ -31,4 +35,6 @@ extern void SDL_TicksQuit(void); extern int SDL_TimerInit(void); extern void SDL_TimerQuit(void); +#endif /* SDL_timer_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/SDL_RLEaccel_c.h b/Engine/lib/sdl/src/video/SDL_RLEaccel_c.h index fe418358b..b6fa6a11e 100644 --- a/Engine/lib/sdl/src/video/SDL_RLEaccel_c.h +++ b/Engine/lib/sdl/src/video/SDL_RLEaccel_c.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_RLEaccel_c_h_ +#define SDL_RLEaccel_c_h_ + #include "../SDL_internal.h" /* Useful functions and variables from SDL_RLEaccel.c */ @@ -28,4 +32,7 @@ extern int SDLCALL SDL_RLEBlit (SDL_Surface * src, SDL_Rect * srcrect, extern int SDLCALL SDL_RLEAlphaBlit(SDL_Surface * src, SDL_Rect * srcrect, SDL_Surface * dst, SDL_Rect * dstrect); extern void SDL_UnRLESurface(SDL_Surface * surface, int recode); + +#endif /* SDL_RLEaccel_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/SDL_blit.h b/Engine/lib/sdl/src/video/SDL_blit.h index ca1053445..6c95aafd0 100644 --- a/Engine/lib/sdl/src/video/SDL_blit.h +++ b/Engine/lib/sdl/src/video/SDL_blit.h @@ -126,7 +126,7 @@ extern SDL_BlitFunc SDL_CalculateBlitA(SDL_Surface * surface); b = SDL_expand_byte[fmt->Bloss][((Pixel&fmt->Bmask)>>fmt->Bshift)]; \ } #define RGB_FROM_RGB565(Pixel, r, g, b) \ - { \ +{ \ r = SDL_expand_byte[3][((Pixel&0xF800)>>11)]; \ g = SDL_expand_byte[2][((Pixel&0x07E0)>>5)]; \ b = SDL_expand_byte[3][(Pixel&0x001F)]; \ @@ -262,18 +262,18 @@ do { \ { \ switch (bpp) { \ case 1: { \ - Uint8 Pixel; \ + Uint8 _Pixel; \ \ - PIXEL_FROM_RGB(Pixel, fmt, r, g, b); \ - *((Uint8 *)(buf)) = Pixel; \ + PIXEL_FROM_RGB(_Pixel, fmt, r, g, b); \ + *((Uint8 *)(buf)) = _Pixel; \ } \ break; \ \ case 2: { \ - Uint16 Pixel; \ + Uint16 _Pixel; \ \ - PIXEL_FROM_RGB(Pixel, fmt, r, g, b); \ - *((Uint16 *)(buf)) = Pixel; \ + PIXEL_FROM_RGB(_Pixel, fmt, r, g, b); \ + *((Uint16 *)(buf)) = _Pixel; \ } \ break; \ \ @@ -291,10 +291,10 @@ do { \ break; \ \ case 4: { \ - Uint32 Pixel; \ + Uint32 _Pixel; \ \ - PIXEL_FROM_RGB(Pixel, fmt, r, g, b); \ - *((Uint32 *)(buf)) = Pixel; \ + PIXEL_FROM_RGB(_Pixel, fmt, r, g, b); \ + *((Uint32 *)(buf)) = _Pixel; \ } \ break; \ } \ diff --git a/Engine/lib/sdl/src/video/SDL_blit_1.c b/Engine/lib/sdl/src/video/SDL_blit_1.c index b7c5412ee..56ccf15fc 100644 --- a/Engine/lib/sdl/src/video/SDL_blit_1.c +++ b/Engine/lib/sdl/src/video/SDL_blit_1.c @@ -49,13 +49,13 @@ Blit1to1(SDL_BlitInfo * info) while (height--) { #ifdef USE_DUFFS_LOOP /* *INDENT-OFF* */ - DUFFS_LOOP( - { - *dst = map[*src]; - } - dst++; - src++; - , width); + DUFFS_LOOP( + { + *dst = map[*src]; + } + dst++; + src++; + , width); /* *INDENT-ON* */ #else for (c = width; c; --c) { @@ -72,11 +72,11 @@ Blit1to1(SDL_BlitInfo * info) /* This is now endian dependent */ #ifndef USE_DUFFS_LOOP # if ( SDL_BYTEORDER == SDL_LIL_ENDIAN ) -# define HI 1 -# define LO 0 +# define HI 1 +# define LO 0 # else /* ( SDL_BYTEORDER == SDL_BIG_ENDIAN ) */ -# define HI 0 -# define LO 1 +# define HI 0 +# define LO 1 # endif #endif static void @@ -101,14 +101,14 @@ Blit1to2(SDL_BlitInfo * info) #ifdef USE_DUFFS_LOOP while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP( - { - *(Uint16 *)dst = map[*src++]; - dst += 2; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + *(Uint16 *)dst = map[*src++]; + dst += 2; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } @@ -208,18 +208,18 @@ Blit1to3(SDL_BlitInfo * info) while (height--) { #ifdef USE_DUFFS_LOOP - /* *INDENT-OFF* */ - DUFFS_LOOP( - { - o = *src * 4; - dst[0] = map[o++]; - dst[1] = map[o++]; - dst[2] = map[o++]; - } - src++; - dst += 3; - , width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + o = *src * 4; + dst[0] = map[o++]; + dst[1] = map[o++]; + dst[2] = map[o++]; + } + src++; + dst += 3; + , width); + /* *INDENT-ON* */ #else for (c = width; c; --c) { o = *src * 4; @@ -257,11 +257,11 @@ Blit1to4(SDL_BlitInfo * info) while (height--) { #ifdef USE_DUFFS_LOOP - /* *INDENT-OFF* */ - DUFFS_LOOP( - *dst++ = map[*src++]; - , width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP( + *dst++ = map[*src++]; + , width); + /* *INDENT-ON* */ #else for (c = width / 4; c; --c) { *dst++ = map[*src++]; @@ -297,33 +297,33 @@ Blit1to1Key(SDL_BlitInfo * info) if (palmap) { while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP( - { - if ( *src != ckey ) { - *dst = palmap[*src]; - } - dst++; - src++; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + if ( *src != ckey ) { + *dst = palmap[*src]; + } + dst++; + src++; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } } else { while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP( - { - if ( *src != ckey ) { - *dst = *src; - } - dst++; - src++; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + if ( *src != ckey ) { + *dst = *src; + } + dst++; + src++; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } @@ -346,17 +346,17 @@ Blit1to2Key(SDL_BlitInfo * info) dstskip /= 2; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP( - { - if ( *src != ckey ) { - *dstp=palmap[*src]; - } - src++; - dstp++; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + if ( *src != ckey ) { + *dstp=palmap[*src]; + } + src++; + dstp++; + }, + width); + /* *INDENT-ON* */ src += srcskip; dstp += dstskip; } @@ -376,20 +376,20 @@ Blit1to3Key(SDL_BlitInfo * info) int o; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP( - { - if ( *src != ckey ) { - o = *src * 4; - dst[0] = palmap[o++]; - dst[1] = palmap[o++]; - dst[2] = palmap[o++]; - } - src++; - dst += 3; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + if ( *src != ckey ) { + o = *src * 4; + dst[0] = palmap[o++]; + dst[1] = palmap[o++]; + dst[2] = palmap[o++]; + } + src++; + dst += 3; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } @@ -411,17 +411,17 @@ Blit1to4Key(SDL_BlitInfo * info) dstskip /= 4; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP( - { - if ( *src != ckey ) { - *dstp = palmap[*src]; - } - src++; - dstp++; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + if ( *src != ckey ) { + *dstp = palmap[*src]; + } + src++; + dstp++; + }, + width); + /* *INDENT-ON* */ src += srcskip; dstp += dstskip; } @@ -489,22 +489,22 @@ Blit1toNAlphaKey(SDL_BlitInfo * info) dstbpp = dstfmt->BytesPerPixel; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP( - { - if ( *src != ckey ) { - sR = srcpal[*src].r; - sG = srcpal[*src].g; - sB = srcpal[*src].b; - DISEMBLE_RGBA(dst, dstbpp, dstfmt, pixel, dR, dG, dB, dA); - ALPHA_BLEND_RGBA(sR, sG, sB, A, dR, dG, dB, dA); - ASSEMBLE_RGBA(dst, dstbpp, dstfmt, dR, dG, dB, dA); - } - src++; - dst += dstbpp; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + if ( *src != ckey ) { + sR = srcpal[*src].r; + sG = srcpal[*src].g; + sB = srcpal[*src].b; + DISEMBLE_RGBA(dst, dstbpp, dstfmt, pixel, dR, dG, dB, dA); + ALPHA_BLEND_RGBA(sR, sG, sB, A, dR, dG, dB, dA); + ASSEMBLE_RGBA(dst, dstbpp, dstfmt, dR, dG, dB, dA); + } + src++; + dst += dstbpp; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } diff --git a/Engine/lib/sdl/src/video/SDL_blit_A.c b/Engine/lib/sdl/src/video/SDL_blit_A.c index 1e9c9d89b..350793294 100644 --- a/Engine/lib/sdl/src/video/SDL_blit_A.c +++ b/Engine/lib/sdl/src/video/SDL_blit_A.c @@ -45,28 +45,28 @@ BlitNto1SurfaceAlpha(SDL_BlitInfo * info) const unsigned A = info->a; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4( - { - DISEMBLE_RGB(src, srcbpp, srcfmt, Pixel, sR, sG, sB); - dR = dstfmt->palette->colors[*dst].r; - dG = dstfmt->palette->colors[*dst].g; - dB = dstfmt->palette->colors[*dst].b; - ALPHA_BLEND_RGB(sR, sG, sB, A, dR, dG, dB); - dR &= 0xff; - dG &= 0xff; - dB &= 0xff; - /* Pack RGB into 8bit pixel */ - if ( palmap == NULL ) { - *dst =((dR>>5)<<(3+2))|((dG>>5)<<(2))|((dB>>6)<<(0)); - } else { - *dst = palmap[((dR>>5)<<(3+2))|((dG>>5)<<(2))|((dB>>6)<<(0))]; - } - dst++; - src += srcbpp; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4( + { + DISEMBLE_RGB(src, srcbpp, srcfmt, Pixel, sR, sG, sB); + dR = dstfmt->palette->colors[*dst].r; + dG = dstfmt->palette->colors[*dst].g; + dB = dstfmt->palette->colors[*dst].b; + ALPHA_BLEND_RGB(sR, sG, sB, A, dR, dG, dB); + dR &= 0xff; + dG &= 0xff; + dB &= 0xff; + /* Pack RGB into 8bit pixel */ + if ( palmap == NULL ) { + *dst =((dR>>5)<<(3+2))|((dG>>5)<<(2))|((dB>>6)<<(0)); + } else { + *dst = palmap[((dR>>5)<<(3+2))|((dG>>5)<<(2))|((dB>>6)<<(0))]; + } + dst++; + src += srcbpp; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } @@ -91,28 +91,28 @@ BlitNto1PixelAlpha(SDL_BlitInfo * info) unsigned dR, dG, dB; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4( - { - DISEMBLE_RGBA(src,srcbpp,srcfmt,Pixel,sR,sG,sB,sA); - dR = dstfmt->palette->colors[*dst].r; - dG = dstfmt->palette->colors[*dst].g; - dB = dstfmt->palette->colors[*dst].b; - ALPHA_BLEND_RGB(sR, sG, sB, sA, dR, dG, dB); - dR &= 0xff; - dG &= 0xff; - dB &= 0xff; - /* Pack RGB into 8bit pixel */ - if ( palmap == NULL ) { - *dst =((dR>>5)<<(3+2))|((dG>>5)<<(2))|((dB>>6)<<(0)); - } else { - *dst = palmap[((dR>>5)<<(3+2))|((dG>>5)<<(2))|((dB>>6)<<(0))]; - } - dst++; - src += srcbpp; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4( + { + DISEMBLE_RGBA(src,srcbpp,srcfmt,Pixel,sR,sG,sB,sA); + dR = dstfmt->palette->colors[*dst].r; + dG = dstfmt->palette->colors[*dst].g; + dB = dstfmt->palette->colors[*dst].b; + ALPHA_BLEND_RGB(sR, sG, sB, sA, dR, dG, dB); + dR &= 0xff; + dG &= 0xff; + dB &= 0xff; + /* Pack RGB into 8bit pixel */ + if ( palmap == NULL ) { + *dst =((dR>>5)<<(3+2))|((dG>>5)<<(2))|((dB>>6)<<(0)); + } else { + *dst = palmap[((dR>>5)<<(3+2))|((dG>>5)<<(2))|((dB>>6)<<(0))]; + } + dst++; + src += srcbpp; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } @@ -139,30 +139,30 @@ BlitNto1SurfaceAlphaKey(SDL_BlitInfo * info) const unsigned A = info->a; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP( - { - DISEMBLE_RGB(src, srcbpp, srcfmt, Pixel, sR, sG, sB); - if ( Pixel != ckey ) { - dR = dstfmt->palette->colors[*dst].r; - dG = dstfmt->palette->colors[*dst].g; - dB = dstfmt->palette->colors[*dst].b; - ALPHA_BLEND_RGB(sR, sG, sB, A, dR, dG, dB); - dR &= 0xff; - dG &= 0xff; - dB &= 0xff; - /* Pack RGB into 8bit pixel */ - if ( palmap == NULL ) { + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + DISEMBLE_RGB(src, srcbpp, srcfmt, Pixel, sR, sG, sB); + if ( Pixel != ckey ) { + dR = dstfmt->palette->colors[*dst].r; + dG = dstfmt->palette->colors[*dst].g; + dB = dstfmt->palette->colors[*dst].b; + ALPHA_BLEND_RGB(sR, sG, sB, A, dR, dG, dB); + dR &= 0xff; + dG &= 0xff; + dB &= 0xff; + /* Pack RGB into 8bit pixel */ + if ( palmap == NULL ) { *dst =((dR>>5)<<(3+2))|((dG>>5)<<(2))|((dB>>6)<<(0)); - } else { + } else { *dst = palmap[((dR>>5)<<(3+2))|((dG>>5)<<(2))|((dB>>6)<<(0))]; - } - } - dst++; - src += srcbpp; - }, - width); - /* *INDENT-ON* */ + } + } + dst++; + src += srcbpp; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } @@ -342,45 +342,45 @@ BlitRGBtoRGBPixelAlphaMMX(SDL_BlitInfo * info) mm_zero = _mm_setzero_si64(); /* 0 -> mm_zero */ multmask = 0x00FF; - multmask <<= (ashift * 2); - multmask2 = 0x00FF00FF00FF00FFULL; + multmask <<= (ashift * 2); + multmask2 = 0x00FF00FF00FF00FFULL; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4({ - Uint32 alpha = *srcp & amask; - if (alpha == 0) { - /* do nothing */ - } else if (alpha == amask) { - *dstp = *srcp; - } else { - src1 = _mm_cvtsi32_si64(*srcp); /* src(ARGB) -> src1 (0000ARGB) */ - src1 = _mm_unpacklo_pi8(src1, mm_zero); /* 0A0R0G0B -> src1 */ + /* *INDENT-OFF* */ + DUFFS_LOOP4({ + Uint32 alpha = *srcp & amask; + if (alpha == 0) { + /* do nothing */ + } else if (alpha == amask) { + *dstp = *srcp; + } else { + src1 = _mm_cvtsi32_si64(*srcp); /* src(ARGB) -> src1 (0000ARGB) */ + src1 = _mm_unpacklo_pi8(src1, mm_zero); /* 0A0R0G0B -> src1 */ - dst1 = _mm_cvtsi32_si64(*dstp); /* dst(ARGB) -> dst1 (0000ARGB) */ - dst1 = _mm_unpacklo_pi8(dst1, mm_zero); /* 0A0R0G0B -> dst1 */ + dst1 = _mm_cvtsi32_si64(*dstp); /* dst(ARGB) -> dst1 (0000ARGB) */ + dst1 = _mm_unpacklo_pi8(dst1, mm_zero); /* 0A0R0G0B -> dst1 */ - mm_alpha = _mm_cvtsi32_si64(alpha); /* alpha -> mm_alpha (0000000A) */ - mm_alpha = _mm_srli_si64(mm_alpha, ashift); /* mm_alpha >> ashift -> mm_alpha(0000000A) */ - mm_alpha = _mm_unpacklo_pi16(mm_alpha, mm_alpha); /* 00000A0A -> mm_alpha */ - mm_alpha2 = _mm_unpacklo_pi32(mm_alpha, mm_alpha); /* 0A0A0A0A -> mm_alpha2 */ - mm_alpha = _mm_or_si64(mm_alpha2, *(__m64 *) & multmask); /* 0F0A0A0A -> mm_alpha */ - mm_alpha2 = _mm_xor_si64(mm_alpha2, *(__m64 *) & multmask2); /* 255 - mm_alpha -> mm_alpha */ + mm_alpha = _mm_cvtsi32_si64(alpha); /* alpha -> mm_alpha (0000000A) */ + mm_alpha = _mm_srli_si64(mm_alpha, ashift); /* mm_alpha >> ashift -> mm_alpha(0000000A) */ + mm_alpha = _mm_unpacklo_pi16(mm_alpha, mm_alpha); /* 00000A0A -> mm_alpha */ + mm_alpha2 = _mm_unpacklo_pi32(mm_alpha, mm_alpha); /* 0A0A0A0A -> mm_alpha2 */ + mm_alpha = _mm_or_si64(mm_alpha2, *(__m64 *) & multmask); /* 0F0A0A0A -> mm_alpha */ + mm_alpha2 = _mm_xor_si64(mm_alpha2, *(__m64 *) & multmask2); /* 255 - mm_alpha -> mm_alpha */ - /* blend */ - src1 = _mm_mullo_pi16(src1, mm_alpha); - src1 = _mm_srli_pi16(src1, 8); - dst1 = _mm_mullo_pi16(dst1, mm_alpha2); - dst1 = _mm_srli_pi16(dst1, 8); - dst1 = _mm_add_pi16(src1, dst1); - dst1 = _mm_packs_pu16(dst1, mm_zero); - - *dstp = _mm_cvtsi64_si32(dst1); /* dst1 -> pixel */ - } - ++srcp; - ++dstp; - }, width); - /* *INDENT-ON* */ + /* blend */ + src1 = _mm_mullo_pi16(src1, mm_alpha); + src1 = _mm_srli_pi16(src1, 8); + dst1 = _mm_mullo_pi16(dst1, mm_alpha2); + dst1 = _mm_srli_pi16(dst1, 8); + dst1 = _mm_add_pi16(src1, dst1); + dst1 = _mm_packs_pu16(dst1, mm_zero); + + *dstp = _mm_cvtsi64_si32(dst1); /* dst1 -> pixel */ + } + ++srcp; + ++dstp; + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -401,14 +401,14 @@ BlitRGBtoRGBSurfaceAlpha128(SDL_BlitInfo * info) int dstskip = info->dst_skip >> 2; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4({ - Uint32 s = *srcp++; - Uint32 d = *dstp; - *dstp++ = ((((s & 0x00fefefe) + (d & 0x00fefefe)) >> 1) - + (s & d & 0x00010101)) | 0xff000000; - }, width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4({ + Uint32 s = *srcp++; + Uint32 d = *dstp; + *dstp++ = ((((s & 0x00fefefe) + (d & 0x00fefefe)) >> 1) + + (s & d & 0x00010101)) | 0xff000000; + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -434,22 +434,22 @@ BlitRGBtoRGBSurfaceAlpha(SDL_BlitInfo * info) Uint32 d1; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4({ - s = *srcp; - d = *dstp; - s1 = s & 0xff00ff; - d1 = d & 0xff00ff; - d1 = (d1 + ((s1 - d1) * alpha >> 8)) - & 0xff00ff; - s &= 0xff00; - d &= 0xff00; - d = (d + ((s - d) * alpha >> 8)) & 0xff00; - *dstp = d1 | d | 0xff000000; - ++srcp; - ++dstp; - }, width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4({ + s = *srcp; + d = *dstp; + s1 = s & 0xff00ff; + d1 = d & 0xff00ff; + d1 = (d1 + ((s1 - d1) * alpha >> 8)) + & 0xff00ff; + s &= 0xff00; + d &= 0xff00; + d = (d + ((s - d) * alpha >> 8)) & 0xff00; + *dstp = d1 | d | 0xff000000; + ++srcp; + ++dstp; + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -468,42 +468,42 @@ BlitRGBtoRGBPixelAlpha(SDL_BlitInfo * info) int dstskip = info->dst_skip >> 2; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4({ - Uint32 dalpha; - Uint32 d; - Uint32 s1; - Uint32 d1; - Uint32 s = *srcp; - Uint32 alpha = s >> 24; - /* FIXME: Here we special-case opaque alpha since the - compositioning used (>>8 instead of /255) doesn't handle - it correctly. Also special-case alpha=0 for speed? - Benchmark this! */ - if (alpha) { - if (alpha == SDL_ALPHA_OPAQUE) { - *dstp = *srcp; - } else { - /* - * take out the middle component (green), and process - * the other two in parallel. One multiply less. - */ - d = *dstp; - dalpha = d >> 24; - s1 = s & 0xff00ff; - d1 = d & 0xff00ff; - d1 = (d1 + ((s1 - d1) * alpha >> 8)) & 0xff00ff; - s &= 0xff00; - d &= 0xff00; - d = (d + ((s - d) * alpha >> 8)) & 0xff00; - dalpha = alpha + (dalpha * (alpha ^ 0xFF) >> 8); - *dstp = d1 | d | (dalpha << 24); - } - } - ++srcp; - ++dstp; - }, width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4({ + Uint32 dalpha; + Uint32 d; + Uint32 s1; + Uint32 d1; + Uint32 s = *srcp; + Uint32 alpha = s >> 24; + /* FIXME: Here we special-case opaque alpha since the + compositioning used (>>8 instead of /255) doesn't handle + it correctly. Also special-case alpha=0 for speed? + Benchmark this! */ + if (alpha) { + if (alpha == SDL_ALPHA_OPAQUE) { + *dstp = *srcp; + } else { + /* + * take out the middle component (green), and process + * the other two in parallel. One multiply less. + */ + d = *dstp; + dalpha = d >> 24; + s1 = s & 0xff00ff; + d1 = d & 0xff00ff; + d1 = (d1 + ((s1 - d1) * alpha >> 8)) & 0xff00ff; + s &= 0xff00; + d &= 0xff00; + d = (d + ((s - d) * alpha >> 8)) & 0xff00; + dalpha = alpha + (dalpha * (alpha ^ 0xFF) >> 8); + *dstp = d1 | d | (dalpha << 24); + } + } + ++srcp; + ++dstp; + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -533,47 +533,47 @@ BlitRGBtoRGBPixelAlphaMMX3DNOW(SDL_BlitInfo * info) multmask2 = 0x00FF00FF00FF00FFULL; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4({ - Uint32 alpha; + /* *INDENT-OFF* */ + DUFFS_LOOP4({ + Uint32 alpha; - _m_prefetch(srcp + 16); - _m_prefetch(dstp + 16); + _m_prefetch(srcp + 16); + _m_prefetch(dstp + 16); - alpha = *srcp & amask; - if (alpha == 0) { - /* do nothing */ - } else if (alpha == amask) { - *dstp = *srcp; - } else { - src1 = _mm_cvtsi32_si64(*srcp); /* src(ARGB) -> src1 (0000ARGB) */ - src1 = _mm_unpacklo_pi8(src1, mm_zero); /* 0A0R0G0B -> src1 */ + alpha = *srcp & amask; + if (alpha == 0) { + /* do nothing */ + } else if (alpha == amask) { + *dstp = *srcp; + } else { + src1 = _mm_cvtsi32_si64(*srcp); /* src(ARGB) -> src1 (0000ARGB) */ + src1 = _mm_unpacklo_pi8(src1, mm_zero); /* 0A0R0G0B -> src1 */ - dst1 = _mm_cvtsi32_si64(*dstp); /* dst(ARGB) -> dst1 (0000ARGB) */ - dst1 = _mm_unpacklo_pi8(dst1, mm_zero); /* 0A0R0G0B -> dst1 */ + dst1 = _mm_cvtsi32_si64(*dstp); /* dst(ARGB) -> dst1 (0000ARGB) */ + dst1 = _mm_unpacklo_pi8(dst1, mm_zero); /* 0A0R0G0B -> dst1 */ - mm_alpha = _mm_cvtsi32_si64(alpha); /* alpha -> mm_alpha (0000000A) */ - mm_alpha = _mm_srli_si64(mm_alpha, ashift); /* mm_alpha >> ashift -> mm_alpha(0000000A) */ - mm_alpha = _mm_unpacklo_pi16(mm_alpha, mm_alpha); /* 00000A0A -> mm_alpha */ - mm_alpha2 = _mm_unpacklo_pi32(mm_alpha, mm_alpha); /* 0A0A0A0A -> mm_alpha2 */ - mm_alpha = _mm_or_si64(mm_alpha2, *(__m64 *) & multmask); /* 0F0A0A0A -> mm_alpha */ - mm_alpha2 = _mm_xor_si64(mm_alpha2, *(__m64 *) & multmask2); /* 255 - mm_alpha -> mm_alpha */ + mm_alpha = _mm_cvtsi32_si64(alpha); /* alpha -> mm_alpha (0000000A) */ + mm_alpha = _mm_srli_si64(mm_alpha, ashift); /* mm_alpha >> ashift -> mm_alpha(0000000A) */ + mm_alpha = _mm_unpacklo_pi16(mm_alpha, mm_alpha); /* 00000A0A -> mm_alpha */ + mm_alpha2 = _mm_unpacklo_pi32(mm_alpha, mm_alpha); /* 0A0A0A0A -> mm_alpha2 */ + mm_alpha = _mm_or_si64(mm_alpha2, *(__m64 *) & multmask); /* 0F0A0A0A -> mm_alpha */ + mm_alpha2 = _mm_xor_si64(mm_alpha2, *(__m64 *) & multmask2); /* 255 - mm_alpha -> mm_alpha */ - /* blend */ - src1 = _mm_mullo_pi16(src1, mm_alpha); - src1 = _mm_srli_pi16(src1, 8); - dst1 = _mm_mullo_pi16(dst1, mm_alpha2); - dst1 = _mm_srli_pi16(dst1, 8); - dst1 = _mm_add_pi16(src1, dst1); - dst1 = _mm_packs_pu16(dst1, mm_zero); - - *dstp = _mm_cvtsi64_si32(dst1); /* dst1 -> pixel */ - } - ++srcp; - ++dstp; - }, width); - /* *INDENT-ON* */ + /* blend */ + src1 = _mm_mullo_pi16(src1, mm_alpha); + src1 = _mm_srli_pi16(src1, 8); + dst1 = _mm_mullo_pi16(dst1, mm_alpha2); + dst1 = _mm_srli_pi16(dst1, 8); + dst1 = _mm_add_pi16(src1, dst1); + dst1 = _mm_packs_pu16(dst1, mm_zero); + + *dstp = _mm_cvtsi64_si32(dst1); /* dst1 -> pixel */ + } + ++srcp; + ++dstp; + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -585,13 +585,13 @@ BlitRGBtoRGBPixelAlphaMMX3DNOW(SDL_BlitInfo * info) /* 16bpp special case for per-surface alpha=50%: blend 2 pixels in parallel */ /* blend a single 16 bit pixel at 50% */ -#define BLEND16_50(d, s, mask) \ - ((((s & mask) + (d & mask)) >> 1) + (s & d & (~mask & 0xffff))) +#define BLEND16_50(d, s, mask) \ + ((((s & mask) + (d & mask)) >> 1) + (s & d & (~mask & 0xffff))) /* blend two 16 bit pixels at 50% */ -#define BLEND2x16_50(d, s, mask) \ - (((s & (mask | mask << 16)) >> 1) + ((d & (mask | mask << 16)) >> 1) \ - + (s & d & (~(mask | mask << 16)))) +#define BLEND2x16_50(d, s, mask) \ + (((s & (mask | mask << 16)) >> 1) + ((d & (mask | mask << 16)) >> 1) \ + + (s & d & (~(mask | mask << 16)))) static void Blit16to16SurfaceAlpha128(SDL_BlitInfo * info, Uint16 mask) @@ -727,103 +727,103 @@ Blit565to565SurfaceAlphaMMX(SDL_BlitInfo * info) bmask = _mm_set_pi32(0x001F001F, 0x001F001F); /* MASKBLUE -> bmask */ while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP_124( - { - s = *srcp++; - d = *dstp; - /* - * shift out the middle component (green) to - * the high 16 bits, and process all three RGB - * components at the same time. - */ - s = (s | s << 16) & 0x07e0f81f; - d = (d | d << 16) & 0x07e0f81f; - d += (s - d) * alpha >> 5; - d &= 0x07e0f81f; - *dstp++ = (Uint16)(d | d >> 16); - },{ - s = *srcp++; - d = *dstp; - /* - * shift out the middle component (green) to - * the high 16 bits, and process all three RGB - * components at the same time. - */ - s = (s | s << 16) & 0x07e0f81f; - d = (d | d << 16) & 0x07e0f81f; - d += (s - d) * alpha >> 5; - d &= 0x07e0f81f; - *dstp++ = (Uint16)(d | d >> 16); - s = *srcp++; - d = *dstp; - /* - * shift out the middle component (green) to - * the high 16 bits, and process all three RGB - * components at the same time. - */ - s = (s | s << 16) & 0x07e0f81f; - d = (d | d << 16) & 0x07e0f81f; - d += (s - d) * alpha >> 5; - d &= 0x07e0f81f; - *dstp++ = (Uint16)(d | d >> 16); - },{ - src1 = *(__m64*)srcp; /* 4 src pixels -> src1 */ - dst1 = *(__m64*)dstp; /* 4 dst pixels -> dst1 */ + /* *INDENT-OFF* */ + DUFFS_LOOP_124( + { + s = *srcp++; + d = *dstp; + /* + * shift out the middle component (green) to + * the high 16 bits, and process all three RGB + * components at the same time. + */ + s = (s | s << 16) & 0x07e0f81f; + d = (d | d << 16) & 0x07e0f81f; + d += (s - d) * alpha >> 5; + d &= 0x07e0f81f; + *dstp++ = (Uint16)(d | d >> 16); + },{ + s = *srcp++; + d = *dstp; + /* + * shift out the middle component (green) to + * the high 16 bits, and process all three RGB + * components at the same time. + */ + s = (s | s << 16) & 0x07e0f81f; + d = (d | d << 16) & 0x07e0f81f; + d += (s - d) * alpha >> 5; + d &= 0x07e0f81f; + *dstp++ = (Uint16)(d | d >> 16); + s = *srcp++; + d = *dstp; + /* + * shift out the middle component (green) to + * the high 16 bits, and process all three RGB + * components at the same time. + */ + s = (s | s << 16) & 0x07e0f81f; + d = (d | d << 16) & 0x07e0f81f; + d += (s - d) * alpha >> 5; + d &= 0x07e0f81f; + *dstp++ = (Uint16)(d | d >> 16); + },{ + src1 = *(__m64*)srcp; /* 4 src pixels -> src1 */ + dst1 = *(__m64*)dstp; /* 4 dst pixels -> dst1 */ - /* red */ - src2 = src1; - src2 = _mm_srli_pi16(src2, 11); /* src2 >> 11 -> src2 [000r 000r 000r 000r] */ + /* red */ + src2 = src1; + src2 = _mm_srli_pi16(src2, 11); /* src2 >> 11 -> src2 [000r 000r 000r 000r] */ - dst2 = dst1; - dst2 = _mm_srli_pi16(dst2, 11); /* dst2 >> 11 -> dst2 [000r 000r 000r 000r] */ + dst2 = dst1; + dst2 = _mm_srli_pi16(dst2, 11); /* dst2 >> 11 -> dst2 [000r 000r 000r 000r] */ - /* blend */ - src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ - src2 = _mm_mullo_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ - src2 = _mm_srli_pi16(src2, 11); /* src2 >> 11 -> src2 */ - dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ - dst2 = _mm_slli_pi16(dst2, 11); /* dst2 << 11 -> dst2 */ + /* blend */ + src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ + src2 = _mm_mullo_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ + src2 = _mm_srli_pi16(src2, 11); /* src2 >> 11 -> src2 */ + dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ + dst2 = _mm_slli_pi16(dst2, 11); /* dst2 << 11 -> dst2 */ - mm_res = dst2; /* RED -> mm_res */ + mm_res = dst2; /* RED -> mm_res */ - /* green -- process the bits in place */ - src2 = src1; - src2 = _mm_and_si64(src2, gmask); /* src & MASKGREEN -> src2 */ + /* green -- process the bits in place */ + src2 = src1; + src2 = _mm_and_si64(src2, gmask); /* src & MASKGREEN -> src2 */ - dst2 = dst1; - dst2 = _mm_and_si64(dst2, gmask); /* dst & MASKGREEN -> dst2 */ + dst2 = dst1; + dst2 = _mm_and_si64(dst2, gmask); /* dst & MASKGREEN -> dst2 */ - /* blend */ - src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ - src2 = _mm_mulhi_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ - src2 = _mm_slli_pi16(src2, 5); /* src2 << 5 -> src2 */ - dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ + /* blend */ + src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ + src2 = _mm_mulhi_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ + src2 = _mm_slli_pi16(src2, 5); /* src2 << 5 -> src2 */ + dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ - mm_res = _mm_or_si64(mm_res, dst2); /* RED | GREEN -> mm_res */ + mm_res = _mm_or_si64(mm_res, dst2); /* RED | GREEN -> mm_res */ - /* blue */ - src2 = src1; - src2 = _mm_and_si64(src2, bmask); /* src & MASKBLUE -> src2[000b 000b 000b 000b] */ + /* blue */ + src2 = src1; + src2 = _mm_and_si64(src2, bmask); /* src & MASKBLUE -> src2[000b 000b 000b 000b] */ - dst2 = dst1; - dst2 = _mm_and_si64(dst2, bmask); /* dst & MASKBLUE -> dst2[000b 000b 000b 000b] */ + dst2 = dst1; + dst2 = _mm_and_si64(dst2, bmask); /* dst & MASKBLUE -> dst2[000b 000b 000b 000b] */ - /* blend */ - src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ - src2 = _mm_mullo_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ - src2 = _mm_srli_pi16(src2, 11); /* src2 >> 11 -> src2 */ - dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ - dst2 = _mm_and_si64(dst2, bmask); /* dst2 & MASKBLUE -> dst2 */ + /* blend */ + src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ + src2 = _mm_mullo_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ + src2 = _mm_srli_pi16(src2, 11); /* src2 >> 11 -> src2 */ + dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ + dst2 = _mm_and_si64(dst2, bmask); /* dst2 & MASKBLUE -> dst2 */ - mm_res = _mm_or_si64(mm_res, dst2); /* RED | GREEN | BLUE -> mm_res */ + mm_res = _mm_or_si64(mm_res, dst2); /* RED | GREEN | BLUE -> mm_res */ - *(__m64*)dstp = mm_res; /* mm_res -> 4 dst pixels */ + *(__m64*)dstp = mm_res; /* mm_res -> 4 dst pixels */ - srcp += 4; - dstp += 4; - }, width); - /* *INDENT-ON* */ + srcp += 4; + dstp += 4; + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -865,103 +865,103 @@ Blit555to555SurfaceAlphaMMX(SDL_BlitInfo * info) bmask = _mm_set_pi32(0x001F001F, 0x001F001F); /* MASKBLUE -> bmask */ while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP_124( - { - s = *srcp++; - d = *dstp; - /* - * shift out the middle component (green) to - * the high 16 bits, and process all three RGB - * components at the same time. - */ - s = (s | s << 16) & 0x03e07c1f; - d = (d | d << 16) & 0x03e07c1f; - d += (s - d) * alpha >> 5; - d &= 0x03e07c1f; - *dstp++ = (Uint16)(d | d >> 16); - },{ - s = *srcp++; - d = *dstp; - /* - * shift out the middle component (green) to - * the high 16 bits, and process all three RGB - * components at the same time. - */ - s = (s | s << 16) & 0x03e07c1f; - d = (d | d << 16) & 0x03e07c1f; - d += (s - d) * alpha >> 5; - d &= 0x03e07c1f; - *dstp++ = (Uint16)(d | d >> 16); - s = *srcp++; - d = *dstp; - /* - * shift out the middle component (green) to - * the high 16 bits, and process all three RGB - * components at the same time. - */ - s = (s | s << 16) & 0x03e07c1f; - d = (d | d << 16) & 0x03e07c1f; - d += (s - d) * alpha >> 5; - d &= 0x03e07c1f; - *dstp++ = (Uint16)(d | d >> 16); - },{ - src1 = *(__m64*)srcp; /* 4 src pixels -> src1 */ - dst1 = *(__m64*)dstp; /* 4 dst pixels -> dst1 */ + /* *INDENT-OFF* */ + DUFFS_LOOP_124( + { + s = *srcp++; + d = *dstp; + /* + * shift out the middle component (green) to + * the high 16 bits, and process all three RGB + * components at the same time. + */ + s = (s | s << 16) & 0x03e07c1f; + d = (d | d << 16) & 0x03e07c1f; + d += (s - d) * alpha >> 5; + d &= 0x03e07c1f; + *dstp++ = (Uint16)(d | d >> 16); + },{ + s = *srcp++; + d = *dstp; + /* + * shift out the middle component (green) to + * the high 16 bits, and process all three RGB + * components at the same time. + */ + s = (s | s << 16) & 0x03e07c1f; + d = (d | d << 16) & 0x03e07c1f; + d += (s - d) * alpha >> 5; + d &= 0x03e07c1f; + *dstp++ = (Uint16)(d | d >> 16); + s = *srcp++; + d = *dstp; + /* + * shift out the middle component (green) to + * the high 16 bits, and process all three RGB + * components at the same time. + */ + s = (s | s << 16) & 0x03e07c1f; + d = (d | d << 16) & 0x03e07c1f; + d += (s - d) * alpha >> 5; + d &= 0x03e07c1f; + *dstp++ = (Uint16)(d | d >> 16); + },{ + src1 = *(__m64*)srcp; /* 4 src pixels -> src1 */ + dst1 = *(__m64*)dstp; /* 4 dst pixels -> dst1 */ - /* red -- process the bits in place */ - src2 = src1; - src2 = _mm_and_si64(src2, rmask); /* src & MASKRED -> src2 */ + /* red -- process the bits in place */ + src2 = src1; + src2 = _mm_and_si64(src2, rmask); /* src & MASKRED -> src2 */ - dst2 = dst1; - dst2 = _mm_and_si64(dst2, rmask); /* dst & MASKRED -> dst2 */ + dst2 = dst1; + dst2 = _mm_and_si64(dst2, rmask); /* dst & MASKRED -> dst2 */ - /* blend */ - src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ - src2 = _mm_mulhi_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ - src2 = _mm_slli_pi16(src2, 5); /* src2 << 5 -> src2 */ - dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ - dst2 = _mm_and_si64(dst2, rmask); /* dst2 & MASKRED -> dst2 */ + /* blend */ + src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ + src2 = _mm_mulhi_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ + src2 = _mm_slli_pi16(src2, 5); /* src2 << 5 -> src2 */ + dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ + dst2 = _mm_and_si64(dst2, rmask); /* dst2 & MASKRED -> dst2 */ - mm_res = dst2; /* RED -> mm_res */ - - /* green -- process the bits in place */ - src2 = src1; - src2 = _mm_and_si64(src2, gmask); /* src & MASKGREEN -> src2 */ + mm_res = dst2; /* RED -> mm_res */ + + /* green -- process the bits in place */ + src2 = src1; + src2 = _mm_and_si64(src2, gmask); /* src & MASKGREEN -> src2 */ - dst2 = dst1; - dst2 = _mm_and_si64(dst2, gmask); /* dst & MASKGREEN -> dst2 */ + dst2 = dst1; + dst2 = _mm_and_si64(dst2, gmask); /* dst & MASKGREEN -> dst2 */ - /* blend */ - src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ - src2 = _mm_mulhi_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ - src2 = _mm_slli_pi16(src2, 5); /* src2 << 5 -> src2 */ - dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ + /* blend */ + src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ + src2 = _mm_mulhi_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ + src2 = _mm_slli_pi16(src2, 5); /* src2 << 5 -> src2 */ + dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ - mm_res = _mm_or_si64(mm_res, dst2); /* RED | GREEN -> mm_res */ + mm_res = _mm_or_si64(mm_res, dst2); /* RED | GREEN -> mm_res */ - /* blue */ - src2 = src1; /* src -> src2 */ - src2 = _mm_and_si64(src2, bmask); /* src & MASKBLUE -> src2[000b 000b 000b 000b] */ + /* blue */ + src2 = src1; /* src -> src2 */ + src2 = _mm_and_si64(src2, bmask); /* src & MASKBLUE -> src2[000b 000b 000b 000b] */ - dst2 = dst1; /* dst -> dst2 */ - dst2 = _mm_and_si64(dst2, bmask); /* dst & MASKBLUE -> dst2[000b 000b 000b 000b] */ + dst2 = dst1; /* dst -> dst2 */ + dst2 = _mm_and_si64(dst2, bmask); /* dst & MASKBLUE -> dst2[000b 000b 000b 000b] */ - /* blend */ - src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ - src2 = _mm_mullo_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ - src2 = _mm_srli_pi16(src2, 11); /* src2 >> 11 -> src2 */ - dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ - dst2 = _mm_and_si64(dst2, bmask); /* dst2 & MASKBLUE -> dst2 */ + /* blend */ + src2 = _mm_sub_pi16(src2, dst2);/* src - dst -> src2 */ + src2 = _mm_mullo_pi16(src2, mm_alpha); /* src2 * alpha -> src2 */ + src2 = _mm_srli_pi16(src2, 11); /* src2 >> 11 -> src2 */ + dst2 = _mm_add_pi16(src2, dst2); /* src2 + dst2 -> dst2 */ + dst2 = _mm_and_si64(dst2, bmask); /* dst2 & MASKBLUE -> dst2 */ - mm_res = _mm_or_si64(mm_res, dst2); /* RED | GREEN | BLUE -> mm_res */ + mm_res = _mm_or_si64(mm_res, dst2); /* RED | GREEN | BLUE -> mm_res */ - *(__m64*)dstp = mm_res; /* mm_res -> 4 dst pixels */ + *(__m64*)dstp = mm_res; /* mm_res -> 4 dst pixels */ - srcp += 4; - dstp += 4; - }, width); - /* *INDENT-ON* */ + srcp += 4; + dstp += 4; + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -988,22 +988,22 @@ Blit565to565SurfaceAlpha(SDL_BlitInfo * info) alpha >>= 3; /* downscale alpha to 5 bits */ while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4({ - Uint32 s = *srcp++; - Uint32 d = *dstp; - /* - * shift out the middle component (green) to - * the high 16 bits, and process all three RGB - * components at the same time. - */ - s = (s | s << 16) & 0x07e0f81f; - d = (d | d << 16) & 0x07e0f81f; - d += (s - d) * alpha >> 5; - d &= 0x07e0f81f; - *dstp++ = (Uint16)(d | d >> 16); - }, width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4({ + Uint32 s = *srcp++; + Uint32 d = *dstp; + /* + * shift out the middle component (green) to + * the high 16 bits, and process all three RGB + * components at the same time. + */ + s = (s | s << 16) & 0x07e0f81f; + d = (d | d << 16) & 0x07e0f81f; + d += (s - d) * alpha >> 5; + d &= 0x07e0f81f; + *dstp++ = (Uint16)(d | d >> 16); + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -1027,22 +1027,22 @@ Blit555to555SurfaceAlpha(SDL_BlitInfo * info) alpha >>= 3; /* downscale alpha to 5 bits */ while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4({ - Uint32 s = *srcp++; - Uint32 d = *dstp; - /* - * shift out the middle component (green) to - * the high 16 bits, and process all three RGB - * components at the same time. - */ - s = (s | s << 16) & 0x03e07c1f; - d = (d | d << 16) & 0x03e07c1f; - d += (s - d) * alpha >> 5; - d &= 0x03e07c1f; - *dstp++ = (Uint16)(d | d >> 16); - }, width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4({ + Uint32 s = *srcp++; + Uint32 d = *dstp; + /* + * shift out the middle component (green) to + * the high 16 bits, and process all three RGB + * components at the same time. + */ + s = (s | s << 16) & 0x03e07c1f; + d = (d | d << 16) & 0x03e07c1f; + d += (s - d) * alpha >> 5; + d &= 0x03e07c1f; + *dstp++ = (Uint16)(d | d >> 16); + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -1061,35 +1061,35 @@ BlitARGBto565PixelAlpha(SDL_BlitInfo * info) int dstskip = info->dst_skip >> 1; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4({ - Uint32 s = *srcp; - unsigned alpha = s >> 27; /* downscale alpha to 5 bits */ - /* FIXME: Here we special-case opaque alpha since the - compositioning used (>>8 instead of /255) doesn't handle - it correctly. Also special-case alpha=0 for speed? - Benchmark this! */ - if(alpha) { - if(alpha == (SDL_ALPHA_OPAQUE >> 3)) { - *dstp = (Uint16)((s >> 8 & 0xf800) + (s >> 5 & 0x7e0) + (s >> 3 & 0x1f)); - } else { - Uint32 d = *dstp; - /* - * convert source and destination to G0RAB65565 - * and blend all components at the same time - */ - s = ((s & 0xfc00) << 11) + (s >> 8 & 0xf800) - + (s >> 3 & 0x1f); - d = (d | d << 16) & 0x07e0f81f; - d += (s - d) * alpha >> 5; - d &= 0x07e0f81f; - *dstp = (Uint16)(d | d >> 16); - } - } - srcp++; - dstp++; - }, width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4({ + Uint32 s = *srcp; + unsigned alpha = s >> 27; /* downscale alpha to 5 bits */ + /* FIXME: Here we special-case opaque alpha since the + compositioning used (>>8 instead of /255) doesn't handle + it correctly. Also special-case alpha=0 for speed? + Benchmark this! */ + if(alpha) { + if(alpha == (SDL_ALPHA_OPAQUE >> 3)) { + *dstp = (Uint16)((s >> 8 & 0xf800) + (s >> 5 & 0x7e0) + (s >> 3 & 0x1f)); + } else { + Uint32 d = *dstp; + /* + * convert source and destination to G0RAB65565 + * and blend all components at the same time + */ + s = ((s & 0xfc00) << 11) + (s >> 8 & 0xf800) + + (s >> 3 & 0x1f); + d = (d | d << 16) & 0x07e0f81f; + d += (s - d) * alpha >> 5; + d &= 0x07e0f81f; + *dstp = (Uint16)(d | d >> 16); + } + } + srcp++; + dstp++; + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -1107,36 +1107,36 @@ BlitARGBto555PixelAlpha(SDL_BlitInfo * info) int dstskip = info->dst_skip >> 1; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4({ - unsigned alpha; - Uint32 s = *srcp; - alpha = s >> 27; /* downscale alpha to 5 bits */ - /* FIXME: Here we special-case opaque alpha since the - compositioning used (>>8 instead of /255) doesn't handle - it correctly. Also special-case alpha=0 for speed? - Benchmark this! */ - if(alpha) { - if(alpha == (SDL_ALPHA_OPAQUE >> 3)) { - *dstp = (Uint16)((s >> 9 & 0x7c00) + (s >> 6 & 0x3e0) + (s >> 3 & 0x1f)); - } else { - Uint32 d = *dstp; - /* - * convert source and destination to G0RAB65565 - * and blend all components at the same time - */ - s = ((s & 0xf800) << 10) + (s >> 9 & 0x7c00) - + (s >> 3 & 0x1f); - d = (d | d << 16) & 0x03e07c1f; - d += (s - d) * alpha >> 5; - d &= 0x03e07c1f; - *dstp = (Uint16)(d | d >> 16); - } - } - srcp++; - dstp++; - }, width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4({ + unsigned alpha; + Uint32 s = *srcp; + alpha = s >> 27; /* downscale alpha to 5 bits */ + /* FIXME: Here we special-case opaque alpha since the + compositioning used (>>8 instead of /255) doesn't handle + it correctly. Also special-case alpha=0 for speed? + Benchmark this! */ + if(alpha) { + if(alpha == (SDL_ALPHA_OPAQUE >> 3)) { + *dstp = (Uint16)((s >> 9 & 0x7c00) + (s >> 6 & 0x3e0) + (s >> 3 & 0x1f)); + } else { + Uint32 d = *dstp; + /* + * convert source and destination to G0RAB65565 + * and blend all components at the same time + */ + s = ((s & 0xf800) << 10) + (s >> 9 & 0x7c00) + + (s >> 3 & 0x1f); + d = (d | d << 16) & 0x03e07c1f; + d += (s - d) * alpha >> 5; + d &= 0x03e07c1f; + *dstp = (Uint16)(d | d >> 16); + } + } + srcp++; + dstp++; + }, width); + /* *INDENT-ON* */ srcp += srcskip; dstp += dstskip; } @@ -1163,18 +1163,18 @@ BlitNtoNSurfaceAlpha(SDL_BlitInfo * info) if (sA) { while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4( - { - DISEMBLE_RGB(src, srcbpp, srcfmt, Pixel, sR, sG, sB); - DISEMBLE_RGBA(dst, dstbpp, dstfmt, Pixel, dR, dG, dB, dA); - ALPHA_BLEND_RGBA(sR, sG, sB, sA, dR, dG, dB, dA); - ASSEMBLE_RGBA(dst, dstbpp, dstfmt, dR, dG, dB, dA); - src += srcbpp; - dst += dstbpp; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4( + { + DISEMBLE_RGB(src, srcbpp, srcfmt, Pixel, sR, sG, sB); + DISEMBLE_RGBA(dst, dstbpp, dstfmt, Pixel, dR, dG, dB, dA); + ALPHA_BLEND_RGBA(sR, sG, sB, sA, dR, dG, dB, dA); + ASSEMBLE_RGBA(dst, dstbpp, dstfmt, dR, dG, dB, dA); + src += srcbpp; + dst += dstbpp; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } @@ -1202,21 +1202,21 @@ BlitNtoNSurfaceAlphaKey(SDL_BlitInfo * info) const unsigned sA = info->a; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4( - { - RETRIEVE_RGB_PIXEL(src, srcbpp, Pixel); - if(sA && Pixel != ckey) { - RGB_FROM_PIXEL(Pixel, srcfmt, sR, sG, sB); - DISEMBLE_RGBA(dst, dstbpp, dstfmt, Pixel, dR, dG, dB, dA); - ALPHA_BLEND_RGBA(sR, sG, sB, sA, dR, dG, dB, dA); - ASSEMBLE_RGBA(dst, dstbpp, dstfmt, dR, dG, dB, dA); - } - src += srcbpp; - dst += dstbpp; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4( + { + RETRIEVE_RGB_PIXEL(src, srcbpp, Pixel); + if(sA && Pixel != ckey) { + RGB_FROM_PIXEL(Pixel, srcfmt, sR, sG, sB); + DISEMBLE_RGBA(dst, dstbpp, dstfmt, Pixel, dR, dG, dB, dA); + ALPHA_BLEND_RGBA(sR, sG, sB, sA, dR, dG, dB, dA); + ASSEMBLE_RGBA(dst, dstbpp, dstfmt, dR, dG, dB, dA); + } + src += srcbpp; + dst += dstbpp; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } @@ -1245,20 +1245,20 @@ BlitNtoNPixelAlpha(SDL_BlitInfo * info) dstbpp = dstfmt->BytesPerPixel; while (height--) { - /* *INDENT-OFF* */ - DUFFS_LOOP4( - { - DISEMBLE_RGBA(src, srcbpp, srcfmt, Pixel, sR, sG, sB, sA); - if(sA) { - DISEMBLE_RGBA(dst, dstbpp, dstfmt, Pixel, dR, dG, dB, dA); - ALPHA_BLEND_RGBA(sR, sG, sB, sA, dR, dG, dB, dA); - ASSEMBLE_RGBA(dst, dstbpp, dstfmt, dR, dG, dB, dA); - } - src += srcbpp; - dst += dstbpp; - }, - width); - /* *INDENT-ON* */ + /* *INDENT-OFF* */ + DUFFS_LOOP4( + { + DISEMBLE_RGBA(src, srcbpp, srcfmt, Pixel, sR, sG, sB, sA); + if(sA) { + DISEMBLE_RGBA(dst, dstbpp, dstfmt, Pixel, dR, dG, dB, dA); + ALPHA_BLEND_RGBA(sR, sG, sB, sA, dR, dG, dB, dA); + ASSEMBLE_RGBA(dst, dstbpp, dstfmt, dR, dG, dB, dA); + } + src += srcbpp; + dst += dstbpp; + }, + width); + /* *INDENT-ON* */ src += srcskip; dst += dstskip; } diff --git a/Engine/lib/sdl/src/video/SDL_blit_N.c b/Engine/lib/sdl/src/video/SDL_blit_N.c index 441cd9a21..d6ec417fe 100644 --- a/Engine/lib/sdl/src/video/SDL_blit_N.c +++ b/Engine/lib/sdl/src/video/SDL_blit_N.c @@ -2333,6 +2333,31 @@ BlitNtoNKey(SDL_BlitInfo * info) /* Set up some basic variables */ ckey &= rgbmask; + /* Fastpath: same source/destination format, no Amask, bpp 32, loop is vectorized. ~10x faster */ + if (srcfmt->format == dstfmt->format && + (srcfmt->format == SDL_PIXELFORMAT_RGB888 || srcfmt->format == SDL_PIXELFORMAT_BGR888)) { + Uint32 *src32 = (Uint32*)src; + Uint32 *dst32 = (Uint32*)dst; + srcskip /= sizeof(Uint32); + dstskip /= sizeof(Uint32); + while (height--) { + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + if (*src32 != ckey) { + *dst32 = *src32; + } + ++src32; + ++dst32; + }, + width); + /* *INDENT-ON* */ + src32 += srcskip; + dst32 += dstskip; + } + return; + } + while (height--) { /* *INDENT-OFF* */ DUFFS_LOOP( @@ -2380,6 +2405,34 @@ BlitNtoNKeyCopyAlpha(SDL_BlitInfo * info) dstbpp = dstfmt->BytesPerPixel; ckey &= rgbmask; + /* Fastpath: same source/destination format, with Amask, bpp 32, loop is vectorized. ~10x faster */ + if (srcfmt->format == dstfmt->format && + (srcfmt->format == SDL_PIXELFORMAT_ARGB8888 || + srcfmt->format == SDL_PIXELFORMAT_ABGR8888 || + srcfmt->format == SDL_PIXELFORMAT_BGRA8888 || + srcfmt->format == SDL_PIXELFORMAT_RGBA8888)) { + Uint32 *src32 = (Uint32*)src; + Uint32 *dst32 = (Uint32*)dst; + srcskip /= sizeof(Uint32); + dstskip /= sizeof(Uint32); + while (height--) { + /* *INDENT-OFF* */ + DUFFS_LOOP( + { + if ((*src32 & rgbmask) != ckey) { + *dst32 = *src32; + } + ++src32; + ++dst32; + }, + width); + /* *INDENT-ON* */ + src32 += srcskip; + dst32 += dstskip; + } + return; + } + while (height--) { /* *INDENT-OFF* */ DUFFS_LOOP( diff --git a/Engine/lib/sdl/src/video/SDL_blit_copy.h b/Engine/lib/sdl/src/video/SDL_blit_copy.h index 46651791e..d569ae00c 100644 --- a/Engine/lib/sdl/src/video/SDL_blit_copy.h +++ b/Engine/lib/sdl/src/video/SDL_blit_copy.h @@ -19,6 +19,11 @@ 3. This notice may not be removed or altered from any source distribution. */ +#ifndef SDL_blit_copy_h_ +#define SDL_blit_copy_h_ + void SDL_BlitCopy(SDL_BlitInfo * info); +#endif /* SDL_blit_copy_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/SDL_blit_slow.h b/Engine/lib/sdl/src/video/SDL_blit_slow.h index 02d360a6e..d27fcd275 100644 --- a/Engine/lib/sdl/src/video/SDL_blit_slow.h +++ b/Engine/lib/sdl/src/video/SDL_blit_slow.h @@ -18,8 +18,14 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_blit_slow_h_ +#define SDL_blit_slow_h_ + #include "../SDL_internal.h" extern void SDL_Blit_Slow(SDL_BlitInfo * info); +#endif /* SDL_blit_slow_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/SDL_egl.c b/Engine/lib/sdl/src/video/SDL_egl.c index 0daf98ac1..78abc030e 100644 --- a/Engine/lib/sdl/src/video/SDL_egl.c +++ b/Engine/lib/sdl/src/video/SDL_egl.c @@ -406,6 +406,9 @@ SDL_EGL_LoadLibrary(_THIS, const char *egl_path, NativeDisplayType native_displa } } + _this->egl_data->egl_version_major = egl_version_major; + _this->egl_data->egl_version_minor = egl_version_minor; + if (egl_version_major == 1 && egl_version_minor == 5) { LOAD_FUNC(eglGetPlatformDisplay); } @@ -446,6 +449,64 @@ SDL_EGL_LoadLibrary(_THIS, const char *egl_path, NativeDisplayType native_displa return 0; } +#ifdef DUMP_EGL_CONFIG + +#define ATTRIBUTE(_attr) { _attr, #_attr } + +typedef struct { + EGLint attribute; + char const* name; +} Attribute; + +Attribute attributes[] = { + ATTRIBUTE( EGL_BUFFER_SIZE ), + ATTRIBUTE( EGL_ALPHA_SIZE ), + ATTRIBUTE( EGL_BLUE_SIZE ), + ATTRIBUTE( EGL_GREEN_SIZE ), + ATTRIBUTE( EGL_RED_SIZE ), + ATTRIBUTE( EGL_DEPTH_SIZE ), + ATTRIBUTE( EGL_STENCIL_SIZE ), + ATTRIBUTE( EGL_CONFIG_CAVEAT ), + ATTRIBUTE( EGL_CONFIG_ID ), + ATTRIBUTE( EGL_LEVEL ), + ATTRIBUTE( EGL_MAX_PBUFFER_HEIGHT ), + ATTRIBUTE( EGL_MAX_PBUFFER_WIDTH ), + ATTRIBUTE( EGL_MAX_PBUFFER_PIXELS ), + ATTRIBUTE( EGL_NATIVE_RENDERABLE ), + ATTRIBUTE( EGL_NATIVE_VISUAL_ID ), + ATTRIBUTE( EGL_NATIVE_VISUAL_TYPE ), + ATTRIBUTE( EGL_SAMPLES ), + ATTRIBUTE( EGL_SAMPLE_BUFFERS ), + ATTRIBUTE( EGL_SURFACE_TYPE ), + ATTRIBUTE( EGL_TRANSPARENT_TYPE ), + ATTRIBUTE( EGL_TRANSPARENT_BLUE_VALUE ), + ATTRIBUTE( EGL_TRANSPARENT_GREEN_VALUE ), + ATTRIBUTE( EGL_TRANSPARENT_RED_VALUE ), + ATTRIBUTE( EGL_BIND_TO_TEXTURE_RGB ), + ATTRIBUTE( EGL_BIND_TO_TEXTURE_RGBA ), + ATTRIBUTE( EGL_MIN_SWAP_INTERVAL ), + ATTRIBUTE( EGL_MAX_SWAP_INTERVAL ), + ATTRIBUTE( EGL_LUMINANCE_SIZE ), + ATTRIBUTE( EGL_ALPHA_MASK_SIZE ), + ATTRIBUTE( EGL_COLOR_BUFFER_TYPE ), + ATTRIBUTE( EGL_RENDERABLE_TYPE ), + ATTRIBUTE( EGL_MATCH_NATIVE_PIXMAP ), + ATTRIBUTE( EGL_CONFORMANT ), +}; + + +static void dumpconfig(_THIS, EGLConfig config) +{ + int attr; + for (attr = 0 ; attregl_data->eglGetConfigAttrib(_this->egl_data->egl_display, config, attributes[attr].attribute, &value); + SDL_Log("\t%-32s: %10d (0x%08x)\n", attributes[attr].name, value, value); + } +} + +#endif /* DUMP_EGL_CONFIG */ + int SDL_EGL_ChooseConfig(_THIS) { @@ -570,6 +631,10 @@ SDL_EGL_ChooseConfig(_THIS) break; /* we found an exact match! */ } } + +#ifdef DUMP_EGL_CONFIG + dumpconfig(_this, _this->egl_data->egl_config); +#endif return 0; } @@ -596,6 +661,24 @@ SDL_EGL_CreateContext(_THIS, EGLSurface egl_surface) share_context = (EGLContext)SDL_GL_GetCurrentContext(); } +#if SDL_VIDEO_DRIVER_ANDROID + if ((_this->gl_config.flags & SDL_GL_CONTEXT_DEBUG_FLAG) != 0) { + /* If SDL_GL_CONTEXT_DEBUG_FLAG is set but EGL_KHR_debug unsupported, unset. + * This is required because some Android devices like to complain about it + * by "silently" failing, logging a hint which could be easily overlooked: + * E/libEGL (26984): validate_display:255 error 3008 (EGL_BAD_DISPLAY) + * The following explicitly checks for EGL_KHR_debug before EGL 1.5 + */ + int egl_version_major = _this->egl_data->egl_version_major; + int egl_version_minor = _this->egl_data->egl_version_minor; + if (((egl_version_major < 1) || (egl_version_major == 1 && egl_version_minor < 5)) && + !SDL_EGL_HasExtension(_this, SDL_EGL_DISPLAY_EXTENSION, "EGL_KHR_debug")) { + /* SDL profile bits match EGL profile bits. */ + _this->gl_config.flags &= ~SDL_GL_CONTEXT_DEBUG_FLAG; + } + } +#endif + /* Set the context version and other attributes. */ if ((major_version < 3 || (minor_version == 0 && profile_es)) && _this->gl_config.flags == 0 && @@ -763,7 +846,6 @@ SDL_EGL_DeleteContext(_THIS, SDL_GLContext context) } if (egl_context != NULL && egl_context != EGL_NO_CONTEXT) { - SDL_EGL_MakeCurrent(_this, NULL, NULL); _this->egl_data->eglDestroyContext(_this->egl_data->egl_display, egl_context); } @@ -775,7 +857,7 @@ SDL_EGL_CreateSurface(_THIS, NativeWindowType nw) /* max 2 values plus terminator. */ EGLint attribs[3]; int attr = 0; - + EGLSurface * surface; if (SDL_EGL_ChooseConfig(_this) != 0) { @@ -807,7 +889,7 @@ SDL_EGL_CreateSurface(_THIS, NativeWindowType nw) return EGL_NO_SURFACE; } } - + attribs[attr++] = EGL_NONE; surface = _this->egl_data->eglCreateWindowSurface( diff --git a/Engine/lib/sdl/src/video/SDL_egl_c.h b/Engine/lib/sdl/src/video/SDL_egl_c.h index 80b13192e..d1c41297c 100644 --- a/Engine/lib/sdl/src/video/SDL_egl_c.h +++ b/Engine/lib/sdl/src/video/SDL_egl_c.h @@ -36,6 +36,7 @@ typedef struct SDL_EGL_VideoData EGLConfig egl_config; int egl_swapinterval; int egl_surfacetype; + int egl_version_major, egl_version_minor; EGLDisplay(EGLAPIENTRY *eglGetDisplay) (NativeDisplayType display); EGLDisplay(EGLAPIENTRY *eglGetPlatformDisplay) (EGLenum platform, diff --git a/Engine/lib/sdl/src/video/SDL_pixels.c b/Engine/lib/sdl/src/video/SDL_pixels.c index 2e263955c..c2e41632f 100644 --- a/Engine/lib/sdl/src/video/SDL_pixels.c +++ b/Engine/lib/sdl/src/video/SDL_pixels.c @@ -326,7 +326,7 @@ SDL_MasksToPixelFormatEnum(int bpp, Uint32 Rmask, Uint32 Gmask, Uint32 Bmask, if (Rmask == 0) { return SDL_PIXELFORMAT_RGB555; } - /* fallthrough */ + /* fallthrough */ case 16: if (Rmask == 0) { return SDL_PIXELFORMAT_RGB565; diff --git a/Engine/lib/sdl/src/video/SDL_pixels_c.h b/Engine/lib/sdl/src/video/SDL_pixels_c.h index 900f0b080..c84e155f1 100644 --- a/Engine/lib/sdl/src/video/SDL_pixels_c.h +++ b/Engine/lib/sdl/src/video/SDL_pixels_c.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_pixels_c_h_ +#define SDL_pixels_c_h_ + #include "../SDL_internal.h" /* Useful functions and variables from SDL_pixel.c */ @@ -37,4 +41,6 @@ extern void SDL_FreeBlitMap(SDL_BlitMap * map); extern void SDL_DitherColors(SDL_Color * colors, int bpp); extern Uint8 SDL_FindColor(SDL_Palette * pal, Uint8 r, Uint8 g, Uint8 b, Uint8 a); +#endif /* SDL_pixels_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/SDL_rect_c.h b/Engine/lib/sdl/src/video/SDL_rect_c.h index d67e49348..56d6f2e07 100644 --- a/Engine/lib/sdl/src/video/SDL_rect_c.h +++ b/Engine/lib/sdl/src/video/SDL_rect_c.h @@ -18,8 +18,14 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_rect_c_h_ +#define SDL_rect_c_h_ + #include "../SDL_internal.h" extern SDL_bool SDL_GetSpanEnclosingRect(int width, int height, int numrects, const SDL_Rect * rects, SDL_Rect *span); +#endif /* SDL_rect_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/SDL_shape_internals.h b/Engine/lib/sdl/src/video/SDL_shape_internals.h index 3af175cdf..49a8786a1 100644 --- a/Engine/lib/sdl/src/video/SDL_shape_internals.h +++ b/Engine/lib/sdl/src/video/SDL_shape_internals.h @@ -36,21 +36,21 @@ extern "C" { #endif typedef struct { - struct SDL_ShapeTree *upleft,*upright,*downleft,*downright; + struct SDL_ShapeTree *upleft,*upright,*downleft,*downright; } SDL_QuadTreeChildren; typedef union { - SDL_QuadTreeChildren children; - SDL_Rect shape; + SDL_QuadTreeChildren children; + SDL_Rect shape; } SDL_ShapeUnion; typedef enum { QuadShape,TransparentShape,OpaqueShape } SDL_ShapeKind; typedef struct { - SDL_ShapeKind kind; - SDL_ShapeUnion data; + SDL_ShapeKind kind; + SDL_ShapeUnion data; } SDL_ShapeTree; - + typedef void(*SDL_TraversalFunction)(SDL_ShapeTree*,void*); extern void SDL_CalculateShapeBitmap(SDL_WindowShapeMode mode,SDL_Surface *shape,Uint8* bitmap,Uint8 ppb); diff --git a/Engine/lib/sdl/src/video/SDL_surface.c b/Engine/lib/sdl/src/video/SDL_surface.c index 719f831e8..1b2ee6cea 100644 --- a/Engine/lib/sdl/src/video/SDL_surface.c +++ b/Engine/lib/sdl/src/video/SDL_surface.c @@ -37,7 +37,7 @@ SDL_COMPILE_TIME_ASSERT(surface_size_assumptions, /* * Calculate the pad-aligned scanline width of a surface */ -int +static int SDL_CalculatePitch(Uint32 format, int width) { int pitch; @@ -292,6 +292,20 @@ SDL_SetColorKey(SDL_Surface * surface, int flag, Uint32 key) return 0; } +SDL_bool +SDL_HasColorKey(SDL_Surface * surface) +{ + if (!surface) { + return SDL_FALSE; + } + + if (!(surface->map->info.flags & SDL_COPY_COLORKEY)) { + return SDL_FALSE; + } + + return SDL_TRUE; +} + int SDL_GetColorKey(SDL_Surface * surface, Uint32 * key) { diff --git a/Engine/lib/sdl/src/video/SDL_sysvideo.h b/Engine/lib/sdl/src/video/SDL_sysvideo.h index 9df71c9aa..25862ca1b 100644 --- a/Engine/lib/sdl/src/video/SDL_sysvideo.h +++ b/Engine/lib/sdl/src/video/SDL_sysvideo.h @@ -119,8 +119,8 @@ struct SDL_Window !((W)->flags & SDL_WINDOW_MINIMIZED)) /* - * Define the SDL display structure This corresponds to physical monitors - * attached to the system. + * Define the SDL display structure. + * This corresponds to physical monitors attached to the system. */ struct SDL_VideoDisplay { @@ -130,6 +130,7 @@ struct SDL_VideoDisplay SDL_DisplayMode *display_modes; SDL_DisplayMode desktop_mode; SDL_DisplayMode current_mode; + SDL_DisplayOrientation orientation; SDL_Window *fullscreen_window; @@ -180,16 +181,16 @@ struct SDL_VideoDevice */ int (*GetDisplayBounds) (_THIS, SDL_VideoDisplay * display, SDL_Rect * rect); - /* - * Get the dots/pixels-per-inch of a display - */ - int (*GetDisplayDPI) (_THIS, SDL_VideoDisplay * display, float * ddpi, float * hdpi, float * vdpi); - /* * Get the usable bounds of a display (bounds minus menubar or whatever) */ int (*GetDisplayUsableBounds) (_THIS, SDL_VideoDisplay * display, SDL_Rect * rect); + /* + * Get the dots/pixels-per-inch of a display + */ + int (*GetDisplayDPI) (_THIS, SDL_VideoDisplay * display, float * ddpi, float * hdpi, float * vdpi); + /* * Get a list of the available display modes for a display. */ @@ -304,6 +305,9 @@ struct SDL_VideoDevice /* Hit-testing */ int (*SetWindowHitTest)(SDL_Window * window, SDL_bool enabled); + /* Tell window that app enabled drag'n'drop events */ + void (*AcceptDragAndDrop)(SDL_Window * window, SDL_bool accept); + /* * * */ /* Data common to all drivers */ SDL_bool is_dummy; @@ -423,6 +427,8 @@ extern SDL_VideoDevice *SDL_GetVideoDevice(void); extern int SDL_AddBasicVideoDisplay(const SDL_DisplayMode * desktop_mode); extern int SDL_AddVideoDisplay(const SDL_VideoDisplay * display); extern SDL_bool SDL_AddDisplayMode(SDL_VideoDisplay *display, const SDL_DisplayMode * mode); +extern int SDL_GetIndexOfDisplay(SDL_VideoDisplay *display); +extern SDL_VideoDisplay *SDL_GetDisplay(int displayIndex); extern SDL_VideoDisplay *SDL_GetDisplayForWindow(SDL_Window *window); extern void *SDL_GetDisplayDriverData( int displayIndex ); @@ -454,6 +460,8 @@ extern void SDL_OnApplicationDidEnterBackground(void); extern void SDL_OnApplicationWillEnterForeground(void); extern void SDL_OnApplicationDidBecomeActive(void); +extern void SDL_ToggleDragAndDropSupport(void); + #endif /* SDL_sysvideo_h_ */ /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/SDL_video.c b/Engine/lib/sdl/src/video/SDL_video.c index 8cf195d1a..336fdaa50 100644 --- a/Engine/lib/sdl/src/video/SDL_video.c +++ b/Engine/lib/sdl/src/video/SDL_video.c @@ -641,7 +641,7 @@ SDL_GetNumVideoDisplays(void) return _this->num_displays; } -static int +int SDL_GetIndexOfDisplay(SDL_VideoDisplay *display) { int displayIndex; @@ -739,6 +739,17 @@ SDL_GetDisplayDPI(int displayIndex, float * ddpi, float * hdpi, float * vdpi) return -1; } +SDL_DisplayOrientation +SDL_GetDisplayOrientation(int displayIndex) +{ + SDL_VideoDisplay *display; + + CHECK_DISPLAY_INDEX(displayIndex, SDL_ORIENTATION_UNKNOWN); + + display = &_this->displays[displayIndex]; + return display->orientation; +} + SDL_bool SDL_AddDisplayMode(SDL_VideoDisplay * display, const SDL_DisplayMode * mode) { @@ -1000,7 +1011,7 @@ SDL_SetDisplayModeForDisplay(SDL_VideoDisplay * display, const SDL_DisplayMode * /* Actually change the display mode */ if (!_this->SetDisplayMode) { - return SDL_SetError("Video driver doesn't support changing display mode"); + return SDL_SetError("SDL video driver doesn't support changing display mode"); } if (_this->SetDisplayMode(_this, display, &display_mode) < 0) { return -1; @@ -1009,6 +1020,14 @@ SDL_SetDisplayModeForDisplay(SDL_VideoDisplay * display, const SDL_DisplayMode * return 0; } +SDL_VideoDisplay * +SDL_GetDisplay(int displayIndex) +{ + CHECK_DISPLAY_INDEX(displayIndex, NULL); + + return &_this->displays[displayIndex]; +} + int SDL_GetWindowDisplayIndex(SDL_Window * window) { @@ -1289,8 +1308,15 @@ SDL_UpdateFullscreenMode(SDL_Window * window, SDL_bool fullscreen) /* Generate a mode change event here */ if (resized) { +#ifndef ANDROID + // Android may not resize the window to exactly what our fullscreen mode is, especially on + // windowed Android environments like the Chromebook or Samsung DeX. Given this, we shouldn't + // use fullscreen_mode.w and fullscreen_mode.h, but rather get our current native size. As such, + // Android's SetWindowFullscreen will generate the window event for us with the proper final size. + SDL_SendWindowEvent(other, SDL_WINDOWEVENT_RESIZED, fullscreen_mode.w, fullscreen_mode.h); +#endif } else { SDL_OnWindowResized(other); } @@ -1322,11 +1348,45 @@ SDL_UpdateFullscreenMode(SDL_Window * window, SDL_bool fullscreen) } #define CREATE_FLAGS \ - (SDL_WINDOW_OPENGL | SDL_WINDOW_BORDERLESS | SDL_WINDOW_RESIZABLE | SDL_WINDOW_ALLOW_HIGHDPI | SDL_WINDOW_ALWAYS_ON_TOP | SDL_WINDOW_SKIP_TASKBAR | SDL_WINDOW_POPUP_MENU | SDL_WINDOW_UTILITY | SDL_WINDOW_TOOLTIP | SDL_WINDOW_VULKAN) + (SDL_WINDOW_OPENGL | SDL_WINDOW_BORDERLESS | SDL_WINDOW_RESIZABLE | SDL_WINDOW_ALLOW_HIGHDPI | SDL_WINDOW_ALWAYS_ON_TOP | SDL_WINDOW_SKIP_TASKBAR | SDL_WINDOW_POPUP_MENU | SDL_WINDOW_UTILITY | SDL_WINDOW_TOOLTIP | SDL_WINDOW_VULKAN | SDL_WINDOW_MINIMIZED) + +static SDL_INLINE SDL_bool +IsAcceptingDragAndDrop(void) +{ + if ((SDL_GetEventState(SDL_DROPFILE) == SDL_ENABLE) || + (SDL_GetEventState(SDL_DROPTEXT) == SDL_ENABLE)) { + return SDL_TRUE; + } + return SDL_FALSE; +} + +/* prepare a newly-created window */ +static SDL_INLINE void +PrepareDragAndDropSupport(SDL_Window *window) +{ + if (_this->AcceptDragAndDrop) { + _this->AcceptDragAndDrop(window, IsAcceptingDragAndDrop()); + } +} + +/* toggle d'n'd for all existing windows. */ +void +SDL_ToggleDragAndDropSupport(void) +{ + if (_this && _this->AcceptDragAndDrop) { + const SDL_bool enable = IsAcceptingDragAndDrop(); + SDL_Window *window; + for (window = _this->windows; window; window = window->next) { + _this->AcceptDragAndDrop(window, enable); + } + } +} static void SDL_FinishWindowCreation(SDL_Window *window, Uint32 flags) { + PrepareDragAndDropSupport(window); + if (flags & SDL_WINDOW_MAXIMIZED) { SDL_MaximizeWindow(window); } @@ -1383,7 +1443,9 @@ SDL_CreateWindow(const char *title, int x, int y, int w, int h, Uint32 flags) #endif if (flags & SDL_WINDOW_OPENGL) { if (!_this->GL_CreateContext) { - SDL_SetError("No OpenGL support in video driver"); + SDL_SetError("OpenGL support is either not configured in SDL " + "or not available in current SDL video driver " + "(%s) or platform", _this->name); return NULL; } if (SDL_GL_LoadLibrary(NULL) < 0) { @@ -1394,7 +1456,8 @@ SDL_CreateWindow(const char *title, int x, int y, int w, int h, Uint32 flags) if (flags & SDL_WINDOW_VULKAN) { if (!_this->Vulkan_CreateSurface) { SDL_SetError("Vulkan support is either not configured in SDL " - "or not available in video driver"); + "or not available in current SDL video driver " + "(%s) or platform", _this->name); return NULL; } if (flags & SDL_WINDOW_OPENGL) { @@ -1477,6 +1540,15 @@ SDL_CreateWindow(const char *title, int x, int y, int w, int h, Uint32 flags) return NULL; } + /* Clear minimized if not on windows, only windows handles it at create rather than FinishWindowCreation, + * but it's important or window focus will get broken on windows! + */ +#if !defined(__WIN32__) + if (window->flags & SDL_WINDOW_MINIMIZED) { + window->flags &= ~SDL_WINDOW_MINIMIZED; + } +#endif + #if __WINRT__ && (NTDDI_VERSION < NTDDI_WIN10) /* HACK: WinRT 8.x apps can't choose whether or not they are fullscreen or not. The user can choose this, via OS-provided UI, but this can't @@ -1535,6 +1607,9 @@ SDL_CreateWindowFrom(const void *data) SDL_DestroyWindow(window); return NULL; } + + PrepareDragAndDropSupport(window); + return window; } @@ -1544,7 +1619,9 @@ SDL_RecreateWindow(SDL_Window * window, Uint32 flags) SDL_bool loaded_opengl = SDL_FALSE; if ((flags & SDL_WINDOW_OPENGL) && !_this->GL_CreateContext) { - return SDL_SetError("No OpenGL support in video driver"); + return SDL_SetError("OpenGL support is either not configured in SDL " + "or not available in current SDL video driver " + "(%s) or platform", _this->name); } if (window->flags & SDL_WINDOW_FOREIGN) { @@ -2787,7 +2864,7 @@ SDL_GL_LoadLibrary(const char *path) retval = 0; } else { if (!_this->GL_LoadLibrary) { - return SDL_SetError("No dynamic GL support in video driver"); + return SDL_SetError("No dynamic GL support in current SDL video driver (%s)", _this->name); } retval = _this->GL_LoadLibrary(_this, path); } @@ -2818,7 +2895,7 @@ SDL_GL_GetProcAddress(const char *proc) SDL_SetError("No GL driver has been loaded"); } } else { - SDL_SetError("No dynamic GL support in video driver"); + SDL_SetError("No dynamic GL support in current SDL video driver (%s)", _this->name); } return func; } @@ -3774,6 +3851,8 @@ SDL_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) if (!messageboxdata) { return SDL_InvalidParamError("messageboxdata"); + } else if (messageboxdata->numbuttons < 0) { + return SDL_SetError("Invalid number of buttons"); } current_window = SDL_GetKeyboardFocus(); @@ -3983,7 +4062,9 @@ int SDL_Vulkan_LoadLibrary(const char *path) retval = 0; } else { if (!_this->Vulkan_LoadLibrary) { - return SDL_SetError("No Vulkan support in video driver"); + return SDL_SetError("Vulkan support is either not configured in SDL " + "or not available in current SDL video driver " + "(%s) or platform", _this->name); } retval = _this->Vulkan_LoadLibrary(_this, path); } @@ -4024,11 +4105,14 @@ void SDL_Vulkan_UnloadLibrary(void) SDL_bool SDL_Vulkan_GetInstanceExtensions(SDL_Window *window, unsigned *count, const char **names) { - CHECK_WINDOW_MAGIC(window, SDL_FALSE); + if (window) { + CHECK_WINDOW_MAGIC(window, SDL_FALSE); - if (!(window->flags & SDL_WINDOW_VULKAN)) { - SDL_SetError(NOT_A_VULKAN_WINDOW); - return SDL_FALSE; + if (!(window->flags & SDL_WINDOW_VULKAN)) + { + SDL_SetError(NOT_A_VULKAN_WINDOW); + return SDL_FALSE; + } } if (!count) { diff --git a/Engine/lib/sdl/src/video/SDL_vulkan_utils.c b/Engine/lib/sdl/src/video/SDL_vulkan_utils.c index d4cbed4d9..1b242f132 100644 --- a/Engine/lib/sdl/src/video/SDL_vulkan_utils.c +++ b/Engine/lib/sdl/src/video/SDL_vulkan_utils.c @@ -123,10 +123,10 @@ VkExtensionProperties *SDL_Vulkan_CreateInstanceExtensionsList( { retval = SDL_calloc(1, sizeof(VkExtensionProperties)); // so we can return non-null } - else - { - retval = SDL_calloc(count, sizeof(VkExtensionProperties)); - } + else + { + retval = SDL_calloc(count, sizeof(VkExtensionProperties)); + } if(!retval) { SDL_OutOfMemory(); diff --git a/Engine/lib/sdl/src/video/SDL_yuv.c b/Engine/lib/sdl/src/video/SDL_yuv.c index 50910a5f7..03b04dc8a 100644 --- a/Engine/lib/sdl/src/video/SDL_yuv.c +++ b/Engine/lib/sdl/src/video/SDL_yuv.c @@ -23,6 +23,7 @@ #include "SDL_endian.h" #include "SDL_video.h" #include "SDL_pixels_c.h" +#include "SDL_yuv_c.h" #include "yuv2rgb/yuv_rgb.h" @@ -89,10 +90,10 @@ static SDL_bool IsPacked4Format(Uint32 format) } static int GetYUVPlanes(int width, int height, Uint32 format, const void *yuv, int yuv_pitch, - const Uint8 **y, const Uint8 **u, const Uint8 **v, Uint32 *y_stride, Uint32 *uv_stride) + const Uint8 **y, const Uint8 **u, const Uint8 **v, Uint32 *y_stride, Uint32 *uv_stride) { - const Uint8 *planes[3] = { NULL, NULL, NULL }; - int pitches[3] = { 0, 0, 0 }; + const Uint8 *planes[3] = { NULL, NULL, NULL }; + int pitches[3] = { 0, 0, 0 }; switch (format) { case SDL_PIXELFORMAT_YV12: @@ -180,10 +181,10 @@ static int GetYUVPlanes(int width, int height, Uint32 format, const void *yuv, i static SDL_bool yuv_rgb_sse( Uint32 src_format, Uint32 dst_format, - Uint32 width, Uint32 height, - const Uint8 *y, const Uint8 *u, const Uint8 *v, Uint32 y_stride, Uint32 uv_stride, - Uint8 *rgb, Uint32 rgb_stride, - YCbCrType yuv_type) + Uint32 width, Uint32 height, + const Uint8 *y, const Uint8 *u, const Uint8 *v, Uint32 y_stride, Uint32 uv_stride, + Uint8 *rgb, Uint32 rgb_stride, + YCbCrType yuv_type) { #ifdef __SSE2__ if (!SDL_HasSSE2()) { @@ -289,10 +290,10 @@ static SDL_bool yuv_rgb_sse( static SDL_bool yuv_rgb_std( Uint32 src_format, Uint32 dst_format, - Uint32 width, Uint32 height, - const Uint8 *y, const Uint8 *u, const Uint8 *v, Uint32 y_stride, Uint32 uv_stride, - Uint8 *rgb, Uint32 rgb_stride, - YCbCrType yuv_type) + Uint32 width, Uint32 height, + const Uint8 *y, const Uint8 *u, const Uint8 *v, Uint32 y_stride, Uint32 uv_stride, + Uint8 *rgb, Uint32 rgb_stride, + YCbCrType yuv_type) { if (src_format == SDL_PIXELFORMAT_YV12 || src_format == SDL_PIXELFORMAT_IYUV) { @@ -395,7 +396,7 @@ SDL_ConvertPixels_YUV_to_RGB(int width, int height, Uint32 src_format, const void *src, int src_pitch, Uint32 dst_format, void *dst, int dst_pitch) { - const Uint8 *y = NULL; + const Uint8 *y = NULL; const Uint8 *u = NULL; const Uint8 *v = NULL; Uint32 y_stride = 0; @@ -553,7 +554,7 @@ SDL_ConvertPixels_ARGB8888_to_YUV(int width, int height, const void *src, int sr Uint32 y_stride, uv_stride, y_skip, uv_skip; GetYUVPlanes(width, height, dst_format, dst, dst_pitch, - (const Uint8 **)&plane_y, (const Uint8 **)&plane_u, (const Uint8 **)&plane_v, + (const Uint8 **)&plane_y, (const Uint8 **)&plane_u, (const Uint8 **)&plane_v, &y_stride, &uv_stride); plane_interleaved_uv = (plane_y + height * y_stride); y_skip = (y_stride - width); diff --git a/Engine/lib/sdl/src/video/SDL_yuv_c.h b/Engine/lib/sdl/src/video/SDL_yuv_c.h index 6fe02b0fc..192bd2c57 100644 --- a/Engine/lib/sdl/src/video/SDL_yuv_c.h +++ b/Engine/lib/sdl/src/video/SDL_yuv_c.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_yuv_c_h_ +#define SDL_yuv_c_h_ + #include "../SDL_internal.h" @@ -27,4 +31,6 @@ extern int SDL_ConvertPixels_YUV_to_RGB(int width, int height, Uint32 src_format extern int SDL_ConvertPixels_RGB_to_YUV(int width, int height, Uint32 src_format, const void *src, int src_pitch, Uint32 dst_format, void *dst, int dst_pitch); extern int SDL_ConvertPixels_YUV_to_YUV(int width, int height, Uint32 src_format, const void *src, int src_pitch, Uint32 dst_format, void *dst, int dst_pitch); +#endif /* SDL_yuv_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/android/SDL_androidmouse.c b/Engine/lib/sdl/src/video/android/SDL_androidmouse.c index 1c075fb4a..037b4538c 100644 --- a/Engine/lib/sdl/src/video/android/SDL_androidmouse.c +++ b/Engine/lib/sdl/src/video/android/SDL_androidmouse.c @@ -42,15 +42,164 @@ #define BUTTON_BACK 8 #define BUTTON_FORWARD 16 +typedef struct +{ + int custom_cursor; + int system_cursor; + +} SDL_AndroidCursorData; + /* Last known Android mouse button state (includes all buttons) */ static int last_state; +/* Blank cursor */ +static SDL_Cursor *empty_cursor; + +static SDL_Cursor * +Android_WrapCursor(int custom_cursor, int system_cursor) +{ + SDL_Cursor *cursor; + + cursor = SDL_calloc(1, sizeof(*cursor)); + if (cursor) { + SDL_AndroidCursorData *data = (SDL_AndroidCursorData*)SDL_calloc(1, sizeof(*data)); + if (data) { + data->custom_cursor = custom_cursor; + data->system_cursor = system_cursor; + cursor->driverdata = data; + } else { + SDL_free(cursor); + cursor = NULL; + SDL_OutOfMemory(); + } + } else { + SDL_OutOfMemory(); + } + + return cursor; +} + +static SDL_Cursor * +Android_CreateDefaultCursor() +{ + return Android_WrapCursor(0, SDL_SYSTEM_CURSOR_ARROW); +} + +static SDL_Cursor * +Android_CreateCursor(SDL_Surface * surface, int hot_x, int hot_y) +{ + int custom_cursor; + SDL_Surface *converted; + + converted = SDL_ConvertSurfaceFormat(surface, SDL_PIXELFORMAT_ARGB8888, 0); + if (!converted) { + return NULL; + } + custom_cursor = Android_JNI_CreateCustomCursor(converted, hot_x, hot_y); + SDL_FreeSurface(converted); + if (!custom_cursor) { + SDL_Unsupported(); + return NULL; + } + return Android_WrapCursor(custom_cursor, 0); +} + +static SDL_Cursor * +Android_CreateSystemCursor(SDL_SystemCursor id) +{ + return Android_WrapCursor(0, id); +} + +static void +Android_FreeCursor(SDL_Cursor * cursor) +{ + SDL_free(cursor->driverdata); + SDL_free(cursor); +} + +static SDL_Cursor * +Android_CreateEmptyCursor() +{ + if (!empty_cursor) { + SDL_Surface *empty_surface = SDL_CreateRGBSurfaceWithFormat(0, 1, 1, 32, SDL_PIXELFORMAT_ARGB8888); + if (empty_surface) { + SDL_memset(empty_surface->pixels, 0, empty_surface->h * empty_surface->pitch); + empty_cursor = Android_CreateCursor(empty_surface, 0, 0); + SDL_FreeSurface(empty_surface); + } + } + return empty_cursor; +} + +static void +Android_DestroyEmptyCursor() +{ + if (empty_cursor) { + Android_FreeCursor(empty_cursor); + empty_cursor = NULL; + } +} + +static int +Android_ShowCursor(SDL_Cursor * cursor) +{ + if (!cursor) { + cursor = Android_CreateEmptyCursor(); + } + if (cursor) { + SDL_AndroidCursorData *data = (SDL_AndroidCursorData*)cursor->driverdata; + if (data->custom_cursor) { + if (!Android_JNI_SetCustomCursor(data->custom_cursor)) { + return SDL_Unsupported(); + } + } else { + if (!Android_JNI_SetSystemCursor(data->system_cursor)) { + return SDL_Unsupported(); + } + } + return 0; + } else { + /* SDL error set inside Android_CreateEmptyCursor() */ + return -1; + } +} + +static int +Android_SetRelativeMouseMode(SDL_bool enabled) +{ + if (!Android_JNI_SupportsRelativeMouse()) { + return SDL_Unsupported(); + } + + if (!Android_JNI_SetRelativeMouseEnabled(enabled)) { + return SDL_Unsupported(); + } + + return 0; +} + void Android_InitMouse(void) { + SDL_Mouse *mouse = SDL_GetMouse(); + + mouse->CreateCursor = Android_CreateCursor; + mouse->CreateSystemCursor = Android_CreateSystemCursor; + mouse->ShowCursor = Android_ShowCursor; + mouse->FreeCursor = Android_FreeCursor; + mouse->SetRelativeMouseMode = Android_SetRelativeMouseMode; + + SDL_SetDefaultCursor(Android_CreateDefaultCursor()); + last_state = 0; } +void +Android_QuitMouse(void) +{ + Android_DestroyEmptyCursor(); +} + /* Translate Android mouse button state to SDL mouse button */ static Uint8 TranslateButton(int state) @@ -71,7 +220,7 @@ TranslateButton(int state) } void -Android_OnMouse(int state, int action, float x, float y) +Android_OnMouse(int state, int action, float x, float y, SDL_bool relative) { int changes; Uint8 button; @@ -85,7 +234,7 @@ Android_OnMouse(int state, int action, float x, float y) changes = state & ~last_state; button = TranslateButton(changes); last_state = state; - SDL_SendMouseMotion(Android_Window, 0, 0, x, y); + SDL_SendMouseMotion(Android_Window, 0, relative, x, y); SDL_SendMouseButton(Android_Window, 0, SDL_PRESSED, button); break; @@ -93,13 +242,13 @@ Android_OnMouse(int state, int action, float x, float y) changes = last_state & ~state; button = TranslateButton(changes); last_state = state; - SDL_SendMouseMotion(Android_Window, 0, 0, x, y); + SDL_SendMouseMotion(Android_Window, 0, relative, x, y); SDL_SendMouseButton(Android_Window, 0, SDL_RELEASED, button); break; case ACTION_MOVE: case ACTION_HOVER_MOVE: - SDL_SendMouseMotion(Android_Window, 0, 0, x, y); + SDL_SendMouseMotion(Android_Window, 0, relative, x, y); break; case ACTION_SCROLL: diff --git a/Engine/lib/sdl/src/video/android/SDL_androidmouse.h b/Engine/lib/sdl/src/video/android/SDL_androidmouse.h index f201fad97..eca9e476a 100644 --- a/Engine/lib/sdl/src/video/android/SDL_androidmouse.h +++ b/Engine/lib/sdl/src/video/android/SDL_androidmouse.h @@ -25,7 +25,8 @@ #include "SDL_androidvideo.h" extern void Android_InitMouse(void); -extern void Android_OnMouse( int button, int action, float x, float y); +extern void Android_OnMouse(int button, int action, float x, float y, SDL_bool relative); +extern void Android_QuitMouse(void); #endif /* SDL_androidmouse_h_ */ diff --git a/Engine/lib/sdl/src/video/android/SDL_androidvideo.c b/Engine/lib/sdl/src/video/android/SDL_androidvideo.c index 357f5cf17..589461afb 100644 --- a/Engine/lib/sdl/src/video/android/SDL_androidvideo.c +++ b/Engine/lib/sdl/src/video/android/SDL_androidvideo.c @@ -60,8 +60,10 @@ int Android_GetDisplayDPI(_THIS, SDL_VideoDisplay * display, float * ddpi, float /* These are filled in with real values in Android_SetScreenResolution on init (before SDL_main()) */ -int Android_ScreenWidth = 0; -int Android_ScreenHeight = 0; +int Android_SurfaceWidth = 0; +int Android_SurfaceHeight = 0; +int Android_DeviceWidth = 0; +int Android_DeviceHeight = 0; Uint32 Android_ScreenFormat = SDL_PIXELFORMAT_UNKNOWN; static int Android_ScreenRate = 0; @@ -176,8 +178,8 @@ Android_VideoInit(_THIS) SDL_DisplayMode mode; mode.format = Android_ScreenFormat; - mode.w = Android_ScreenWidth; - mode.h = Android_ScreenHeight; + mode.w = Android_DeviceWidth; + mode.h = Android_DeviceHeight; mode.refresh_rate = Android_ScreenRate; mode.driverdata = NULL; if (SDL_AddBasicVideoDisplay(&mode) < 0) { @@ -199,6 +201,7 @@ Android_VideoInit(_THIS) void Android_VideoQuit(_THIS) { + Android_QuitMouse(); Android_QuitTouch(); } @@ -209,12 +212,14 @@ Android_GetDisplayDPI(_THIS, SDL_VideoDisplay * display, float * ddpi, float * h } void -Android_SetScreenResolution(int width, int height, Uint32 format, float rate) +Android_SetScreenResolution(int surfaceWidth, int surfaceHeight, int deviceWidth, int deviceHeight, Uint32 format, float rate) { - SDL_VideoDevice* device; - SDL_VideoDisplay *display; - Android_ScreenWidth = width; - Android_ScreenHeight = height; + SDL_VideoDevice* device; + SDL_VideoDisplay *display; + Android_SurfaceWidth = surfaceWidth; + Android_SurfaceHeight = surfaceHeight; + Android_DeviceWidth = deviceWidth; + Android_DeviceHeight = deviceHeight; Android_ScreenFormat = format; Android_ScreenRate = rate; @@ -229,8 +234,8 @@ Android_SetScreenResolution(int width, int height, Uint32 format, float rate) { display = &device->displays[0]; display->desktop_mode.format = Android_ScreenFormat; - display->desktop_mode.w = Android_ScreenWidth; - display->desktop_mode.h = Android_ScreenHeight; + display->desktop_mode.w = Android_DeviceWidth; + display->desktop_mode.h = Android_DeviceHeight; display->desktop_mode.refresh_rate = Android_ScreenRate; } @@ -240,12 +245,12 @@ Android_SetScreenResolution(int width, int height, Uint32 format, float rate) display = SDL_GetDisplayForWindow(Android_Window); display->display_modes[0].format = format; - display->display_modes[0].w = width; - display->display_modes[0].h = height; + display->display_modes[0].w = Android_DeviceWidth; + display->display_modes[0].h = Android_DeviceHeight; display->display_modes[0].refresh_rate = rate; display->current_mode = display->display_modes[0]; - SDL_SendWindowEvent(Android_Window, SDL_WINDOWEVENT_RESIZED, width, height); + SDL_SendWindowEvent(Android_Window, SDL_WINDOWEVENT_RESIZED, surfaceWidth, surfaceHeight); } } diff --git a/Engine/lib/sdl/src/video/android/SDL_androidvideo.h b/Engine/lib/sdl/src/video/android/SDL_androidvideo.h index a62c98383..6dce7ed7d 100644 --- a/Engine/lib/sdl/src/video/android/SDL_androidvideo.h +++ b/Engine/lib/sdl/src/video/android/SDL_androidvideo.h @@ -28,7 +28,7 @@ #include "../SDL_sysvideo.h" /* Called by the JNI layer when the screen changes size or format */ -extern void Android_SetScreenResolution(int width, int height, Uint32 format, float rate); +extern void Android_SetScreenResolution(int surfaceWidth, int surfaceHeight, int deviceWidth, int deviceHeight, Uint32 format, float rate); /* Private display data */ @@ -37,8 +37,10 @@ typedef struct SDL_VideoData SDL_Rect textRect; } SDL_VideoData; -extern int Android_ScreenWidth; -extern int Android_ScreenHeight; +extern int Android_SurfaceWidth; +extern int Android_SurfaceHeight; +extern int Android_DeviceWidth; +extern int Android_DeviceHeight; extern Uint32 Android_ScreenFormat; extern SDL_sem *Android_PauseSem, *Android_ResumeSem; extern SDL_Window *Android_Window; diff --git a/Engine/lib/sdl/src/video/android/SDL_androidwindow.c b/Engine/lib/sdl/src/video/android/SDL_androidwindow.c index f1cbf5861..cf18e67b4 100644 --- a/Engine/lib/sdl/src/video/android/SDL_androidwindow.c +++ b/Engine/lib/sdl/src/video/android/SDL_androidwindow.c @@ -26,6 +26,8 @@ #include "../SDL_sysvideo.h" #include "../../events/SDL_keyboard_c.h" #include "../../events/SDL_mouse_c.h" +#include "../../events/SDL_windowevents_c.h" +#include "../../core/android/SDL_android.h" #include "SDL_androidvideo.h" #include "SDL_androidwindow.h" @@ -49,8 +51,8 @@ Android_CreateWindow(_THIS, SDL_Window * window) /* Adjust the window data to match the screen */ window->x = 0; window->y = 0; - window->w = Android_ScreenWidth; - window->h = Android_ScreenHeight; + window->w = Android_SurfaceWidth; + window->h = Android_SurfaceHeight; window->flags &= ~SDL_WINDOW_RESIZABLE; /* window is NEVER resizeable */ window->flags &= ~SDL_WINDOW_HIDDEN; @@ -100,7 +102,36 @@ Android_SetWindowTitle(_THIS, SDL_Window * window) void Android_SetWindowFullscreen(_THIS, SDL_Window * window, SDL_VideoDisplay * display, SDL_bool fullscreen) { - Android_JNI_SetWindowStyle(fullscreen); + /* If the window is being destroyed don't change visible state */ + if (!window->is_destroying) { + Android_JNI_SetWindowStyle(fullscreen); + } + + /* Ensure our size matches reality after we've executed the window style change. + * + * It is possible that we've set width and height to the full-size display, but on + * Samsung DeX or Chromebooks or other windowed Android environemtns, our window may + * still not be the full display size. + */ + if (!SDL_IsDeXMode() && !SDL_IsChromebook()) { + return; + } + + SDL_WindowData * data = (SDL_WindowData *)window->driverdata; + + if (!data || !data->native_window) { + return; + } + + int old_w = window->w; + int old_h = window->h; + + int new_w = ANativeWindow_getWidth(data->native_window); + int new_h = ANativeWindow_getHeight(data->native_window); + + if (old_w != new_w || old_h != new_h) { + SDL_SendWindowEvent(window, SDL_WINDOWEVENT_RESIZED, new_w, new_h); + } } void diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoaevents.m b/Engine/lib/sdl/src/video/cocoa/SDL_cocoaevents.m index 38f4ba676..76d235e3b 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoaevents.m +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoaevents.m @@ -390,8 +390,8 @@ Cocoa_RegisterApp(void) if (!SDL_GetHintBoolean(SDL_HINT_MAC_BACKGROUND_APP, SDL_FALSE)) { [NSApp setActivationPolicy:NSApplicationActivationPolicyRegular]; - } - + } + if ([NSApp mainMenu] == nil) { CreateApplicationMenus(); } diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoametalview.h b/Engine/lib/sdl/src/video/cocoa/SDL_cocoametalview.h index c0a582ff4..185d45deb 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoametalview.h +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoametalview.h @@ -39,16 +39,16 @@ #define METALVIEW_TAG 255 -@interface SDL_cocoametalview : NSView { - NSInteger _tag; -} +@interface SDL_cocoametalview : NSView - (instancetype)initWithFrame:(NSRect)frame - scale:(CGFloat)scale; + highDPI:(BOOL)highDPI; /* Override superclass tag so this class can set it. */ @property (assign, readonly) NSInteger tag; +@property (nonatomic) BOOL highDPI; + @end SDL_cocoametalview* Cocoa_Mtl_AddMetalView(SDL_Window* window); diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoametalview.m b/Engine/lib/sdl/src/video/cocoa/SDL_cocoametalview.m index e9c08a007..9447fb8c6 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoametalview.m +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoametalview.m @@ -33,13 +33,10 @@ @implementation SDL_cocoametalview -/* The synthesized getter should be called by super's viewWithTag. */ -@synthesize tag = _tag; - /* Return a Metal-compatible layer. */ + (Class)layerClass { - return NSClassFromString(@"CAMetalLayer"); + return NSClassFromString(@"CAMetalLayer"); } /* Indicate the view wants to draw using a backing layer instead of drawRect. */ @@ -57,28 +54,48 @@ } - (instancetype)initWithFrame:(NSRect)frame - scale:(CGFloat)scale + highDPI:(BOOL)highDPI { - if ((self = [super initWithFrame:frame])) { - _tag = METALVIEW_TAG; + if ((self = [super initWithFrame:frame])) { + self.highDPI = highDPI; self.wantsLayer = YES; /* Allow resize. */ self.autoresizingMask = NSViewWidthSizable | NSViewHeightSizable; - /* Set the desired scale. The default drawableSize of a CAMetalLayer - * is its bounds x its scale so nothing further needs to be done. - */ - self.layer.contentsScale = scale; - } + [self updateDrawableSize]; + } - return self; + return self; +} + +- (NSInteger)tag +{ + return METALVIEW_TAG; +} + +- (void)updateDrawableSize +{ + CAMetalLayer *metalLayer = (CAMetalLayer *)self.layer; + CGSize size = self.bounds.size; + CGSize backingSize = size; + + if (self.highDPI) { + /* Note: NSHighResolutionCapable must be set to true in the app's + * Info.plist in order for the backing size to be high res. + */ + backingSize = [self convertSizeToBacking:size]; + } + + metalLayer.contentsScale = backingSize.height / size.height; + metalLayer.drawableSize = backingSize; } /* Set the size of the metal drawables when the view is resized. */ - (void)resizeWithOldSuperviewSize:(NSSize)oldSize { [super resizeWithOldSuperviewSize:oldSize]; + [self updateDrawableSize]; } @end @@ -88,24 +105,10 @@ Cocoa_Mtl_AddMetalView(SDL_Window* window) { SDL_WindowData* data = (__bridge SDL_WindowData *)window->driverdata; NSView *view = data->nswindow.contentView; - CGFloat scale = 1.0; + BOOL highDPI = (window->flags & SDL_WINDOW_ALLOW_HIGHDPI) != 0; + SDL_cocoametalview *metalview; - if (window->flags & SDL_WINDOW_ALLOW_HIGHDPI) { - /* Set the scale to the natural scale factor of the screen - then - * the backing dimensions of the Metal view will match the pixel - * dimensions of the screen rather than the dimensions in points - * yielding high resolution on retine displays. - * - * N.B. In order for backingScaleFactor to be > 1, - * NSHighResolutionCapable must be set to true in the app's Info.plist. - */ - NSWindow* nswindow = data->nswindow; - if ([nswindow.screen respondsToSelector:@selector(backingScaleFactor)]) - scale = data->nswindow.screen.backingScaleFactor; - } - - SDL_cocoametalview *metalview - = [[SDL_cocoametalview alloc] initWithFrame:view.frame scale:scale]; + metalview = [[SDL_cocoametalview alloc] initWithFrame:view.frame highDPI:highDPI]; [view addSubview:metalview]; return metalview; } diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoamouse.m b/Engine/lib/sdl/src/video/cocoa/SDL_cocoamouse.m index 029a31832..c9db25394 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoamouse.m +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoamouse.m @@ -432,6 +432,16 @@ Cocoa_HandleMouseWheel(SDL_Window *window, NSEvent *event) } } + if (x > 0) { + x = SDL_ceil(x); + } else if (x < 0) { + x = SDL_floor(x); + } + if (y > 0) { + y = SDL_ceil(y); + } else if (y < 0) { + y = SDL_floor(y); + } SDL_SendMouseWheel(window, mouse->mouseID, x, y, direction); } diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoamousetap.m b/Engine/lib/sdl/src/video/cocoa/SDL_cocoamousetap.m index 3c4fcf23e..aa4f152e6 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoamousetap.m +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoamousetap.m @@ -211,7 +211,7 @@ Cocoa_InitMouseEventTap(SDL_MouseData* driverdata) tapdata->thread = SDL_CreateThreadInternal(&Cocoa_MouseTapThread, "Event Tap Loop", 512 * 1024, tapdata); if (tapdata->thread) { /* Success - early out. Ownership transferred to thread. */ - return; + return; } CFRelease(tapdata->tap); } @@ -237,6 +237,13 @@ Cocoa_QuitMouseEventTap(SDL_MouseData *driverdata) SDL_MouseEventTapData *tapdata = (SDL_MouseEventTapData*)driverdata->tapdata; int status; + if (tapdata == NULL) { + /* event tap was already cleaned up (possibly due to CGEventTapCreate + * returning null.) + */ + return; + } + /* Ensure that the runloop has been started first. * TODO: Move this to InitMouseEventTap, check for error conditions that can * happen in Cocoa_MouseTapThread, and fall back to the non-EventTap way of diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoaopengl.m b/Engine/lib/sdl/src/video/cocoa/SDL_cocoaopengl.m index 5f18a2ee8..9539c1779 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoaopengl.m +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoaopengl.m @@ -347,10 +347,12 @@ Cocoa_GL_GetDrawableSize(_THIS, SDL_Window * window, int * w, int * h) NSView *contentView = [windata->nswindow contentView]; NSRect viewport = [contentView bounds]; - /* This gives us the correct viewport for a Retina-enabled view, only - * supported on 10.7+. */ - if ([contentView respondsToSelector:@selector(convertRectToBacking:)]) { - viewport = [contentView convertRectToBacking:viewport]; + if (window->flags & SDL_WINDOW_ALLOW_HIGHDPI) { + /* This gives us the correct viewport for a Retina-enabled view, only + * supported on 10.7+. */ + if ([contentView respondsToSelector:@selector(convertRectToBacking:)]) { + viewport = [contentView convertRectToBacking:viewport]; + } } if (w) { @@ -408,8 +410,14 @@ Cocoa_GL_SwapWindow(_THIS, SDL_Window * window) { @autoreleasepool { SDLOpenGLContext* nscontext = (SDLOpenGLContext*)SDL_GL_GetCurrentContext(); + SDL_VideoData *videodata = (SDL_VideoData *) _this->driverdata; + + /* on 10.14 ("Mojave") and later, this deadlocks if two contexts in two + threads try to swap at the same time, so put a mutex around it. */ + SDL_LockMutex(videodata->swaplock); [nscontext flushBuffer]; [nscontext updateIfNeeded]; + SDL_UnlockMutex(videodata->swaplock); return 0; }} diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoavideo.h b/Engine/lib/sdl/src/video/cocoa/SDL_cocoavideo.h index 05bbd3483..b1c26fa7e 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoavideo.h +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoavideo.h @@ -107,7 +107,7 @@ typedef struct SDL_VideoData Uint32 screensaver_activity; BOOL screensaver_use_iopm; IOPMAssertionID screensaver_assertion; - + SDL_mutex *swaplock; } SDL_VideoData; /* Utility functions */ diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoavideo.m b/Engine/lib/sdl/src/video/cocoa/SDL_cocoavideo.m index 545dc1ed9..20bdfa791 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoavideo.m +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoavideo.m @@ -105,6 +105,7 @@ Cocoa_CreateDevice(int devindex) device->DestroyWindow = Cocoa_DestroyWindow; device->GetWindowWMInfo = Cocoa_GetWindowWMInfo; device->SetWindowHitTest = Cocoa_SetWindowHitTest; + device->AcceptDragAndDrop = Cocoa_AcceptDragAndDrop; device->shape_driver.CreateShaper = Cocoa_CreateShaper; device->shape_driver.SetWindowShape = Cocoa_SetWindowShape; @@ -174,15 +175,23 @@ Cocoa_VideoInit(_THIS) /* The IOPM assertion API can disable the screensaver as of 10.7. */ data->screensaver_use_iopm = floor(NSAppKitVersionNumber) > NSAppKitVersionNumber10_6; + data->swaplock = SDL_CreateMutex(); + if (!data->swaplock) { + return -1; + } + return 0; } void Cocoa_VideoQuit(_THIS) { + SDL_VideoData *data = (SDL_VideoData *) _this->driverdata; Cocoa_QuitModes(_this); Cocoa_QuitKeyboard(_this); Cocoa_QuitMouse(_this); + SDL_DestroyMutex(data->swaplock); + data->swaplock = NULL; } /* This function assumes that it's called from within an autorelease pool */ diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoavulkan.m b/Engine/lib/sdl/src/video/cocoa/SDL_cocoavulkan.m index 2cf55bb5a..0e53d219d 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoavulkan.m +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoavulkan.m @@ -58,8 +58,7 @@ int Cocoa_Vulkan_LoadLibrary(_THIS, const char *path) PFN_vkGetInstanceProcAddr vkGetInstanceProcAddr = NULL; if (_this->vulkan_config.loader_handle) { - SDL_SetError("Vulkan/MoltenVK already loaded"); - return -1; + return SDL_SetError("Vulkan/MoltenVK already loaded"); } /* Load the Vulkan loader library */ @@ -80,6 +79,7 @@ int Cocoa_Vulkan_LoadLibrary(_THIS, const char *path) _this->vulkan_config.loader_handle = DEFAULT_HANDLE; } else { const char** paths; + const char *foundPath = NULL; int numPaths; int i; @@ -92,18 +92,17 @@ int Cocoa_Vulkan_LoadLibrary(_THIS, const char *path) paths = defaultPaths; numPaths = SDL_arraysize(defaultPaths); } - - for (i=0; i < numPaths; i++) { - _this->vulkan_config.loader_handle = SDL_LoadObject(paths[i]); - if (_this->vulkan_config.loader_handle) - break; - else - continue; - } - if (i == numPaths) - return -1; - SDL_strlcpy(_this->vulkan_config.loader_path, paths[i], + for (i = 0; i < numPaths && _this->vulkan_config.loader_handle == NULL; i++) { + foundPath = paths[i]; + _this->vulkan_config.loader_handle = SDL_LoadObject(foundPath); + } + + if (_this->vulkan_config.loader_handle == NULL) { + return SDL_SetError("Failed to load Vulkan/MoltenVK library"); + } + + SDL_strlcpy(_this->vulkan_config.loader_path, foundPath, SDL_arraysize(_this->vulkan_config.loader_path)); vkGetInstanceProcAddr = (PFN_vkGetInstanceProcAddr)SDL_LoadFunction( _this->vulkan_config.loader_handle, "vkGetInstanceProcAddr"); diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoawindow.h b/Engine/lib/sdl/src/video/cocoa/SDL_cocoawindow.h index df6f173a7..2311e3d74 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoawindow.h +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoawindow.h @@ -148,6 +148,7 @@ extern void Cocoa_SetWindowGrab(_THIS, SDL_Window * window, SDL_bool grabbed); extern void Cocoa_DestroyWindow(_THIS, SDL_Window * window); extern SDL_bool Cocoa_GetWindowWMInfo(_THIS, SDL_Window * window, struct SDL_SysWMinfo *info); extern int Cocoa_SetWindowHitTest(SDL_Window *window, SDL_bool enabled); +extern void Cocoa_AcceptDragAndDrop(SDL_Window * window, SDL_bool accept); #endif /* SDL_cocoawindow_h_ */ diff --git a/Engine/lib/sdl/src/video/cocoa/SDL_cocoawindow.m b/Engine/lib/sdl/src/video/cocoa/SDL_cocoawindow.m index b1a5b465d..a8e95ccd9 100644 --- a/Engine/lib/sdl/src/video/cocoa/SDL_cocoawindow.m +++ b/Engine/lib/sdl/src/video/cocoa/SDL_cocoawindow.m @@ -241,7 +241,7 @@ ScheduleContextUpdates(SDL_WindowData *data) static int GetHintCtrlClickEmulateRightClick() { - return SDL_GetHintBoolean(SDL_HINT_MAC_CTRL_CLICK_EMULATE_RIGHT_CLICK, SDL_FALSE); + return SDL_GetHintBoolean(SDL_HINT_MAC_CTRL_CLICK_EMULATE_RIGHT_CLICK, SDL_FALSE); } static NSUInteger @@ -908,7 +908,7 @@ SetWindowStyle(SDL_Window * window, NSUInteger style) switch ([theEvent buttonNumber]) { case 0: if (([theEvent modifierFlags] & NSEventModifierFlagControl) && - GetHintCtrlClickEmulateRightClick()) { + GetHintCtrlClickEmulateRightClick()) { wasCtrlLeft = YES; button = SDL_BUTTON_RIGHT; } else { @@ -1143,14 +1143,18 @@ SetWindowStyle(SDL_Window * window, NSUInteger style) - (BOOL)mouseDownCanMoveWindow; - (void)drawRect:(NSRect)dirtyRect; - (BOOL)acceptsFirstMouse:(NSEvent *)theEvent; +- (BOOL)wantsUpdateLayer; +- (void)updateLayer; @end @implementation SDLView + - (void)setSDLWindow:(SDL_Window*)window { _sdlWindow = window; } +/* this is used on older macOS revisions. 10.8 and later use updateLayer. */ - (void)drawRect:(NSRect)dirtyRect { /* Force the graphics context to clear to black so we don't get a flash of @@ -1161,6 +1165,21 @@ SetWindowStyle(SDL_Window * window, NSUInteger style) SDL_SendWindowEvent(_sdlWindow, SDL_WINDOWEVENT_EXPOSED, 0, 0); } +-(BOOL) wantsUpdateLayer +{ + return YES; +} + +-(void) updateLayer +{ + /* Force the graphics context to clear to black so we don't get a flash of + white until the app is ready to draw. In practice on modern macOS, this + only gets called for window creation and other extraordinary events. */ + self.layer.backgroundColor = NSColor.blackColor.CGColor; + ScheduleContextUpdates((SDL_WindowData *) _sdlWindow->driverdata); + SDL_SendWindowEvent(_sdlWindow, SDL_WINDOWEVENT_EXPOSED, 0, 0); +} + - (void)rightMouseDown:(NSEvent *)theEvent { [[self nextResponder] rightMouseDown:theEvent]; @@ -1343,6 +1362,7 @@ Cocoa_CreateWindow(_THIS, SDL_Window * window) [contentView setWantsBestResolutionOpenGLSurface:YES]; } } + #if SDL_VIDEO_OPENGL_ES2 #if SDL_VIDEO_OPENGL_EGL if ((window->flags & SDL_WINDOW_OPENGL) && @@ -1354,9 +1374,6 @@ Cocoa_CreateWindow(_THIS, SDL_Window * window) [nswindow setContentView:contentView]; [contentView release]; - /* Allow files and folders to be dragged onto the window by users */ - [nswindow registerForDraggedTypes:[NSArray arrayWithObject:(NSString *)kUTTypeFileURL]]; - if (SetupWindowData(_this, window, nswindow, SDL_TRUE) < 0) { [nswindow release]; return -1; @@ -1864,6 +1881,17 @@ Cocoa_SetWindowHitTest(SDL_Window * window, SDL_bool enabled) return 0; /* just succeed, the real work is done elsewhere. */ } +void +Cocoa_AcceptDragAndDrop(SDL_Window * window, SDL_bool accept) +{ + SDL_WindowData *data = (SDL_WindowData *) window->driverdata; + if (accept) { + [data->nswindow registerForDraggedTypes:[NSArray arrayWithObject:(NSString *)kUTTypeFileURL]]; + } else { + [data->nswindow unregisterDraggedTypes]; + } +} + int Cocoa_SetWindowOpacity(_THIS, SDL_Window * window, float opacity) { diff --git a/Engine/lib/sdl/src/video/directfb/SDL_DirectFB_video.c b/Engine/lib/sdl/src/video/directfb/SDL_DirectFB_video.c index 45fa81dc1..8740ce111 100644 --- a/Engine/lib/sdl/src/video/directfb/SDL_DirectFB_video.c +++ b/Engine/lib/sdl/src/video/directfb/SDL_DirectFB_video.c @@ -324,6 +324,7 @@ static const struct { { DSPF_YUY2, SDL_PIXELFORMAT_YUY2 }, /* 16 bit YUV (4 byte/ 2 pixel, macropixel contains CbYCrY [31:0]) */ { DSPF_UYVY, SDL_PIXELFORMAT_UYVY }, /* 16 bit YUV (4 byte/ 2 pixel, macropixel contains YCbYCr [31:0]) */ { DSPF_RGB555, SDL_PIXELFORMAT_RGB555 }, /* 16 bit RGB (2 byte, nothing @15, red 5@10, green 5@5, blue 5@0) */ + { DSPF_ABGR, SDL_PIXELFORMAT_ABGR8888 }, /* 32 bit ABGR (4 byte, alpha 8@24, blue 8@16, green 8@8, red 8@0) */ #if (ENABLE_LUT8) { DSPF_LUT8, SDL_PIXELFORMAT_INDEX8 }, /* 8 bit LUT (8 bit color and alpha lookup from palette) */ #endif @@ -370,7 +371,6 @@ static const struct { { DSPF_UNKNOWN, SDL_PIXELFORMAT_BGR24 }, { DSPF_UNKNOWN, SDL_PIXELFORMAT_BGR888 }, { DSPF_UNKNOWN, SDL_PIXELFORMAT_RGBA8888 }, - { DSPF_UNKNOWN, SDL_PIXELFORMAT_ABGR8888 }, { DSPF_UNKNOWN, SDL_PIXELFORMAT_BGRA8888 }, { DSPF_UNKNOWN, SDL_PIXELFORMAT_ARGB2101010 }, { DSPF_UNKNOWN, SDL_PIXELFORMAT_ABGR4444 }, diff --git a/Engine/lib/sdl/src/video/dummy/SDL_nullevents_c.h b/Engine/lib/sdl/src/video/dummy/SDL_nullevents_c.h index a5636be53..454d39401 100644 --- a/Engine/lib/sdl/src/video/dummy/SDL_nullevents_c.h +++ b/Engine/lib/sdl/src/video/dummy/SDL_nullevents_c.h @@ -18,10 +18,16 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_nullevents_c_h_ +#define SDL_nullevents_c_h_ + #include "../../SDL_internal.h" #include "SDL_nullvideo.h" extern void DUMMY_PumpEvents(_THIS); +#endif /* SDL_nullevents_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/dummy/SDL_nullframebuffer_c.h b/Engine/lib/sdl/src/video/dummy/SDL_nullframebuffer_c.h index 5d6b7aed5..b7d0c633f 100644 --- a/Engine/lib/sdl/src/video/dummy/SDL_nullframebuffer_c.h +++ b/Engine/lib/sdl/src/video/dummy/SDL_nullframebuffer_c.h @@ -18,10 +18,16 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_nullframebuffer_c_h_ +#define SDL_nullframebuffer_c_h_ + #include "../../SDL_internal.h" extern int SDL_DUMMY_CreateWindowFramebuffer(_THIS, SDL_Window * window, Uint32 * format, void ** pixels, int *pitch); extern int SDL_DUMMY_UpdateWindowFramebuffer(_THIS, SDL_Window * window, const SDL_Rect * rects, int numrects); extern void SDL_DUMMY_DestroyWindowFramebuffer(_THIS, SDL_Window * window); +#endif /* SDL_nullframebuffer_c_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/emscripten/SDL_emscriptenmouse.c b/Engine/lib/sdl/src/video/emscripten/SDL_emscriptenmouse.c index 490f5b05d..e120980a3 100644 --- a/Engine/lib/sdl/src/video/emscripten/SDL_emscriptenmouse.c +++ b/Engine/lib/sdl/src/video/emscripten/SDL_emscriptenmouse.c @@ -165,6 +165,7 @@ Emscripten_CreateSystemCursor(SDL_SystemCursor id) cursor_name = "ns-resize"; break; case SDL_SYSTEM_CURSOR_SIZEALL: + cursor_name = "move"; break; case SDL_SYSTEM_CURSOR_NO: cursor_name = "not-allowed"; diff --git a/Engine/lib/sdl/src/video/emscripten/SDL_emscriptenopengles.c b/Engine/lib/sdl/src/video/emscripten/SDL_emscriptenopengles.c index a6609edc0..7d8c00575 100644 --- a/Engine/lib/sdl/src/video/emscripten/SDL_emscriptenopengles.c +++ b/Engine/lib/sdl/src/video/emscripten/SDL_emscriptenopengles.c @@ -60,7 +60,9 @@ Emscripten_GLES_LoadLibrary(_THIS, const char *path) { LOAD_FUNC(eglWaitNative); LOAD_FUNC(eglWaitGL); LOAD_FUNC(eglBindAPI); - + LOAD_FUNC(eglQueryString); + LOAD_FUNC(eglGetError); + _this->egl_data->egl_display = _this->egl_data->eglGetDisplay(EGL_DEFAULT_DISPLAY); if (!_this->egl_data->egl_display) { return SDL_SetError("Could not get EGL display"); diff --git a/Engine/lib/sdl/src/video/haiku/SDL_BWin.h b/Engine/lib/sdl/src/video/haiku/SDL_BWin.h index 3e6188860..b22f74b25 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_BWin.h +++ b/Engine/lib/sdl/src/video/haiku/SDL_BWin.h @@ -88,7 +88,7 @@ class SDL_BWin:public BDirectWindow _clips = NULL; #ifdef DRAWTHREAD - _draw_thread_id = spawn_thread(BE_DrawThread, "drawing_thread", + _draw_thread_id = spawn_thread(HAIKU_DrawThread, "drawing_thread", B_NORMAL_PRIORITY, (void*) this); resume_thread(_draw_thread_id); #endif @@ -538,7 +538,7 @@ private: msg.AddInt32("key-state", keyState); msg.AddInt32("key-scancode", keyCode); if (keyUtf8 != NULL) { - msg.AddData("key-utf8", B_INT8_TYPE, (const void*)keyUtf8, len); + msg.AddData("key-utf8", B_INT8_TYPE, (const void*)keyUtf8, len); } be_app->PostMessage(&msg); } diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bclipboard.cc b/Engine/lib/sdl/src/video/haiku/SDL_bclipboard.cc index e7d01b9be..3138603d8 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bclipboard.cc +++ b/Engine/lib/sdl/src/video/haiku/SDL_bclipboard.cc @@ -35,55 +35,55 @@ extern "C" { #endif -int BE_SetClipboardText(_THIS, const char *text) { - BMessage *clip = NULL; - if(be_clipboard->Lock()) { - be_clipboard->Clear(); - if((clip = be_clipboard->Data())) { - /* Presumably the string of characters is ascii-format */ - ssize_t asciiLength = 0; - for(; text[asciiLength] != 0; ++asciiLength) {} - clip->AddData("text/plain", B_MIME_TYPE, text, asciiLength); - be_clipboard->Commit(); - } - be_clipboard->Unlock(); - } - return 0; +int HAIKU_SetClipboardText(_THIS, const char *text) { + BMessage *clip = NULL; + if(be_clipboard->Lock()) { + be_clipboard->Clear(); + if((clip = be_clipboard->Data())) { + /* Presumably the string of characters is ascii-format */ + ssize_t asciiLength = 0; + for(; text[asciiLength] != 0; ++asciiLength) {} + clip->AddData("text/plain", B_MIME_TYPE, text, asciiLength); + be_clipboard->Commit(); + } + be_clipboard->Unlock(); + } + return 0; } -char *BE_GetClipboardText(_THIS) { - BMessage *clip = NULL; - const char *text = NULL; - ssize_t length; - char *result; - if(be_clipboard->Lock()) { - if((clip = be_clipboard->Data())) { - /* Presumably the string of characters is ascii-format */ - clip->FindData("text/plain", B_MIME_TYPE, (const void**)&text, - &length); - } - be_clipboard->Unlock(); - } - - if (!text) { - result = SDL_strdup(""); - } else { - /* Copy the data and pass on to SDL */ - result = (char *)SDL_malloc((length + 1) * sizeof(char)); - SDL_strlcpy(result, text, length + 1); - } - - return result; +char *HAIKU_GetClipboardText(_THIS) { + BMessage *clip = NULL; + const char *text = NULL; + ssize_t length; + char *result; + if(be_clipboard->Lock()) { + if((clip = be_clipboard->Data())) { + /* Presumably the string of characters is ascii-format */ + clip->FindData("text/plain", B_MIME_TYPE, (const void**)&text, + &length); + } + be_clipboard->Unlock(); + } + + if (!text) { + result = SDL_strdup(""); + } else { + /* Copy the data and pass on to SDL */ + result = (char *)SDL_malloc((length + 1) * sizeof(char)); + SDL_strlcpy(result, text, length + 1); + } + + return result; } -SDL_bool BE_HasClipboardText(_THIS) { - SDL_bool result = SDL_FALSE; - char *text = BE_GetClipboardText(_this); - if (text) { - result = text[0] != '\0' ? SDL_TRUE : SDL_FALSE; - SDL_free(text); - } - return result; +SDL_bool HAIKU_HasClipboardText(_THIS) { + SDL_bool result = SDL_FALSE; + char *text = HAIKU_GetClipboardText(_this); + if (text) { + result = text[0] != '\0' ? SDL_TRUE : SDL_FALSE; + SDL_free(text); + } + return result; } #ifdef __cplusplus diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bclipboard.h b/Engine/lib/sdl/src/video/haiku/SDL_bclipboard.h index 2f7a1c243..de69ed39d 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bclipboard.h +++ b/Engine/lib/sdl/src/video/haiku/SDL_bclipboard.h @@ -24,9 +24,9 @@ #ifndef SDL_BCLIPBOARD_H #define SDL_BCLIPBOARD_H -extern int BE_SetClipboardText(_THIS, const char *text); -extern char *BE_GetClipboardText(_THIS); -extern SDL_bool BE_HasClipboardText(_THIS); +extern int HAIKU_SetClipboardText(_THIS, const char *text); +extern char *HAIKU_GetClipboardText(_THIS); +extern SDL_bool HAIKU_HasClipboardText(_THIS); #endif diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bevents.cc b/Engine/lib/sdl/src/video/haiku/SDL_bevents.cc index c716731a4..c918ab243 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bevents.cc +++ b/Engine/lib/sdl/src/video/haiku/SDL_bevents.cc @@ -28,8 +28,8 @@ extern "C" { #endif -void BE_PumpEvents(_THIS) { - /* Since the event thread is its own thread, this isn't really necessary */ +void HAIKU_PumpEvents(_THIS) { + /* Since the event thread is its own thread, this isn't really necessary */ } #ifdef __cplusplus diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bevents.h b/Engine/lib/sdl/src/video/haiku/SDL_bevents.h index 3c090c89e..5c34fcf14 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bevents.h +++ b/Engine/lib/sdl/src/video/haiku/SDL_bevents.h @@ -28,7 +28,7 @@ extern "C" { #endif -extern void BE_PumpEvents(_THIS); +extern void HAIKU_PumpEvents(_THIS); #ifdef __cplusplus } diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bframebuffer.cc b/Engine/lib/sdl/src/video/haiku/SDL_bframebuffer.cc index f53c50019..967570612 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bframebuffer.cc +++ b/Engine/lib/sdl/src/video/haiku/SDL_bframebuffer.cc @@ -36,162 +36,162 @@ extern "C" { #endif #ifndef DRAWTHREAD -static int32 BE_UpdateOnce(SDL_Window *window); +static int32 HAIKU_UpdateOnce(SDL_Window *window); #endif static SDL_INLINE SDL_BWin *_ToBeWin(SDL_Window *window) { - return ((SDL_BWin*)(window->driverdata)); + return ((SDL_BWin*)(window->driverdata)); } static SDL_INLINE SDL_BApp *_GetBeApp() { - return ((SDL_BApp*)be_app); + return ((SDL_BApp*)be_app); } -int BE_CreateWindowFramebuffer(_THIS, SDL_Window * window, +int HAIKU_CreateWindowFramebuffer(_THIS, SDL_Window * window, Uint32 * format, void ** pixels, int *pitch) { - SDL_BWin *bwin = _ToBeWin(window); - BScreen bscreen; - if(!bscreen.IsValid()) { - return -1; - } + SDL_BWin *bwin = _ToBeWin(window); + BScreen bscreen; + if(!bscreen.IsValid()) { + return -1; + } - while(!bwin->Connected()) { snooze(100); } - - /* Make sure we have exclusive access to frame buffer data */ - bwin->LockBuffer(); + while(!bwin->Connected()) { snooze(100); } + + /* Make sure we have exclusive access to frame buffer data */ + bwin->LockBuffer(); - /* format */ - display_mode bmode; - bscreen.GetMode(&bmode); - int32 bpp = BE_ColorSpaceToBitsPerPixel(bmode.space); - *format = BE_BPPToSDLPxFormat(bpp); + /* format */ + display_mode bmode; + bscreen.GetMode(&bmode); + int32 bpp = HAIKU_ColorSpaceToBitsPerPixel(bmode.space); + *format = HAIKU_BPPToSDLPxFormat(bpp); - /* Create the new bitmap object */ - BBitmap *bitmap = bwin->GetBitmap(); + /* Create the new bitmap object */ + BBitmap *bitmap = bwin->GetBitmap(); - if(bitmap) { - delete bitmap; - } - bitmap = new BBitmap(bwin->Bounds(), (color_space)bmode.space, - false, /* Views not accepted */ - true); /* Contiguous memory required */ - - if(bitmap->InitCheck() != B_OK) { - delete bitmap; - return SDL_SetError("Could not initialize back buffer!"); - } + if(bitmap) { + delete bitmap; + } + bitmap = new BBitmap(bwin->Bounds(), (color_space)bmode.space, + false, /* Views not accepted */ + true); /* Contiguous memory required */ + + if(bitmap->InitCheck() != B_OK) { + delete bitmap; + return SDL_SetError("Could not initialize back buffer!"); + } - bwin->SetBitmap(bitmap); - - /* Set the pixel pointer */ - *pixels = bitmap->Bits(); + bwin->SetBitmap(bitmap); + + /* Set the pixel pointer */ + *pixels = bitmap->Bits(); - /* pitch = width of window, in bytes */ - *pitch = bitmap->BytesPerRow(); + /* pitch = width of window, in bytes */ + *pitch = bitmap->BytesPerRow(); - bwin->SetBufferExists(true); - bwin->SetTrashBuffer(false); - bwin->UnlockBuffer(); - return 0; + bwin->SetBufferExists(true); + bwin->SetTrashBuffer(false); + bwin->UnlockBuffer(); + return 0; } -int BE_UpdateWindowFramebuffer(_THIS, SDL_Window * window, +int HAIKU_UpdateWindowFramebuffer(_THIS, SDL_Window * window, const SDL_Rect * rects, int numrects) { - if(!window) - return 0; + if(!window) + return 0; - SDL_BWin *bwin = _ToBeWin(window); + SDL_BWin *bwin = _ToBeWin(window); -#ifdef DRAWTHREAD - bwin->LockBuffer(); - bwin->SetBufferDirty(true); - bwin->UnlockBuffer(); +#ifdef DRAWTHREAD + bwin->LockBuffer(); + bwin->SetBufferDirty(true); + bwin->UnlockBuffer(); #else - bwin->SetBufferDirty(true); - BE_UpdateOnce(window); + bwin->SetBufferDirty(true); + HAIKU_UpdateOnce(window); #endif - return 0; + return 0; } -int32 BE_DrawThread(void *data) { - SDL_BWin *bwin = (SDL_BWin*)data; - - BScreen bscreen; - if(!bscreen.IsValid()) { - return -1; - } +int32 HAIKU_DrawThread(void *data) { + SDL_BWin *bwin = (SDL_BWin*)data; + + BScreen bscreen; + if(!bscreen.IsValid()) { + return -1; + } - while(bwin->ConnectionEnabled()) { - if( bwin->Connected() && bwin->BufferExists() && bwin->BufferIsDirty() ) { - bwin->LockBuffer(); - BBitmap *bitmap = NULL; - bitmap = bwin->GetBitmap(); - int32 windowPitch = bitmap->BytesPerRow(); - int32 bufferPitch = bwin->GetRowBytes(); - uint8 *windowpx; - uint8 *bufferpx; + while(bwin->ConnectionEnabled()) { + if( bwin->Connected() && bwin->BufferExists() && bwin->BufferIsDirty() ) { + bwin->LockBuffer(); + BBitmap *bitmap = NULL; + bitmap = bwin->GetBitmap(); + int32 windowPitch = bitmap->BytesPerRow(); + int32 bufferPitch = bwin->GetRowBytes(); + uint8 *windowpx; + uint8 *bufferpx; - int32 BPP = bwin->GetBytesPerPx(); - int32 windowSub = bwin->GetFbX() * BPP + - bwin->GetFbY() * windowPitch; - clipping_rect *clips = bwin->GetClips(); - int32 numClips = bwin->GetNumClips(); - int i, y; + int32 BPP = bwin->GetBytesPerPx(); + int32 windowSub = bwin->GetFbX() * BPP + + bwin->GetFbY() * windowPitch; + clipping_rect *clips = bwin->GetClips(); + int32 numClips = bwin->GetNumClips(); + int i, y; - /* Blit each clipping rectangle */ - bscreen.WaitForRetrace(); - for(i = 0; i < numClips; ++i) { - /* Get addresses of the start of each clipping rectangle */ - int32 width = clips[i].right - clips[i].left + 1; - int32 height = clips[i].bottom - clips[i].top + 1; - bufferpx = bwin->GetBufferPx() + - clips[i].top * bufferPitch + clips[i].left * BPP; - windowpx = (uint8*)bitmap->Bits() + - clips[i].top * windowPitch + clips[i].left * BPP - - windowSub; + /* Blit each clipping rectangle */ + bscreen.WaitForRetrace(); + for(i = 0; i < numClips; ++i) { + /* Get addresses of the start of each clipping rectangle */ + int32 width = clips[i].right - clips[i].left + 1; + int32 height = clips[i].bottom - clips[i].top + 1; + bufferpx = bwin->GetBufferPx() + + clips[i].top * bufferPitch + clips[i].left * BPP; + windowpx = (uint8*)bitmap->Bits() + + clips[i].top * windowPitch + clips[i].left * BPP - + windowSub; - /* Copy each row of pixels from the window buffer into the frame - buffer */ - for(y = 0; y < height; ++y) - { + /* Copy each row of pixels from the window buffer into the frame + buffer */ + for(y = 0; y < height; ++y) + { - if(bwin->CanTrashWindowBuffer()) { - goto escape; /* Break out before the buffer is killed */ - } + if(bwin->CanTrashWindowBuffer()) { + goto escape; /* Break out before the buffer is killed */ + } - memcpy(bufferpx, windowpx, width * BPP); - bufferpx += bufferPitch; - windowpx += windowPitch; - } - } + memcpy(bufferpx, windowpx, width * BPP); + bufferpx += bufferPitch; + windowpx += windowPitch; + } + } - bwin->SetBufferDirty(false); + bwin->SetBufferDirty(false); escape: - bwin->UnlockBuffer(); - } else { - snooze(16000); - } - } - - return B_OK; + bwin->UnlockBuffer(); + } else { + snooze(16000); + } + } + + return B_OK; } -void BE_DestroyWindowFramebuffer(_THIS, SDL_Window * window) { - SDL_BWin *bwin = _ToBeWin(window); - - bwin->LockBuffer(); - - /* Free and clear the window buffer */ - BBitmap *bitmap = bwin->GetBitmap(); - delete bitmap; - bwin->SetBitmap(NULL); - bwin->SetBufferExists(false); - bwin->UnlockBuffer(); +void HAIKU_DestroyWindowFramebuffer(_THIS, SDL_Window * window) { + SDL_BWin *bwin = _ToBeWin(window); + + bwin->LockBuffer(); + + /* Free and clear the window buffer */ + BBitmap *bitmap = bwin->GetBitmap(); + delete bitmap; + bwin->SetBitmap(NULL); + bwin->SetBufferExists(false); + bwin->UnlockBuffer(); } @@ -202,51 +202,51 @@ void BE_DestroyWindowFramebuffer(_THIS, SDL_Window * window) { * solved, but I doubt it- they were pretty sporadic before now. */ #ifndef DRAWTHREAD -static int32 BE_UpdateOnce(SDL_Window *window) { - SDL_BWin *bwin = _ToBeWin(window); - BScreen bscreen; - if(!bscreen.IsValid()) { - return -1; - } +static int32 HAIKU_UpdateOnce(SDL_Window *window) { + SDL_BWin *bwin = _ToBeWin(window); + BScreen bscreen; + if(!bscreen.IsValid()) { + return -1; + } - if(bwin->ConnectionEnabled() && bwin->Connected()) { - bwin->LockBuffer(); - int32 windowPitch = window->surface->pitch; - int32 bufferPitch = bwin->GetRowBytes(); - uint8 *windowpx; - uint8 *bufferpx; + if(bwin->ConnectionEnabled() && bwin->Connected()) { + bwin->LockBuffer(); + int32 windowPitch = window->surface->pitch; + int32 bufferPitch = bwin->GetRowBytes(); + uint8 *windowpx; + uint8 *bufferpx; - int32 BPP = bwin->GetBytesPerPx(); - uint8 *windowBaseAddress = (uint8*)window->surface->pixels; - int32 windowSub = bwin->GetFbX() * BPP + - bwin->GetFbY() * windowPitch; - clipping_rect *clips = bwin->GetClips(); - int32 numClips = bwin->GetNumClips(); - int i, y; + int32 BPP = bwin->GetBytesPerPx(); + uint8 *windowBaseAddress = (uint8*)window->surface->pixels; + int32 windowSub = bwin->GetFbX() * BPP + + bwin->GetFbY() * windowPitch; + clipping_rect *clips = bwin->GetClips(); + int32 numClips = bwin->GetNumClips(); + int i, y; - /* Blit each clipping rectangle */ - bscreen.WaitForRetrace(); - for(i = 0; i < numClips; ++i) { - /* Get addresses of the start of each clipping rectangle */ - int32 width = clips[i].right - clips[i].left + 1; - int32 height = clips[i].bottom - clips[i].top + 1; - bufferpx = bwin->GetBufferPx() + - clips[i].top * bufferPitch + clips[i].left * BPP; - windowpx = windowBaseAddress + - clips[i].top * windowPitch + clips[i].left * BPP - windowSub; + /* Blit each clipping rectangle */ + bscreen.WaitForRetrace(); + for(i = 0; i < numClips; ++i) { + /* Get addresses of the start of each clipping rectangle */ + int32 width = clips[i].right - clips[i].left + 1; + int32 height = clips[i].bottom - clips[i].top + 1; + bufferpx = bwin->GetBufferPx() + + clips[i].top * bufferPitch + clips[i].left * BPP; + windowpx = windowBaseAddress + + clips[i].top * windowPitch + clips[i].left * BPP - windowSub; - /* Copy each row of pixels from the window buffer into the frame - buffer */ - for(y = 0; y < height; ++y) - { - memcpy(bufferpx, windowpx, width * BPP); - bufferpx += bufferPitch; - windowpx += windowPitch; - } - } - bwin->UnlockBuffer(); - } - return 0; + /* Copy each row of pixels from the window buffer into the frame + buffer */ + for(y = 0; y < height; ++y) + { + memcpy(bufferpx, windowpx, width * BPP); + bufferpx += bufferPitch; + windowpx += windowPitch; + } + } + bwin->UnlockBuffer(); + } + return 0; } #endif diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bframebuffer.h b/Engine/lib/sdl/src/video/haiku/SDL_bframebuffer.h index ce0fc6284..e48156d13 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bframebuffer.h +++ b/Engine/lib/sdl/src/video/haiku/SDL_bframebuffer.h @@ -30,13 +30,13 @@ extern "C" { #include "../SDL_sysvideo.h" -extern int BE_CreateWindowFramebuffer(_THIS, SDL_Window * window, +extern int HAIKU_CreateWindowFramebuffer(_THIS, SDL_Window * window, Uint32 * format, void ** pixels, int *pitch); -extern int BE_UpdateWindowFramebuffer(_THIS, SDL_Window * window, +extern int HAIKU_UpdateWindowFramebuffer(_THIS, SDL_Window * window, const SDL_Rect * rects, int numrects); -extern void BE_DestroyWindowFramebuffer(_THIS, SDL_Window * window); -extern int32 BE_DrawThread(void *data); +extern void HAIKU_DestroyWindowFramebuffer(_THIS, SDL_Window * window); +extern int32 HAIKU_DrawThread(void *data); #ifdef __cplusplus } diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bkeyboard.cc b/Engine/lib/sdl/src/video/haiku/SDL_bkeyboard.cc index 5c72ecf98..9a8b9a4a9 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bkeyboard.cc +++ b/Engine/lib/sdl/src/video/haiku/SDL_bkeyboard.cc @@ -41,144 +41,144 @@ extern "C" { static SDL_Scancode keymap[KEYMAP_SIZE]; static int8 keystate[KEYMAP_SIZE]; -void BE_InitOSKeymap(void) { - for( uint i = 0; i < SDL_TABLESIZE(keymap); ++i ) { - keymap[i] = SDL_SCANCODE_UNKNOWN; - } +void HAIKU_InitOSKeymap(void) { + for( uint i = 0; i < SDL_TABLESIZE(keymap); ++i ) { + keymap[i] = SDL_SCANCODE_UNKNOWN; + } - for( uint i = 0; i < KEYMAP_SIZE; ++i ) { - keystate[i] = SDL_RELEASED; - } + for( uint i = 0; i < KEYMAP_SIZE; ++i ) { + keystate[i] = SDL_RELEASED; + } - keymap[0x01] = SDL_GetScancodeFromKey(SDLK_ESCAPE); - keymap[B_F1_KEY] = SDL_GetScancodeFromKey(SDLK_F1); - keymap[B_F2_KEY] = SDL_GetScancodeFromKey(SDLK_F2); - keymap[B_F3_KEY] = SDL_GetScancodeFromKey(SDLK_F3); - keymap[B_F4_KEY] = SDL_GetScancodeFromKey(SDLK_F4); - keymap[B_F5_KEY] = SDL_GetScancodeFromKey(SDLK_F5); - keymap[B_F6_KEY] = SDL_GetScancodeFromKey(SDLK_F6); - keymap[B_F7_KEY] = SDL_GetScancodeFromKey(SDLK_F7); - keymap[B_F8_KEY] = SDL_GetScancodeFromKey(SDLK_F8); - keymap[B_F9_KEY] = SDL_GetScancodeFromKey(SDLK_F9); - keymap[B_F10_KEY] = SDL_GetScancodeFromKey(SDLK_F10); - keymap[B_F11_KEY] = SDL_GetScancodeFromKey(SDLK_F11); - keymap[B_F12_KEY] = SDL_GetScancodeFromKey(SDLK_F12); - keymap[B_PRINT_KEY] = SDL_GetScancodeFromKey(SDLK_PRINTSCREEN); - keymap[B_SCROLL_KEY] = SDL_GetScancodeFromKey(SDLK_SCROLLLOCK); - keymap[B_PAUSE_KEY] = SDL_GetScancodeFromKey(SDLK_PAUSE); - keymap[0x11] = SDL_GetScancodeFromKey(SDLK_BACKQUOTE); - keymap[0x12] = SDL_GetScancodeFromKey(SDLK_1); - keymap[0x13] = SDL_GetScancodeFromKey(SDLK_2); - keymap[0x14] = SDL_GetScancodeFromKey(SDLK_3); - keymap[0x15] = SDL_GetScancodeFromKey(SDLK_4); - keymap[0x16] = SDL_GetScancodeFromKey(SDLK_5); - keymap[0x17] = SDL_GetScancodeFromKey(SDLK_6); - keymap[0x18] = SDL_GetScancodeFromKey(SDLK_7); - keymap[0x19] = SDL_GetScancodeFromKey(SDLK_8); - keymap[0x1a] = SDL_GetScancodeFromKey(SDLK_9); - keymap[0x1b] = SDL_GetScancodeFromKey(SDLK_0); - keymap[0x1c] = SDL_GetScancodeFromKey(SDLK_MINUS); - keymap[0x1d] = SDL_GetScancodeFromKey(SDLK_EQUALS); - keymap[0x1e] = SDL_GetScancodeFromKey(SDLK_BACKSPACE); - keymap[0x1f] = SDL_GetScancodeFromKey(SDLK_INSERT); - keymap[0x20] = SDL_GetScancodeFromKey(SDLK_HOME); - keymap[0x21] = SDL_GetScancodeFromKey(SDLK_PAGEUP); - keymap[0x22] = SDL_GetScancodeFromKey(SDLK_NUMLOCKCLEAR); - keymap[0x23] = SDL_GetScancodeFromKey(SDLK_KP_DIVIDE); - keymap[0x24] = SDL_GetScancodeFromKey(SDLK_KP_MULTIPLY); - keymap[0x25] = SDL_GetScancodeFromKey(SDLK_KP_MINUS); - keymap[0x26] = SDL_GetScancodeFromKey(SDLK_TAB); - keymap[0x27] = SDL_GetScancodeFromKey(SDLK_q); - keymap[0x28] = SDL_GetScancodeFromKey(SDLK_w); - keymap[0x29] = SDL_GetScancodeFromKey(SDLK_e); - keymap[0x2a] = SDL_GetScancodeFromKey(SDLK_r); - keymap[0x2b] = SDL_GetScancodeFromKey(SDLK_t); - keymap[0x2c] = SDL_GetScancodeFromKey(SDLK_y); - keymap[0x2d] = SDL_GetScancodeFromKey(SDLK_u); - keymap[0x2e] = SDL_GetScancodeFromKey(SDLK_i); - keymap[0x2f] = SDL_GetScancodeFromKey(SDLK_o); - keymap[0x30] = SDL_GetScancodeFromKey(SDLK_p); - keymap[0x31] = SDL_GetScancodeFromKey(SDLK_LEFTBRACKET); - keymap[0x32] = SDL_GetScancodeFromKey(SDLK_RIGHTBRACKET); - keymap[0x33] = SDL_GetScancodeFromKey(SDLK_BACKSLASH); - keymap[0x34] = SDL_GetScancodeFromKey(SDLK_DELETE); - keymap[0x35] = SDL_GetScancodeFromKey(SDLK_END); - keymap[0x36] = SDL_GetScancodeFromKey(SDLK_PAGEDOWN); - keymap[0x37] = SDL_GetScancodeFromKey(SDLK_KP_7); - keymap[0x38] = SDL_GetScancodeFromKey(SDLK_KP_8); - keymap[0x39] = SDL_GetScancodeFromKey(SDLK_KP_9); - keymap[0x3a] = SDL_GetScancodeFromKey(SDLK_KP_PLUS); - keymap[0x3b] = SDL_GetScancodeFromKey(SDLK_CAPSLOCK); - keymap[0x3c] = SDL_GetScancodeFromKey(SDLK_a); - keymap[0x3d] = SDL_GetScancodeFromKey(SDLK_s); - keymap[0x3e] = SDL_GetScancodeFromKey(SDLK_d); - keymap[0x3f] = SDL_GetScancodeFromKey(SDLK_f); - keymap[0x40] = SDL_GetScancodeFromKey(SDLK_g); - keymap[0x41] = SDL_GetScancodeFromKey(SDLK_h); - keymap[0x42] = SDL_GetScancodeFromKey(SDLK_j); - keymap[0x43] = SDL_GetScancodeFromKey(SDLK_k); - keymap[0x44] = SDL_GetScancodeFromKey(SDLK_l); - keymap[0x45] = SDL_GetScancodeFromKey(SDLK_SEMICOLON); - keymap[0x46] = SDL_GetScancodeFromKey(SDLK_QUOTE); - keymap[0x47] = SDL_GetScancodeFromKey(SDLK_RETURN); - keymap[0x48] = SDL_GetScancodeFromKey(SDLK_KP_4); - keymap[0x49] = SDL_GetScancodeFromKey(SDLK_KP_5); - keymap[0x4a] = SDL_GetScancodeFromKey(SDLK_KP_6); - keymap[0x4b] = SDL_GetScancodeFromKey(SDLK_LSHIFT); - keymap[0x4c] = SDL_GetScancodeFromKey(SDLK_z); - keymap[0x4d] = SDL_GetScancodeFromKey(SDLK_x); - keymap[0x4e] = SDL_GetScancodeFromKey(SDLK_c); - keymap[0x4f] = SDL_GetScancodeFromKey(SDLK_v); - keymap[0x50] = SDL_GetScancodeFromKey(SDLK_b); - keymap[0x51] = SDL_GetScancodeFromKey(SDLK_n); - keymap[0x52] = SDL_GetScancodeFromKey(SDLK_m); - keymap[0x53] = SDL_GetScancodeFromKey(SDLK_COMMA); - keymap[0x54] = SDL_GetScancodeFromKey(SDLK_PERIOD); - keymap[0x55] = SDL_GetScancodeFromKey(SDLK_SLASH); - keymap[0x56] = SDL_GetScancodeFromKey(SDLK_RSHIFT); - keymap[0x57] = SDL_GetScancodeFromKey(SDLK_UP); - keymap[0x58] = SDL_GetScancodeFromKey(SDLK_KP_1); - keymap[0x59] = SDL_GetScancodeFromKey(SDLK_KP_2); - keymap[0x5a] = SDL_GetScancodeFromKey(SDLK_KP_3); - keymap[0x5b] = SDL_GetScancodeFromKey(SDLK_KP_ENTER); - keymap[0x5c] = SDL_GetScancodeFromKey(SDLK_LCTRL); - keymap[0x5d] = SDL_GetScancodeFromKey(SDLK_LALT); - keymap[0x5e] = SDL_GetScancodeFromKey(SDLK_SPACE); - keymap[0x5f] = SDL_GetScancodeFromKey(SDLK_RALT); - keymap[0x60] = SDL_GetScancodeFromKey(SDLK_RCTRL); - keymap[0x61] = SDL_GetScancodeFromKey(SDLK_LEFT); - keymap[0x62] = SDL_GetScancodeFromKey(SDLK_DOWN); - keymap[0x63] = SDL_GetScancodeFromKey(SDLK_RIGHT); - keymap[0x64] = SDL_GetScancodeFromKey(SDLK_KP_0); - keymap[0x65] = SDL_GetScancodeFromKey(SDLK_KP_PERIOD); - keymap[0x66] = SDL_GetScancodeFromKey(SDLK_LGUI); - keymap[0x67] = SDL_GetScancodeFromKey(SDLK_RGUI); - keymap[0x68] = SDL_GetScancodeFromKey(SDLK_MENU); - keymap[0x69] = SDL_GetScancodeFromKey(SDLK_2); /* SDLK_EURO */ - keymap[0x6a] = SDL_GetScancodeFromKey(SDLK_KP_EQUALS); - keymap[0x6b] = SDL_GetScancodeFromKey(SDLK_POWER); + keymap[0x01] = SDL_GetScancodeFromKey(SDLK_ESCAPE); + keymap[B_F1_KEY] = SDL_GetScancodeFromKey(SDLK_F1); + keymap[B_F2_KEY] = SDL_GetScancodeFromKey(SDLK_F2); + keymap[B_F3_KEY] = SDL_GetScancodeFromKey(SDLK_F3); + keymap[B_F4_KEY] = SDL_GetScancodeFromKey(SDLK_F4); + keymap[B_F5_KEY] = SDL_GetScancodeFromKey(SDLK_F5); + keymap[B_F6_KEY] = SDL_GetScancodeFromKey(SDLK_F6); + keymap[B_F7_KEY] = SDL_GetScancodeFromKey(SDLK_F7); + keymap[B_F8_KEY] = SDL_GetScancodeFromKey(SDLK_F8); + keymap[B_F9_KEY] = SDL_GetScancodeFromKey(SDLK_F9); + keymap[B_F10_KEY] = SDL_GetScancodeFromKey(SDLK_F10); + keymap[B_F11_KEY] = SDL_GetScancodeFromKey(SDLK_F11); + keymap[B_F12_KEY] = SDL_GetScancodeFromKey(SDLK_F12); + keymap[B_PRINT_KEY] = SDL_GetScancodeFromKey(SDLK_PRINTSCREEN); + keymap[B_SCROLL_KEY] = SDL_GetScancodeFromKey(SDLK_SCROLLLOCK); + keymap[B_PAUSE_KEY] = SDL_GetScancodeFromKey(SDLK_PAUSE); + keymap[0x11] = SDL_GetScancodeFromKey(SDLK_BACKQUOTE); + keymap[0x12] = SDL_GetScancodeFromKey(SDLK_1); + keymap[0x13] = SDL_GetScancodeFromKey(SDLK_2); + keymap[0x14] = SDL_GetScancodeFromKey(SDLK_3); + keymap[0x15] = SDL_GetScancodeFromKey(SDLK_4); + keymap[0x16] = SDL_GetScancodeFromKey(SDLK_5); + keymap[0x17] = SDL_GetScancodeFromKey(SDLK_6); + keymap[0x18] = SDL_GetScancodeFromKey(SDLK_7); + keymap[0x19] = SDL_GetScancodeFromKey(SDLK_8); + keymap[0x1a] = SDL_GetScancodeFromKey(SDLK_9); + keymap[0x1b] = SDL_GetScancodeFromKey(SDLK_0); + keymap[0x1c] = SDL_GetScancodeFromKey(SDLK_MINUS); + keymap[0x1d] = SDL_GetScancodeFromKey(SDLK_EQUALS); + keymap[0x1e] = SDL_GetScancodeFromKey(SDLK_BACKSPACE); + keymap[0x1f] = SDL_GetScancodeFromKey(SDLK_INSERT); + keymap[0x20] = SDL_GetScancodeFromKey(SDLK_HOME); + keymap[0x21] = SDL_GetScancodeFromKey(SDLK_PAGEUP); + keymap[0x22] = SDL_GetScancodeFromKey(SDLK_NUMLOCKCLEAR); + keymap[0x23] = SDL_GetScancodeFromKey(SDLK_KP_DIVIDE); + keymap[0x24] = SDL_GetScancodeFromKey(SDLK_KP_MULTIPLY); + keymap[0x25] = SDL_GetScancodeFromKey(SDLK_KP_MINUS); + keymap[0x26] = SDL_GetScancodeFromKey(SDLK_TAB); + keymap[0x27] = SDL_GetScancodeFromKey(SDLK_q); + keymap[0x28] = SDL_GetScancodeFromKey(SDLK_w); + keymap[0x29] = SDL_GetScancodeFromKey(SDLK_e); + keymap[0x2a] = SDL_GetScancodeFromKey(SDLK_r); + keymap[0x2b] = SDL_GetScancodeFromKey(SDLK_t); + keymap[0x2c] = SDL_GetScancodeFromKey(SDLK_y); + keymap[0x2d] = SDL_GetScancodeFromKey(SDLK_u); + keymap[0x2e] = SDL_GetScancodeFromKey(SDLK_i); + keymap[0x2f] = SDL_GetScancodeFromKey(SDLK_o); + keymap[0x30] = SDL_GetScancodeFromKey(SDLK_p); + keymap[0x31] = SDL_GetScancodeFromKey(SDLK_LEFTBRACKET); + keymap[0x32] = SDL_GetScancodeFromKey(SDLK_RIGHTBRACKET); + keymap[0x33] = SDL_GetScancodeFromKey(SDLK_BACKSLASH); + keymap[0x34] = SDL_GetScancodeFromKey(SDLK_DELETE); + keymap[0x35] = SDL_GetScancodeFromKey(SDLK_END); + keymap[0x36] = SDL_GetScancodeFromKey(SDLK_PAGEDOWN); + keymap[0x37] = SDL_GetScancodeFromKey(SDLK_KP_7); + keymap[0x38] = SDL_GetScancodeFromKey(SDLK_KP_8); + keymap[0x39] = SDL_GetScancodeFromKey(SDLK_KP_9); + keymap[0x3a] = SDL_GetScancodeFromKey(SDLK_KP_PLUS); + keymap[0x3b] = SDL_GetScancodeFromKey(SDLK_CAPSLOCK); + keymap[0x3c] = SDL_GetScancodeFromKey(SDLK_a); + keymap[0x3d] = SDL_GetScancodeFromKey(SDLK_s); + keymap[0x3e] = SDL_GetScancodeFromKey(SDLK_d); + keymap[0x3f] = SDL_GetScancodeFromKey(SDLK_f); + keymap[0x40] = SDL_GetScancodeFromKey(SDLK_g); + keymap[0x41] = SDL_GetScancodeFromKey(SDLK_h); + keymap[0x42] = SDL_GetScancodeFromKey(SDLK_j); + keymap[0x43] = SDL_GetScancodeFromKey(SDLK_k); + keymap[0x44] = SDL_GetScancodeFromKey(SDLK_l); + keymap[0x45] = SDL_GetScancodeFromKey(SDLK_SEMICOLON); + keymap[0x46] = SDL_GetScancodeFromKey(SDLK_QUOTE); + keymap[0x47] = SDL_GetScancodeFromKey(SDLK_RETURN); + keymap[0x48] = SDL_GetScancodeFromKey(SDLK_KP_4); + keymap[0x49] = SDL_GetScancodeFromKey(SDLK_KP_5); + keymap[0x4a] = SDL_GetScancodeFromKey(SDLK_KP_6); + keymap[0x4b] = SDL_GetScancodeFromKey(SDLK_LSHIFT); + keymap[0x4c] = SDL_GetScancodeFromKey(SDLK_z); + keymap[0x4d] = SDL_GetScancodeFromKey(SDLK_x); + keymap[0x4e] = SDL_GetScancodeFromKey(SDLK_c); + keymap[0x4f] = SDL_GetScancodeFromKey(SDLK_v); + keymap[0x50] = SDL_GetScancodeFromKey(SDLK_b); + keymap[0x51] = SDL_GetScancodeFromKey(SDLK_n); + keymap[0x52] = SDL_GetScancodeFromKey(SDLK_m); + keymap[0x53] = SDL_GetScancodeFromKey(SDLK_COMMA); + keymap[0x54] = SDL_GetScancodeFromKey(SDLK_PERIOD); + keymap[0x55] = SDL_GetScancodeFromKey(SDLK_SLASH); + keymap[0x56] = SDL_GetScancodeFromKey(SDLK_RSHIFT); + keymap[0x57] = SDL_GetScancodeFromKey(SDLK_UP); + keymap[0x58] = SDL_GetScancodeFromKey(SDLK_KP_1); + keymap[0x59] = SDL_GetScancodeFromKey(SDLK_KP_2); + keymap[0x5a] = SDL_GetScancodeFromKey(SDLK_KP_3); + keymap[0x5b] = SDL_GetScancodeFromKey(SDLK_KP_ENTER); + keymap[0x5c] = SDL_GetScancodeFromKey(SDLK_LCTRL); + keymap[0x5d] = SDL_GetScancodeFromKey(SDLK_LALT); + keymap[0x5e] = SDL_GetScancodeFromKey(SDLK_SPACE); + keymap[0x5f] = SDL_GetScancodeFromKey(SDLK_RALT); + keymap[0x60] = SDL_GetScancodeFromKey(SDLK_RCTRL); + keymap[0x61] = SDL_GetScancodeFromKey(SDLK_LEFT); + keymap[0x62] = SDL_GetScancodeFromKey(SDLK_DOWN); + keymap[0x63] = SDL_GetScancodeFromKey(SDLK_RIGHT); + keymap[0x64] = SDL_GetScancodeFromKey(SDLK_KP_0); + keymap[0x65] = SDL_GetScancodeFromKey(SDLK_KP_PERIOD); + keymap[0x66] = SDL_GetScancodeFromKey(SDLK_LGUI); + keymap[0x67] = SDL_GetScancodeFromKey(SDLK_RGUI); + keymap[0x68] = SDL_GetScancodeFromKey(SDLK_MENU); + keymap[0x69] = SDL_GetScancodeFromKey(SDLK_2); /* SDLK_EURO */ + keymap[0x6a] = SDL_GetScancodeFromKey(SDLK_KP_EQUALS); + keymap[0x6b] = SDL_GetScancodeFromKey(SDLK_POWER); } -SDL_Scancode BE_GetScancodeFromBeKey(int32 bkey) { - if(bkey > 0 && bkey < (int32)SDL_TABLESIZE(keymap)) { - return keymap[bkey]; - } else { - return SDL_SCANCODE_UNKNOWN; - } +SDL_Scancode HAIKU_GetScancodeFromBeKey(int32 bkey) { + if(bkey > 0 && bkey < (int32)SDL_TABLESIZE(keymap)) { + return keymap[bkey]; + } else { + return SDL_SCANCODE_UNKNOWN; + } } -int8 BE_GetKeyState(int32 bkey) { - if(bkey > 0 && bkey < KEYMAP_SIZE) { - return keystate[bkey]; - } else { - return SDL_RELEASED; - } +int8 HAIKU_GetKeyState(int32 bkey) { + if(bkey > 0 && bkey < KEYMAP_SIZE) { + return keystate[bkey]; + } else { + return SDL_RELEASED; + } } -void BE_SetKeyState(int32 bkey, int8 state) { - if(bkey > 0 && bkey < KEYMAP_SIZE) { - keystate[bkey] = state; - } +void HAIKU_SetKeyState(int32 bkey, int8 state) { + if(bkey > 0 && bkey < KEYMAP_SIZE) { + keystate[bkey] = state; + } } #ifdef __cplusplus diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bkeyboard.h b/Engine/lib/sdl/src/video/haiku/SDL_bkeyboard.h index 9620c9bdd..0184828ae 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bkeyboard.h +++ b/Engine/lib/sdl/src/video/haiku/SDL_bkeyboard.h @@ -30,10 +30,10 @@ extern "C" { #include "../../../include/SDL_keyboard.h" -extern void BE_InitOSKeymap(void); -extern SDL_Scancode BE_GetScancodeFromBeKey(int32 bkey); -extern int8 BE_GetKeyState(int32 bkey); -extern void BE_SetKeyState(int32 bkey, int8 state); +extern void HAIKU_InitOSKeymap(void); +extern SDL_Scancode HAIKU_GetScancodeFromBeKey(int32 bkey); +extern int8 HAIKU_GetKeyState(int32 bkey); +extern void HAIKU_SetKeyState(int32 bkey, int8 state); #ifdef __cplusplus } diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bmodes.cc b/Engine/lib/sdl/src/video/haiku/SDL_bmodes.cc index 110534280..9d7199673 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bmodes.cc +++ b/Engine/lib/sdl/src/video/haiku/SDL_bmodes.cc @@ -44,30 +44,30 @@ extern "C" { /* This wrapper is here so that the driverdata can be freed without freeing the display_mode structure */ struct SDL_DisplayModeData { - display_mode *bmode; + display_mode *bmode; }; #endif static SDL_INLINE SDL_BWin *_ToBeWin(SDL_Window *window) { - return ((SDL_BWin*)(window->driverdata)); + return ((SDL_BWin*)(window->driverdata)); } static SDL_INLINE SDL_BApp *_GetBeApp() { - return ((SDL_BApp*)be_app); + return ((SDL_BApp*)be_app); } static SDL_INLINE display_mode * _ExtractBMode(SDL_DisplayMode *mode) { #if WRAP_BMODE - return ((SDL_DisplayModeData*)mode->driverdata)->bmode; + return ((SDL_DisplayModeData*)mode->driverdata)->bmode; #else - return (display_mode*)(mode->driverdata); + return (display_mode*)(mode->driverdata); #endif } /* Copied from haiku/trunk/src/preferences/screen/ScreenMode.cpp */ static float get_refresh_rate(display_mode &mode) { - return float(mode.timing.pixel_clock * 1000) - / float(mode.timing.h_total * mode.timing.v_total); + return float(mode.timing.pixel_clock * 1000) + / float(mode.timing.h_total * mode.timing.v_total); } @@ -76,252 +76,252 @@ static float get_refresh_rate(display_mode &mode) { * This is a useful debugging tool. Uncomment and insert into code as needed. */ void _SpoutModeData(display_mode *bmode) { - printf("BMode:\n"); - printf("\tw,h = (%i,%i)\n", bmode->virtual_width, bmode->virtual_height); - printf("\th,v = (%i,%i)\n", bmode->h_display_start, - bmode->v_display_start); - if(bmode->flags) { - printf("\tFlags:\n"); - if(bmode->flags & B_SCROLL) { - printf("\t\tB_SCROLL\n"); - } - if(bmode->flags & B_8_BIT_DAC) { - printf("\t\tB_8_BIT_DAC\n"); - } - if(bmode->flags & B_HARDWARE_CURSOR) { - printf("\t\tB_HARDWARE_CURSOR\n"); - } - if(bmode->flags & B_PARALLEL_ACCESS) { - printf("\t\tB_PARALLEL_ACCESS\n"); - } - if(bmode->flags & B_DPMS) { - printf("\t\tB_DPMS\n"); - } - if(bmode->flags & B_IO_FB_NA) { - printf("\t\tB_IO_FB_NA\n"); - } - } - printf("\tTiming:\n"); - printf("\t\tpx clock: %i\n", bmode->timing.pixel_clock); - printf("\t\th - display: %i sync start: %i sync end: %i total: %i\n", - bmode->timing.h_display, bmode->timing.h_sync_start, - bmode->timing.h_sync_end, bmode->timing.h_total); - printf("\t\tv - display: %i sync start: %i sync end: %i total: %i\n", - bmode->timing.v_display, bmode->timing.v_sync_start, - bmode->timing.v_sync_end, bmode->timing.v_total); - if(bmode->timing.flags) { - printf("\t\tFlags:\n"); - if(bmode->timing.flags & B_BLANK_PEDESTAL) { - printf("\t\t\tB_BLANK_PEDESTAL\n"); - } - if(bmode->timing.flags & B_TIMING_INTERLACED) { - printf("\t\t\tB_TIMING_INTERLACED\n"); - } - if(bmode->timing.flags & B_POSITIVE_HSYNC) { - printf("\t\t\tB_POSITIVE_HSYNC\n"); - } - if(bmode->timing.flags & B_POSITIVE_VSYNC) { - printf("\t\t\tB_POSITIVE_VSYNC\n"); - } - if(bmode->timing.flags & B_SYNC_ON_GREEN) { - printf("\t\t\tB_SYNC_ON_GREEN\n"); - } - } + printf("BMode:\n"); + printf("\tw,h = (%i,%i)\n", bmode->virtual_width, bmode->virtual_height); + printf("\th,v = (%i,%i)\n", bmode->h_display_start, + bmode->v_display_start); + if(bmode->flags) { + printf("\tFlags:\n"); + if(bmode->flags & B_SCROLL) { + printf("\t\tB_SCROLL\n"); + } + if(bmode->flags & B_8_BIT_DAC) { + printf("\t\tB_8_BIT_DAC\n"); + } + if(bmode->flags & B_HARDWARE_CURSOR) { + printf("\t\tB_HARDWARE_CURSOR\n"); + } + if(bmode->flags & B_PARALLEL_ACCESS) { + printf("\t\tB_PARALLEL_ACCESS\n"); + } + if(bmode->flags & B_DPMS) { + printf("\t\tB_DPMS\n"); + } + if(bmode->flags & B_IO_FB_NA) { + printf("\t\tB_IO_FB_NA\n"); + } + } + printf("\tTiming:\n"); + printf("\t\tpx clock: %i\n", bmode->timing.pixel_clock); + printf("\t\th - display: %i sync start: %i sync end: %i total: %i\n", + bmode->timing.h_display, bmode->timing.h_sync_start, + bmode->timing.h_sync_end, bmode->timing.h_total); + printf("\t\tv - display: %i sync start: %i sync end: %i total: %i\n", + bmode->timing.v_display, bmode->timing.v_sync_start, + bmode->timing.v_sync_end, bmode->timing.v_total); + if(bmode->timing.flags) { + printf("\t\tFlags:\n"); + if(bmode->timing.flags & B_BLANK_PEDESTAL) { + printf("\t\t\tB_BLANK_PEDESTAL\n"); + } + if(bmode->timing.flags & B_TIMING_INTERLACED) { + printf("\t\t\tB_TIMING_INTERLACED\n"); + } + if(bmode->timing.flags & B_POSITIVE_HSYNC) { + printf("\t\t\tB_POSITIVE_HSYNC\n"); + } + if(bmode->timing.flags & B_POSITIVE_VSYNC) { + printf("\t\t\tB_POSITIVE_VSYNC\n"); + } + if(bmode->timing.flags & B_SYNC_ON_GREEN) { + printf("\t\t\tB_SYNC_ON_GREEN\n"); + } + } } #endif -int32 BE_ColorSpaceToBitsPerPixel(uint32 colorspace) +int32 HAIKU_ColorSpaceToBitsPerPixel(uint32 colorspace) { - int bitsperpixel; + int bitsperpixel; - bitsperpixel = 0; - switch (colorspace) { - case B_CMAP8: - bitsperpixel = 8; - break; - case B_RGB15: - case B_RGBA15: - case B_RGB15_BIG: - case B_RGBA15_BIG: - bitsperpixel = 15; - break; - case B_RGB16: - case B_RGB16_BIG: - bitsperpixel = 16; - break; - case B_RGB32: - case B_RGBA32: - case B_RGB32_BIG: - case B_RGBA32_BIG: - bitsperpixel = 32; - break; - default: - break; - } - return(bitsperpixel); + bitsperpixel = 0; + switch (colorspace) { + case B_CMAP8: + bitsperpixel = 8; + break; + case B_RGB15: + case B_RGBA15: + case B_RGB15_BIG: + case B_RGBA15_BIG: + bitsperpixel = 15; + break; + case B_RGB16: + case B_RGB16_BIG: + bitsperpixel = 16; + break; + case B_RGB32: + case B_RGBA32: + case B_RGB32_BIG: + case B_RGBA32_BIG: + bitsperpixel = 32; + break; + default: + break; + } + return(bitsperpixel); } -int32 BE_BPPToSDLPxFormat(int32 bpp) { - /* Translation taken from SDL_windowsmodes.c */ - switch (bpp) { - case 32: - return SDL_PIXELFORMAT_RGB888; - break; - case 24: /* May not be supported by Haiku */ - return SDL_PIXELFORMAT_RGB24; - break; - case 16: - return SDL_PIXELFORMAT_RGB565; - break; - case 15: - return SDL_PIXELFORMAT_RGB555; - break; - case 8: - return SDL_PIXELFORMAT_INDEX8; - break; - case 4: /* May not be supported by Haiku */ - return SDL_PIXELFORMAT_INDEX4LSB; - break; - } +int32 HAIKU_BPPToSDLPxFormat(int32 bpp) { + /* Translation taken from SDL_windowsmodes.c */ + switch (bpp) { + case 32: + return SDL_PIXELFORMAT_RGB888; + break; + case 24: /* May not be supported by Haiku */ + return SDL_PIXELFORMAT_RGB24; + break; + case 16: + return SDL_PIXELFORMAT_RGB565; + break; + case 15: + return SDL_PIXELFORMAT_RGB555; + break; + case 8: + return SDL_PIXELFORMAT_INDEX8; + break; + case 4: /* May not be supported by Haiku */ + return SDL_PIXELFORMAT_INDEX4LSB; + break; + } - /* May never get here, but safer and needed to shut up compiler */ - SDL_SetError("Invalid bpp value"); - return 0; + /* May never get here, but safer and needed to shut up compiler */ + SDL_SetError("Invalid bpp value"); + return 0; } static void _BDisplayModeToSdlDisplayMode(display_mode *bmode, - SDL_DisplayMode *mode) { - mode->w = bmode->virtual_width; - mode->h = bmode->virtual_height; - mode->refresh_rate = (int)get_refresh_rate(*bmode); + SDL_DisplayMode *mode) { + mode->w = bmode->virtual_width; + mode->h = bmode->virtual_height; + mode->refresh_rate = (int)get_refresh_rate(*bmode); #if WRAP_BMODE - SDL_DisplayModeData *data = (SDL_DisplayModeData*)SDL_calloc(1, - sizeof(SDL_DisplayModeData)); - data->bmode = bmode; - - mode->driverdata = data; + SDL_DisplayModeData *data = (SDL_DisplayModeData*)SDL_calloc(1, + sizeof(SDL_DisplayModeData)); + data->bmode = bmode; + + mode->driverdata = data; #else - mode->driverdata = bmode; + mode->driverdata = bmode; #endif - /* Set the format */ - int32 bpp = BE_ColorSpaceToBitsPerPixel(bmode->space); - mode->format = BE_BPPToSDLPxFormat(bpp); + /* Set the format */ + int32 bpp = HAIKU_ColorSpaceToBitsPerPixel(bmode->space); + mode->format = HAIKU_BPPToSDLPxFormat(bpp); } /* Later, there may be more than one monitor available */ static void _AddDisplay(BScreen *screen) { - SDL_VideoDisplay display; - SDL_DisplayMode *mode = (SDL_DisplayMode*)SDL_calloc(1, - sizeof(SDL_DisplayMode)); - display_mode *bmode = (display_mode*)SDL_calloc(1, sizeof(display_mode)); - screen->GetMode(bmode); + SDL_VideoDisplay display; + SDL_DisplayMode *mode = (SDL_DisplayMode*)SDL_calloc(1, + sizeof(SDL_DisplayMode)); + display_mode *bmode = (display_mode*)SDL_calloc(1, sizeof(display_mode)); + screen->GetMode(bmode); - _BDisplayModeToSdlDisplayMode(bmode, mode); - - SDL_zero(display); - display.desktop_mode = *mode; - display.current_mode = *mode; - - SDL_AddVideoDisplay(&display); + _BDisplayModeToSdlDisplayMode(bmode, mode); + + SDL_zero(display); + display.desktop_mode = *mode; + display.current_mode = *mode; + + SDL_AddVideoDisplay(&display); } /* * Functions called by SDL */ -int BE_InitModes(_THIS) { - BScreen screen; +int HAIKU_InitModes(_THIS) { + BScreen screen; - /* TODO: When Haiku supports multiple display screens, call - _AddDisplayScreen() for each of them. */ - _AddDisplay(&screen); - return 0; + /* TODO: When Haiku supports multiple display screens, call + _AddDisplayScreen() for each of them. */ + _AddDisplay(&screen); + return 0; } -int BE_QuitModes(_THIS) { - /* FIXME: Nothing really needs to be done here at the moment? */ - return 0; +int HAIKU_QuitModes(_THIS) { + /* FIXME: Nothing really needs to be done here at the moment? */ + return 0; } -int BE_GetDisplayBounds(_THIS, SDL_VideoDisplay *display, SDL_Rect *rect) { - BScreen bscreen; - BRect rc = bscreen.Frame(); - rect->x = (int)rc.left; - rect->y = (int)rc.top; - rect->w = (int)rc.Width() + 1; - rect->h = (int)rc.Height() + 1; - return 0; +int HAIKU_GetDisplayBounds(_THIS, SDL_VideoDisplay *display, SDL_Rect *rect) { + BScreen bscreen; + BRect rc = bscreen.Frame(); + rect->x = (int)rc.left; + rect->y = (int)rc.top; + rect->w = (int)rc.Width() + 1; + rect->h = (int)rc.Height() + 1; + return 0; } -void BE_GetDisplayModes(_THIS, SDL_VideoDisplay *display) { - /* Get the current screen */ - BScreen bscreen; +void HAIKU_GetDisplayModes(_THIS, SDL_VideoDisplay *display) { + /* Get the current screen */ + BScreen bscreen; - /* Iterate through all of the modes */ - SDL_DisplayMode mode; - display_mode this_bmode; - display_mode *bmodes; - uint32 count, i; - - /* Get graphics-hardware supported modes */ - bscreen.GetModeList(&bmodes, &count); - bscreen.GetMode(&this_bmode); - - for(i = 0; i < count; ++i) { - // FIXME: Apparently there are errors with colorspace changes - if (bmodes[i].space == this_bmode.space) { - _BDisplayModeToSdlDisplayMode(&bmodes[i], &mode); - SDL_AddDisplayMode(display, &mode); - } - } - free(bmodes); + /* Iterate through all of the modes */ + SDL_DisplayMode mode; + display_mode this_bmode; + display_mode *bmodes; + uint32 count, i; + + /* Get graphics-hardware supported modes */ + bscreen.GetModeList(&bmodes, &count); + bscreen.GetMode(&this_bmode); + + for(i = 0; i < count; ++i) { + // FIXME: Apparently there are errors with colorspace changes + if (bmodes[i].space == this_bmode.space) { + _BDisplayModeToSdlDisplayMode(&bmodes[i], &mode); + SDL_AddDisplayMode(display, &mode); + } + } + free(bmodes); } -int BE_SetDisplayMode(_THIS, SDL_VideoDisplay *display, SDL_DisplayMode *mode){ - /* Get the current screen */ - BScreen bscreen; - if(!bscreen.IsValid()) { - printf(__FILE__": %d - ERROR: BAD SCREEN\n", __LINE__); - } +int HAIKU_SetDisplayMode(_THIS, SDL_VideoDisplay *display, SDL_DisplayMode *mode){ + /* Get the current screen */ + BScreen bscreen; + if(!bscreen.IsValid()) { + printf(__FILE__": %d - ERROR: BAD SCREEN\n", __LINE__); + } - /* Set the mode using the driver data */ - display_mode *bmode = _ExtractBMode(mode); + /* Set the mode using the driver data */ + display_mode *bmode = _ExtractBMode(mode); - /* FIXME: Is the first option always going to be the right one? */ - uint32 c = 0, i; - display_mode *bmode_list; - bscreen.GetModeList(&bmode_list, &c); - for(i = 0; i < c; ++i) { - if( bmode_list[i].space == bmode->space && - bmode_list[i].virtual_width == bmode->virtual_width && - bmode_list[i].virtual_height == bmode->virtual_height ) { - bmode = &bmode_list[i]; - break; - } - } + /* FIXME: Is the first option always going to be the right one? */ + uint32 c = 0, i; + display_mode *bmode_list; + bscreen.GetModeList(&bmode_list, &c); + for(i = 0; i < c; ++i) { + if( bmode_list[i].space == bmode->space && + bmode_list[i].virtual_width == bmode->virtual_width && + bmode_list[i].virtual_height == bmode->virtual_height ) { + bmode = &bmode_list[i]; + break; + } + } - if(bscreen.SetMode(bmode) != B_OK) { - return SDL_SetError("Bad video mode"); - } - - free(bmode_list); - + if(bscreen.SetMode(bmode) != B_OK) { + return SDL_SetError("Bad video mode"); + } + + free(bmode_list); + #if SDL_VIDEO_OPENGL - /* FIXME: Is there some way to reboot the OpenGL context? This doesn't - help */ -// BE_GL_RebootContexts(_this); + /* FIXME: Is there some way to reboot the OpenGL context? This doesn't + help */ +// HAIKU_GL_RebootContexts(_this); #endif - return 0; + return 0; } #ifdef __cplusplus diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bmodes.h b/Engine/lib/sdl/src/video/haiku/SDL_bmodes.h index 38f4b5843..3abc1dce6 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bmodes.h +++ b/Engine/lib/sdl/src/video/haiku/SDL_bmodes.h @@ -28,15 +28,15 @@ extern "C" { #include "../SDL_sysvideo.h" -extern int32 BE_ColorSpaceToBitsPerPixel(uint32 colorspace); -extern int32 BE_BPPToSDLPxFormat(int32 bpp); +extern int32 HAIKU_ColorSpaceToBitsPerPixel(uint32 colorspace); +extern int32 HAIKU_BPPToSDLPxFormat(int32 bpp); -extern int BE_InitModes(_THIS); -extern int BE_QuitModes(_THIS); -extern int BE_GetDisplayBounds(_THIS, SDL_VideoDisplay *display, +extern int HAIKU_InitModes(_THIS); +extern int HAIKU_QuitModes(_THIS); +extern int HAIKU_GetDisplayBounds(_THIS, SDL_VideoDisplay *display, SDL_Rect *rect); -extern void BE_GetDisplayModes(_THIS, SDL_VideoDisplay *display); -extern int BE_SetDisplayMode(_THIS, SDL_VideoDisplay *display, +extern void HAIKU_GetDisplayModes(_THIS, SDL_VideoDisplay *display); +extern int HAIKU_SetDisplayMode(_THIS, SDL_VideoDisplay *display, SDL_DisplayMode *mode); #ifdef __cplusplus diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bopengl.cc b/Engine/lib/sdl/src/video/haiku/SDL_bopengl.cc index 3456932ce..e59906271 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bopengl.cc +++ b/Engine/lib/sdl/src/video/haiku/SDL_bopengl.cc @@ -44,7 +44,7 @@ static SDL_INLINE SDL_BApp *_GetBeApp() { } /* Passing a NULL path means load pointers from the application */ -int BE_GL_LoadLibrary(_THIS, const char *path) +int HAIKU_GL_LoadLibrary(_THIS, const char *path) { /* FIXME: Is this working correctly? */ image_info info; @@ -63,7 +63,7 @@ int BE_GL_LoadLibrary(_THIS, const char *path) return 0; } -void *BE_GL_GetProcAddress(_THIS, const char *proc) +void *HAIKU_GL_GetProcAddress(_THIS, const char *proc) { if (_this->gl_config.dll_handle != NULL) { void *location = NULL; @@ -86,19 +86,19 @@ void *BE_GL_GetProcAddress(_THIS, const char *proc) -int BE_GL_SwapWindow(_THIS, SDL_Window * window) { +int HAIKU_GL_SwapWindow(_THIS, SDL_Window * window) { _ToBeWin(window)->SwapBuffers(); return 0; } -int BE_GL_MakeCurrent(_THIS, SDL_Window * window, SDL_GLContext context) { +int HAIKU_GL_MakeCurrent(_THIS, SDL_Window * window, SDL_GLContext context) { SDL_BWin* win = (SDL_BWin*)context; _GetBeApp()->SetCurrentContext(win ? win->GetGLView() : NULL); return 0; } -SDL_GLContext BE_GL_CreateContext(_THIS, SDL_Window * window) { +SDL_GLContext HAIKU_GL_CreateContext(_THIS, SDL_Window * window) { /* FIXME: Not sure what flags should be included here; may want to have most of them */ SDL_BWin *bwin = _ToBeWin(window); @@ -127,24 +127,24 @@ SDL_GLContext BE_GL_CreateContext(_THIS, SDL_Window * window) { return (SDL_GLContext)(bwin); } -void BE_GL_DeleteContext(_THIS, SDL_GLContext context) { +void HAIKU_GL_DeleteContext(_THIS, SDL_GLContext context) { /* Currently, automatically unlocks the view */ ((SDL_BWin*)context)->RemoveGLView(); } -int BE_GL_SetSwapInterval(_THIS, int interval) { +int HAIKU_GL_SetSwapInterval(_THIS, int interval) { /* TODO: Implement this, if necessary? */ return SDL_Unsupported(); } -int BE_GL_GetSwapInterval(_THIS) { +int HAIKU_GL_GetSwapInterval(_THIS) { /* TODO: Implement this, if necessary? */ return 0; } -void BE_GL_UnloadLibrary(_THIS) { +void HAIKU_GL_UnloadLibrary(_THIS) { /* TODO: Implement this, if necessary? */ } @@ -152,7 +152,7 @@ void BE_GL_UnloadLibrary(_THIS) { /* FIXME: This function is meant to clear the OpenGL context when the video mode changes (see SDL_bmodes.cc), but it doesn't seem to help, and is not currently in use. */ -void BE_GL_RebootContexts(_THIS) { +void HAIKU_GL_RebootContexts(_THIS) { SDL_Window *window = _this->windows; while(window) { SDL_BWin *bwin = _ToBeWin(window); diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bopengl.h b/Engine/lib/sdl/src/video/haiku/SDL_bopengl.h index bc0798c93..b5b0de692 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bopengl.h +++ b/Engine/lib/sdl/src/video/haiku/SDL_bopengl.h @@ -31,18 +31,18 @@ extern "C" { #include "../SDL_sysvideo.h" -extern int BE_GL_LoadLibrary(_THIS, const char *path); /* FIXME */ -extern void *BE_GL_GetProcAddress(_THIS, const char *proc); /* FIXME */ -extern void BE_GL_UnloadLibrary(_THIS); /* TODO */ -extern int BE_GL_MakeCurrent(_THIS, SDL_Window * window, +extern int HAIKU_GL_LoadLibrary(_THIS, const char *path); /* FIXME */ +extern void *HAIKU_GL_GetProcAddress(_THIS, const char *proc); /* FIXME */ +extern void HAIKU_GL_UnloadLibrary(_THIS); /* TODO */ +extern int HAIKU_GL_MakeCurrent(_THIS, SDL_Window * window, SDL_GLContext context); -extern int BE_GL_SetSwapInterval(_THIS, int interval); /* TODO */ -extern int BE_GL_GetSwapInterval(_THIS); /* TODO */ -extern int BE_GL_SwapWindow(_THIS, SDL_Window * window); -extern SDL_GLContext BE_GL_CreateContext(_THIS, SDL_Window * window); -extern void BE_GL_DeleteContext(_THIS, SDL_GLContext context); +extern int HAIKU_GL_SetSwapInterval(_THIS, int interval); /* TODO */ +extern int HAIKU_GL_GetSwapInterval(_THIS); /* TODO */ +extern int HAIKU_GL_SwapWindow(_THIS, SDL_Window * window); +extern SDL_GLContext HAIKU_GL_CreateContext(_THIS, SDL_Window * window); +extern void HAIKU_GL_DeleteContext(_THIS, SDL_GLContext context); -extern void BE_GL_RebootContexts(_THIS); +extern void HAIKU_GL_RebootContexts(_THIS); #ifdef __cplusplus } diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bvideo.cc b/Engine/lib/sdl/src/video/haiku/SDL_bvideo.cc index afe20e3b8..e7b4b6e30 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bvideo.cc +++ b/Engine/lib/sdl/src/video/haiku/SDL_bvideo.cc @@ -37,17 +37,17 @@ extern "C" { #include "SDL_bevents.h" /* FIXME: Undefined functions */ -// #define BE_PumpEvents NULL - #define BE_StartTextInput NULL - #define BE_StopTextInput NULL - #define BE_SetTextInputRect NULL +// #define HAIKU_PumpEvents NULL + #define HAIKU_StartTextInput NULL + #define HAIKU_StopTextInput NULL + #define HAIKU_SetTextInputRect NULL -// #define BE_DeleteDevice NULL +// #define HAIKU_DeleteDevice NULL /* End undefined functions */ static SDL_VideoDevice * -BE_CreateDevice(int devindex) +HAIKU_CreateDevice(int devindex) { SDL_VideoDevice *device; /*SDL_VideoData *data;*/ @@ -56,115 +56,114 @@ BE_CreateDevice(int devindex) device = (SDL_VideoDevice *) SDL_calloc(1, sizeof(SDL_VideoDevice)); device->driverdata = NULL; /* FIXME: Is this the cause of some of the - SDL_Quit() errors? */ + SDL_Quit() errors? */ /* TODO: Figure out if any initialization needs to go here */ /* Set the function pointers */ - device->VideoInit = BE_VideoInit; - device->VideoQuit = BE_VideoQuit; - device->GetDisplayBounds = BE_GetDisplayBounds; - device->GetDisplayModes = BE_GetDisplayModes; - device->SetDisplayMode = BE_SetDisplayMode; - device->PumpEvents = BE_PumpEvents; + device->VideoInit = HAIKU_VideoInit; + device->VideoQuit = HAIKU_VideoQuit; + device->GetDisplayBounds = HAIKU_GetDisplayBounds; + device->GetDisplayModes = HAIKU_GetDisplayModes; + device->SetDisplayMode = HAIKU_SetDisplayMode; + device->PumpEvents = HAIKU_PumpEvents; - device->CreateSDLWindow = BE_CreateWindow; - device->CreateSDLWindowFrom = BE_CreateWindowFrom; - device->SetWindowTitle = BE_SetWindowTitle; - device->SetWindowIcon = BE_SetWindowIcon; - device->SetWindowPosition = BE_SetWindowPosition; - device->SetWindowSize = BE_SetWindowSize; - device->ShowWindow = BE_ShowWindow; - device->HideWindow = BE_HideWindow; - device->RaiseWindow = BE_RaiseWindow; - device->MaximizeWindow = BE_MaximizeWindow; - device->MinimizeWindow = BE_MinimizeWindow; - device->RestoreWindow = BE_RestoreWindow; - device->SetWindowBordered = BE_SetWindowBordered; - device->SetWindowResizable = BE_SetWindowResizable; - device->SetWindowFullscreen = BE_SetWindowFullscreen; - device->SetWindowGammaRamp = BE_SetWindowGammaRamp; - device->GetWindowGammaRamp = BE_GetWindowGammaRamp; - device->SetWindowGrab = BE_SetWindowGrab; - device->DestroyWindow = BE_DestroyWindow; - device->GetWindowWMInfo = BE_GetWindowWMInfo; - device->CreateWindowFramebuffer = BE_CreateWindowFramebuffer; - device->UpdateWindowFramebuffer = BE_UpdateWindowFramebuffer; - device->DestroyWindowFramebuffer = BE_DestroyWindowFramebuffer; + device->CreateSDLWindow = HAIKU_CreateWindow; + device->CreateSDLWindowFrom = HAIKU_CreateWindowFrom; + device->SetWindowTitle = HAIKU_SetWindowTitle; + device->SetWindowIcon = HAIKU_SetWindowIcon; + device->SetWindowPosition = HAIKU_SetWindowPosition; + device->SetWindowSize = HAIKU_SetWindowSize; + device->ShowWindow = HAIKU_ShowWindow; + device->HideWindow = HAIKU_HideWindow; + device->RaiseWindow = HAIKU_RaiseWindow; + device->MaximizeWindow = HAIKU_MaximizeWindow; + device->MinimizeWindow = HAIKU_MinimizeWindow; + device->RestoreWindow = HAIKU_RestoreWindow; + device->SetWindowBordered = HAIKU_SetWindowBordered; + device->SetWindowResizable = HAIKU_SetWindowResizable; + device->SetWindowFullscreen = HAIKU_SetWindowFullscreen; + device->SetWindowGammaRamp = HAIKU_SetWindowGammaRamp; + device->GetWindowGammaRamp = HAIKU_GetWindowGammaRamp; + device->SetWindowGrab = HAIKU_SetWindowGrab; + device->DestroyWindow = HAIKU_DestroyWindow; + device->GetWindowWMInfo = HAIKU_GetWindowWMInfo; + device->CreateWindowFramebuffer = HAIKU_CreateWindowFramebuffer; + device->UpdateWindowFramebuffer = HAIKU_UpdateWindowFramebuffer; + device->DestroyWindowFramebuffer = HAIKU_DestroyWindowFramebuffer; device->shape_driver.CreateShaper = NULL; device->shape_driver.SetWindowShape = NULL; device->shape_driver.ResizeWindowShape = NULL; #if SDL_VIDEO_OPENGL - device->GL_LoadLibrary = BE_GL_LoadLibrary; - device->GL_GetProcAddress = BE_GL_GetProcAddress; - device->GL_UnloadLibrary = BE_GL_UnloadLibrary; - device->GL_CreateContext = BE_GL_CreateContext; - device->GL_MakeCurrent = BE_GL_MakeCurrent; - device->GL_SetSwapInterval = BE_GL_SetSwapInterval; - device->GL_GetSwapInterval = BE_GL_GetSwapInterval; - device->GL_SwapWindow = BE_GL_SwapWindow; - device->GL_DeleteContext = BE_GL_DeleteContext; + device->GL_LoadLibrary = HAIKU_GL_LoadLibrary; + device->GL_GetProcAddress = HAIKU_GL_GetProcAddress; + device->GL_UnloadLibrary = HAIKU_GL_UnloadLibrary; + device->GL_CreateContext = HAIKU_GL_CreateContext; + device->GL_MakeCurrent = HAIKU_GL_MakeCurrent; + device->GL_SetSwapInterval = HAIKU_GL_SetSwapInterval; + device->GL_GetSwapInterval = HAIKU_GL_GetSwapInterval; + device->GL_SwapWindow = HAIKU_GL_SwapWindow; + device->GL_DeleteContext = HAIKU_GL_DeleteContext; #endif - device->StartTextInput = BE_StartTextInput; - device->StopTextInput = BE_StopTextInput; - device->SetTextInputRect = BE_SetTextInputRect; + device->StartTextInput = HAIKU_StartTextInput; + device->StopTextInput = HAIKU_StopTextInput; + device->SetTextInputRect = HAIKU_SetTextInputRect; - device->SetClipboardText = BE_SetClipboardText; - device->GetClipboardText = BE_GetClipboardText; - device->HasClipboardText = BE_HasClipboardText; + device->SetClipboardText = HAIKU_SetClipboardText; + device->GetClipboardText = HAIKU_GetClipboardText; + device->HasClipboardText = HAIKU_HasClipboardText; - device->free = BE_DeleteDevice; + device->free = HAIKU_DeleteDevice; return device; } VideoBootStrap HAIKU_bootstrap = { - "haiku", "Haiku graphics", - BE_Available, BE_CreateDevice + "haiku", "Haiku graphics", + HAIKU_Available, HAIKU_CreateDevice }; -void BE_DeleteDevice(SDL_VideoDevice * device) +void HAIKU_DeleteDevice(SDL_VideoDevice * device) { - SDL_free(device->driverdata); - SDL_free(device); + SDL_free(device->driverdata); + SDL_free(device); } -int BE_VideoInit(_THIS) +int HAIKU_VideoInit(_THIS) { - /* Initialize the Be Application for appserver interaction */ - if (SDL_InitBeApp() < 0) { - return -1; - } - - /* Initialize video modes */ - BE_InitModes(_this); + /* Initialize the Be Application for appserver interaction */ + if (SDL_InitBeApp() < 0) { + return -1; + } + + /* Initialize video modes */ + HAIKU_InitModes(_this); + + /* Init the keymap */ + HAIKU_InitOSKeymap(); - /* Init the keymap */ - BE_InitOSKeymap(); - - #if SDL_VIDEO_OPENGL /* testgl application doesn't load library, just tries to load symbols */ /* is it correct? if so we have to load library here */ - BE_GL_LoadLibrary(_this, NULL); + HAIKU_GL_LoadLibrary(_this, NULL); #endif - /* We're done! */ + /* We're done! */ return (0); } -int BE_Available(void) +int HAIKU_Available(void) { return (1); } -void BE_VideoQuit(_THIS) +void HAIKU_VideoQuit(_THIS) { - BE_QuitModes(_this); + HAIKU_QuitModes(_this); SDL_QuitBeApp(); } diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bvideo.h b/Engine/lib/sdl/src/video/haiku/SDL_bvideo.h index 0e28220d9..a1d01fb25 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bvideo.h +++ b/Engine/lib/sdl/src/video/haiku/SDL_bvideo.h @@ -30,10 +30,10 @@ extern "C" { #include "../SDL_sysvideo.h" -extern void BE_VideoQuit(_THIS); -extern int BE_VideoInit(_THIS); -extern void BE_DeleteDevice(_THIS); -extern int BE_Available(void); +extern void HAIKU_VideoQuit(_THIS); +extern int HAIKU_VideoInit(_THIS); +extern void HAIKU_DeleteDevice(_THIS); +extern int HAIKU_Available(void); #ifdef __cplusplus } diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bwindow.cc b/Engine/lib/sdl/src/video/haiku/SDL_bwindow.cc index 0931abe9a..142a3fa18 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bwindow.cc +++ b/Engine/lib/sdl/src/video/haiku/SDL_bwindow.cc @@ -32,36 +32,36 @@ extern "C" { #endif static SDL_INLINE SDL_BWin *_ToBeWin(SDL_Window *window) { - return ((SDL_BWin*)(window->driverdata)); + return ((SDL_BWin*)(window->driverdata)); } static SDL_INLINE SDL_BApp *_GetBeApp() { - return ((SDL_BApp*)be_app); + return ((SDL_BApp*)be_app); } static int _InitWindow(_THIS, SDL_Window *window) { - uint32 flags = 0; - window_look look = B_TITLED_WINDOW_LOOK; + uint32 flags = 0; + window_look look = B_TITLED_WINDOW_LOOK; - BRect bounds( + BRect bounds( window->x, window->y, - window->x + window->w - 1, //BeWindows have an off-by-one px w/h thing + window->x + window->w - 1, //BeWindows have an off-by-one px w/h thing window->y + window->h - 1 ); if(window->flags & SDL_WINDOW_FULLSCREEN) { - /* TODO: Add support for this flag */ - printf(__FILE__": %d!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n",__LINE__); + /* TODO: Add support for this flag */ + printf(__FILE__": %d!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n",__LINE__); } if(window->flags & SDL_WINDOW_OPENGL) { - /* TODO: Add support for this flag */ + /* TODO: Add support for this flag */ } if(!(window->flags & SDL_WINDOW_RESIZABLE)) { - flags |= B_NOT_RESIZABLE | B_NOT_ZOOMABLE; + flags |= B_NOT_RESIZABLE | B_NOT_ZOOMABLE; } if(window->flags & SDL_WINDOW_BORDERLESS) { - look = B_NO_BORDER_WINDOW_LOOK; + look = B_NO_BORDER_WINDOW_LOOK; } SDL_BWin *bwin = new(std::nothrow) SDL_BWin(bounds, look, flags); @@ -75,39 +75,39 @@ static int _InitWindow(_THIS, SDL_Window *window) { return 0; } -int BE_CreateWindow(_THIS, SDL_Window *window) { +int HAIKU_CreateWindow(_THIS, SDL_Window *window) { if (_InitWindow(_this, window) < 0) { return -1; } - - /* Start window loop */ + + /* Start window loop */ _ToBeWin(window)->Show(); return 0; } -int BE_CreateWindowFrom(_THIS, SDL_Window * window, const void *data) { +int HAIKU_CreateWindowFrom(_THIS, SDL_Window * window, const void *data) { - SDL_BWin *otherBWin = (SDL_BWin*)data; - if(!otherBWin->LockLooper()) - return -1; - - /* Create the new window and initialize its members */ - window->x = (int)otherBWin->Frame().left; - window->y = (int)otherBWin->Frame().top; - window->w = (int)otherBWin->Frame().Width(); - window->h = (int)otherBWin->Frame().Height(); - - /* Set SDL flags */ - if(!(otherBWin->Flags() & B_NOT_RESIZABLE)) { - window->flags |= SDL_WINDOW_RESIZABLE; - } - - /* If we are out of memory, return the error code */ + SDL_BWin *otherBWin = (SDL_BWin*)data; + if(!otherBWin->LockLooper()) + return -1; + + /* Create the new window and initialize its members */ + window->x = (int)otherBWin->Frame().left; + window->y = (int)otherBWin->Frame().top; + window->w = (int)otherBWin->Frame().Width(); + window->h = (int)otherBWin->Frame().Height(); + + /* Set SDL flags */ + if(!(otherBWin->Flags() & B_NOT_RESIZABLE)) { + window->flags |= SDL_WINDOW_RESIZABLE; + } + + /* If we are out of memory, return the error code */ if (_InitWindow(_this, window) < 0) { return -1; } - - /* TODO: Add any other SDL-supported window attributes here */ + + /* TODO: Add any other SDL-supported window attributes here */ _ToBeWin(window)->SetTitle(otherBWin->Title()); /* Start window loop and unlock the other window */ @@ -117,107 +117,107 @@ int BE_CreateWindowFrom(_THIS, SDL_Window * window, const void *data) { return 0; } -void BE_SetWindowTitle(_THIS, SDL_Window * window) { - BMessage msg(BWIN_SET_TITLE); - msg.AddString("window-title", window->title); - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_SetWindowTitle(_THIS, SDL_Window * window) { + BMessage msg(BWIN_SET_TITLE); + msg.AddString("window-title", window->title); + _ToBeWin(window)->PostMessage(&msg); } -void BE_SetWindowIcon(_THIS, SDL_Window * window, SDL_Surface * icon) { - /* FIXME: Icons not supported by Haiku */ +void HAIKU_SetWindowIcon(_THIS, SDL_Window * window, SDL_Surface * icon) { + /* FIXME: Icons not supported by Haiku */ } -void BE_SetWindowPosition(_THIS, SDL_Window * window) { - BMessage msg(BWIN_MOVE_WINDOW); - msg.AddInt32("window-x", window->x); - msg.AddInt32("window-y", window->y); - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_SetWindowPosition(_THIS, SDL_Window * window) { + BMessage msg(BWIN_MOVE_WINDOW); + msg.AddInt32("window-x", window->x); + msg.AddInt32("window-y", window->y); + _ToBeWin(window)->PostMessage(&msg); } -void BE_SetWindowSize(_THIS, SDL_Window * window) { - BMessage msg(BWIN_RESIZE_WINDOW); - msg.AddInt32("window-w", window->w - 1); - msg.AddInt32("window-h", window->h - 1); - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_SetWindowSize(_THIS, SDL_Window * window) { + BMessage msg(BWIN_RESIZE_WINDOW); + msg.AddInt32("window-w", window->w - 1); + msg.AddInt32("window-h", window->h - 1); + _ToBeWin(window)->PostMessage(&msg); } -void BE_SetWindowBordered(_THIS, SDL_Window * window, SDL_bool bordered) { - BMessage msg(BWIN_SET_BORDERED); - msg.AddBool("window-border", bordered != SDL_FALSE); - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_SetWindowBordered(_THIS, SDL_Window * window, SDL_bool bordered) { + BMessage msg(BWIN_SET_BORDERED); + msg.AddBool("window-border", bordered != SDL_FALSE); + _ToBeWin(window)->PostMessage(&msg); } -void BE_SetWindowResizable(_THIS, SDL_Window * window, SDL_bool resizable) { - BMessage msg(BWIN_SET_RESIZABLE); - msg.AddBool("window-resizable", resizable != SDL_FALSE); - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_SetWindowResizable(_THIS, SDL_Window * window, SDL_bool resizable) { + BMessage msg(BWIN_SET_RESIZABLE); + msg.AddBool("window-resizable", resizable != SDL_FALSE); + _ToBeWin(window)->PostMessage(&msg); } -void BE_ShowWindow(_THIS, SDL_Window * window) { - BMessage msg(BWIN_SHOW_WINDOW); - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_ShowWindow(_THIS, SDL_Window * window) { + BMessage msg(BWIN_SHOW_WINDOW); + _ToBeWin(window)->PostMessage(&msg); } -void BE_HideWindow(_THIS, SDL_Window * window) { - BMessage msg(BWIN_HIDE_WINDOW); - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_HideWindow(_THIS, SDL_Window * window) { + BMessage msg(BWIN_HIDE_WINDOW); + _ToBeWin(window)->PostMessage(&msg); } -void BE_RaiseWindow(_THIS, SDL_Window * window) { - BMessage msg(BWIN_SHOW_WINDOW); /* Activate this window and move to front */ - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_RaiseWindow(_THIS, SDL_Window * window) { + BMessage msg(BWIN_SHOW_WINDOW); /* Activate this window and move to front */ + _ToBeWin(window)->PostMessage(&msg); } -void BE_MaximizeWindow(_THIS, SDL_Window * window) { - BMessage msg(BWIN_MAXIMIZE_WINDOW); - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_MaximizeWindow(_THIS, SDL_Window * window) { + BMessage msg(BWIN_MAXIMIZE_WINDOW); + _ToBeWin(window)->PostMessage(&msg); } -void BE_MinimizeWindow(_THIS, SDL_Window * window) { - BMessage msg(BWIN_MINIMIZE_WINDOW); - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_MinimizeWindow(_THIS, SDL_Window * window) { + BMessage msg(BWIN_MINIMIZE_WINDOW); + _ToBeWin(window)->PostMessage(&msg); } -void BE_RestoreWindow(_THIS, SDL_Window * window) { - BMessage msg(BWIN_RESTORE_WINDOW); - _ToBeWin(window)->PostMessage(&msg); +void HAIKU_RestoreWindow(_THIS, SDL_Window * window) { + BMessage msg(BWIN_RESTORE_WINDOW); + _ToBeWin(window)->PostMessage(&msg); } -void BE_SetWindowFullscreen(_THIS, SDL_Window * window, - SDL_VideoDisplay * display, SDL_bool fullscreen) { - /* Haiku tracks all video display information */ - BMessage msg(BWIN_FULLSCREEN); - msg.AddBool("fullscreen", fullscreen); - _ToBeWin(window)->PostMessage(&msg); - +void HAIKU_SetWindowFullscreen(_THIS, SDL_Window * window, + SDL_VideoDisplay * display, SDL_bool fullscreen) { + /* Haiku tracks all video display information */ + BMessage msg(BWIN_FULLSCREEN); + msg.AddBool("fullscreen", fullscreen); + _ToBeWin(window)->PostMessage(&msg); + } -int BE_SetWindowGammaRamp(_THIS, SDL_Window * window, const Uint16 * ramp) { - /* FIXME: Not Haiku supported */ - return -1; +int HAIKU_SetWindowGammaRamp(_THIS, SDL_Window * window, const Uint16 * ramp) { + /* FIXME: Not Haiku supported */ + return -1; } -int BE_GetWindowGammaRamp(_THIS, SDL_Window * window, Uint16 * ramp) { - /* FIXME: Not Haiku supported */ - return -1; +int HAIKU_GetWindowGammaRamp(_THIS, SDL_Window * window, Uint16 * ramp) { + /* FIXME: Not Haiku supported */ + return -1; } -void BE_SetWindowGrab(_THIS, SDL_Window * window, SDL_bool grabbed) { - /* TODO: Implement this! */ +void HAIKU_SetWindowGrab(_THIS, SDL_Window * window, SDL_bool grabbed) { + /* TODO: Implement this! */ } -void BE_DestroyWindow(_THIS, SDL_Window * window) { - _ToBeWin(window)->LockLooper(); /* This MUST be locked */ - _GetBeApp()->ClearID(_ToBeWin(window)); - _ToBeWin(window)->Quit(); - window->driverdata = NULL; +void HAIKU_DestroyWindow(_THIS, SDL_Window * window) { + _ToBeWin(window)->LockLooper(); /* This MUST be locked */ + _GetBeApp()->ClearID(_ToBeWin(window)); + _ToBeWin(window)->Quit(); + window->driverdata = NULL; } -SDL_bool BE_GetWindowWMInfo(_THIS, SDL_Window * window, +SDL_bool HAIKU_GetWindowWMInfo(_THIS, SDL_Window * window, struct SDL_SysWMinfo *info) { - /* FIXME: What is the point of this? What information should be included? */ - return SDL_FALSE; + /* FIXME: What is the point of this? What information should be included? */ + return SDL_FALSE; } diff --git a/Engine/lib/sdl/src/video/haiku/SDL_bwindow.h b/Engine/lib/sdl/src/video/haiku/SDL_bwindow.h index 100ffed1a..2894f2794 100644 --- a/Engine/lib/sdl/src/video/haiku/SDL_bwindow.h +++ b/Engine/lib/sdl/src/video/haiku/SDL_bwindow.h @@ -26,26 +26,26 @@ #include "../SDL_sysvideo.h" -extern int BE_CreateWindow(_THIS, SDL_Window *window); -extern int BE_CreateWindowFrom(_THIS, SDL_Window * window, const void *data); -extern void BE_SetWindowTitle(_THIS, SDL_Window * window); -extern void BE_SetWindowIcon(_THIS, SDL_Window * window, SDL_Surface * icon); -extern void BE_SetWindowPosition(_THIS, SDL_Window * window); -extern void BE_SetWindowSize(_THIS, SDL_Window * window); -extern void BE_ShowWindow(_THIS, SDL_Window * window); -extern void BE_HideWindow(_THIS, SDL_Window * window); -extern void BE_RaiseWindow(_THIS, SDL_Window * window); -extern void BE_MaximizeWindow(_THIS, SDL_Window * window); -extern void BE_MinimizeWindow(_THIS, SDL_Window * window); -extern void BE_RestoreWindow(_THIS, SDL_Window * window); -extern void BE_SetWindowBordered(_THIS, SDL_Window * window, SDL_bool bordered); -extern void BE_SetWindowResizable(_THIS, SDL_Window * window, SDL_bool resizable); -extern void BE_SetWindowFullscreen(_THIS, SDL_Window * window, SDL_VideoDisplay * display, SDL_bool fullscreen); -extern int BE_SetWindowGammaRamp(_THIS, SDL_Window * window, const Uint16 * ramp); -extern int BE_GetWindowGammaRamp(_THIS, SDL_Window * window, Uint16 * ramp); -extern void BE_SetWindowGrab(_THIS, SDL_Window * window, SDL_bool grabbed); -extern void BE_DestroyWindow(_THIS, SDL_Window * window); -extern SDL_bool BE_GetWindowWMInfo(_THIS, SDL_Window * window, +extern int HAIKU_CreateWindow(_THIS, SDL_Window *window); +extern int HAIKU_CreateWindowFrom(_THIS, SDL_Window * window, const void *data); +extern void HAIKU_SetWindowTitle(_THIS, SDL_Window * window); +extern void HAIKU_SetWindowIcon(_THIS, SDL_Window * window, SDL_Surface * icon); +extern void HAIKU_SetWindowPosition(_THIS, SDL_Window * window); +extern void HAIKU_SetWindowSize(_THIS, SDL_Window * window); +extern void HAIKU_ShowWindow(_THIS, SDL_Window * window); +extern void HAIKU_HideWindow(_THIS, SDL_Window * window); +extern void HAIKU_RaiseWindow(_THIS, SDL_Window * window); +extern void HAIKU_MaximizeWindow(_THIS, SDL_Window * window); +extern void HAIKU_MinimizeWindow(_THIS, SDL_Window * window); +extern void HAIKU_RestoreWindow(_THIS, SDL_Window * window); +extern void HAIKU_SetWindowBordered(_THIS, SDL_Window * window, SDL_bool bordered); +extern void HAIKU_SetWindowResizable(_THIS, SDL_Window * window, SDL_bool resizable); +extern void HAIKU_SetWindowFullscreen(_THIS, SDL_Window * window, SDL_VideoDisplay * display, SDL_bool fullscreen); +extern int HAIKU_SetWindowGammaRamp(_THIS, SDL_Window * window, const Uint16 * ramp); +extern int HAIKU_GetWindowGammaRamp(_THIS, SDL_Window * window, Uint16 * ramp); +extern void HAIKU_SetWindowGrab(_THIS, SDL_Window * window, SDL_bool grabbed); +extern void HAIKU_DestroyWindow(_THIS, SDL_Window * window); +extern SDL_bool HAIKU_GetWindowWMInfo(_THIS, SDL_Window * window, struct SDL_SysWMinfo *info); diff --git a/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmmouse.c b/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmmouse.c index e23dd1310..04740899f 100644 --- a/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmmouse.c +++ b/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmmouse.c @@ -487,12 +487,12 @@ KMSDRM_MoveCursor(SDL_Cursor * cursor) That's why we move the cursor graphic ONLY. */ if (mouse != NULL && mouse->cur_cursor != NULL && mouse->cur_cursor->driverdata != NULL) { curdata = (KMSDRM_CursorData *) mouse->cur_cursor->driverdata; - drm_fd = KMSDRM_gbm_device_get_fd(KMSDRM_gbm_bo_get_device(curdata->bo)); - ret = KMSDRM_drmModeMoveCursor(drm_fd, curdata->crtc_id, mouse->x, mouse->y); + drm_fd = KMSDRM_gbm_device_get_fd(KMSDRM_gbm_bo_get_device(curdata->bo)); + ret = KMSDRM_drmModeMoveCursor(drm_fd, curdata->crtc_id, mouse->x, mouse->y); - if (ret) { - SDL_SetError("drmModeMoveCursor() failed."); - } + if (ret) { + SDL_SetError("drmModeMoveCursor() failed."); + } } } diff --git a/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmopengles.c b/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmopengles.c index 7a0b079ff..fc6304d5f 100644 --- a/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmopengles.c +++ b/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmopengles.c @@ -50,23 +50,23 @@ KMSDRM_GLES_SetupCrtc(_THIS, SDL_Window * window) { KMSDRM_FBInfo *fb_info; if (!(_this->egl_data->eglSwapBuffers(_this->egl_data->egl_display, wdata->egl_surface))) { - SDL_LogError(SDL_LOG_CATEGORY_VIDEO, "eglSwapBuffers failed on CRTC setup"); - return SDL_FALSE; + SDL_LogError(SDL_LOG_CATEGORY_VIDEO, "eglSwapBuffers failed on CRTC setup"); + return SDL_FALSE; } - wdata->next_bo = KMSDRM_gbm_surface_lock_front_buffer(wdata->gs); - if (wdata->next_bo == NULL) { - SDL_LogError(SDL_LOG_CATEGORY_VIDEO, "Could not lock GBM surface front buffer on CRTC setup"); - return SDL_FALSE; + wdata->crtc_bo = KMSDRM_gbm_surface_lock_front_buffer(wdata->gs); + if (wdata->crtc_bo == NULL) { + SDL_LogError(SDL_LOG_CATEGORY_VIDEO, "Could not lock GBM surface front buffer on CRTC setup"); + return SDL_FALSE; } - fb_info = KMSDRM_FBFromBO(_this, wdata->next_bo); + fb_info = KMSDRM_FBFromBO(_this, wdata->crtc_bo); if (fb_info == NULL) { - return SDL_FALSE; + return SDL_FALSE; } if(KMSDRM_drmModeSetCrtc(vdata->drm_fd, displaydata->crtc_id, fb_info->fb_id, - 0, 0, &vdata->saved_conn_id, 1, &displaydata->cur_mode) != 0) { + 0, 0, &vdata->saved_conn_id, 1, &displaydata->cur_mode) != 0) { SDL_LogWarn(SDL_LOG_CATEGORY_VIDEO, "Could not set up CRTC to a GBM buffer"); return SDL_FALSE; @@ -153,14 +153,14 @@ KMSDRM_GLES_SwapWindow(_THIS, SDL_Window * window) { } else { /* Queue page flip at vsync */ - /* Have we already setup the CRTC to one of the GBM buffers? Do so if we have not, + /* Have we already setup the CRTC to one of the GBM buffers? Do so if we have not, or FlipPage won't work in some cases. */ - if (!wdata->crtc_ready) { + if (!wdata->crtc_ready) { if(!KMSDRM_GLES_SetupCrtc(_this, window)) { SDL_LogError(SDL_LOG_CATEGORY_VIDEO, "Could not set up CRTC for doing vsync-ed pageflips"); return 0; } - } + } /* SDL_LogDebug(SDL_LOG_CATEGORY_VIDEO, "drmModePageFlip(%d, %u, %u, DRM_MODE_PAGE_FLIP_EVENT, &wdata->waiting_for_flip)", vdata->drm_fd, displaydata->crtc_id, fb_info->fb_id); */ diff --git a/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmvideo.c b/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmvideo.c index 7855eeddb..bacbe0c1e 100644 --- a/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmvideo.c +++ b/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmvideo.c @@ -41,20 +41,27 @@ #include "SDL_kmsdrmopengles.h" #include "SDL_kmsdrmmouse.h" #include "SDL_kmsdrmdyn.h" +#include +#include +#include -#define KMSDRM_DRI_CARD_0 "/dev/dri/card0" +#define KMSDRM_DRI_PATH "/dev/dri/" static int -KMSDRM_Available(void) +check_modestting(int devindex) { - int available = 0; + SDL_bool available = SDL_FALSE; + char device[512]; + int drm_fd; - int drm_fd = open(KMSDRM_DRI_CARD_0, O_RDWR | O_CLOEXEC); + SDL_snprintf(device, sizeof (device), "%scard%d", KMSDRM_DRI_PATH, devindex); + + drm_fd = open(device, O_RDWR | O_CLOEXEC); if (drm_fd >= 0) { if (SDL_KMSDRM_LoadSymbols()) { drmModeRes *resources = KMSDRM_drmModeGetResources(drm_fd); if (resources != NULL) { - available = 1; + available = SDL_TRUE; KMSDRM_drmModeFreeResources(resources); } SDL_KMSDRM_UnloadSymbols(); @@ -65,6 +72,66 @@ KMSDRM_Available(void) return available; } +static int get_dricount(void) +{ + int devcount = 0; + struct dirent *res; + struct stat sb; + DIR *folder; + + if (!(stat(KMSDRM_DRI_PATH, &sb) == 0 + && S_ISDIR(sb.st_mode))) { + printf("The path %s cannot be opened or is not available\n", + KMSDRM_DRI_PATH); + return 0; + } + + if (access(KMSDRM_DRI_PATH, F_OK) == -1) { + printf("The path %s cannot be opened\n", + KMSDRM_DRI_PATH); + return 0; + } + + folder = opendir(KMSDRM_DRI_PATH); + if (folder) { + while ((res = readdir(folder))) { + if (res->d_type == DT_CHR) { + devcount++; + } + } + closedir(folder); + } + + return devcount; +} + +static int +get_driindex(void) +{ + const int devcount = get_dricount(); + int i; + + for (i = 0; i < devcount; i++) { + if (check_modestting(i)) { + return i; + } + } + + return -ENOENT; +} + +static int +KMSDRM_Available(void) +{ + int ret = -ENOENT; + + ret = get_driindex(); + if (ret >= 0) + return 1; + + return ret; +} + static void KMSDRM_Destroy(SDL_VideoDevice * device) { @@ -83,7 +150,11 @@ KMSDRM_Create(int devindex) SDL_VideoDevice *device; SDL_VideoData *vdata; - if (devindex < 0 || devindex > 99) { + if (!devindex || (devindex > 99)) { + devindex = get_driindex(); + } + + if (devindex < 0) { SDL_SetError("devindex (%d) must be between 0 and 99.\n", devindex); return NULL; } @@ -566,6 +637,10 @@ KMSDRM_DestroyWindow(_THIS, SDL_Window * window) if(data) { /* Wait for any pending page flips and unlock buffer */ KMSDRM_WaitPageFlip(_this, data, -1); + if (data->crtc_bo != NULL) { + KMSDRM_gbm_surface_release_buffer(data->gs, data->crtc_bo); + data->crtc_bo = NULL; + } if (data->next_bo != NULL) { KMSDRM_gbm_surface_release_buffer(data->gs, data->next_bo); data->next_bo = NULL; diff --git a/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmvideo.h b/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmvideo.h index 5f00f0ebc..34f0b105a 100644 --- a/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmvideo.h +++ b/Engine/lib/sdl/src/video/kmsdrm/SDL_kmsdrmvideo.h @@ -62,6 +62,7 @@ typedef struct SDL_WindowData struct gbm_surface *gs; struct gbm_bo *current_bo; struct gbm_bo *next_bo; + struct gbm_bo *crtc_bo; SDL_bool waiting_for_flip; SDL_bool crtc_ready; SDL_bool double_buffer; diff --git a/Engine/lib/sdl/src/video/raspberry/SDL_rpivideo.c b/Engine/lib/sdl/src/video/raspberry/SDL_rpivideo.c index e3863803a..c4f4a6069 100644 --- a/Engine/lib/sdl/src/video/raspberry/SDL_rpivideo.c +++ b/Engine/lib/sdl/src/video/raspberry/SDL_rpivideo.c @@ -343,17 +343,17 @@ RPI_DestroyWindow(_THIS, SDL_Window * window) SDL_DisplayData *displaydata = (SDL_DisplayData *) display->driverdata; if(data) { - if (data->double_buffer) { - /* Wait for vsync, and then stop vsync callbacks and destroy related stuff, if needed */ - SDL_LockMutex(data->vsync_cond_mutex); - SDL_CondWait(data->vsync_cond, data->vsync_cond_mutex); - SDL_UnlockMutex(data->vsync_cond_mutex); + if (data->double_buffer) { + /* Wait for vsync, and then stop vsync callbacks and destroy related stuff, if needed */ + SDL_LockMutex(data->vsync_cond_mutex); + SDL_CondWait(data->vsync_cond, data->vsync_cond_mutex); + SDL_UnlockMutex(data->vsync_cond_mutex); - vc_dispmanx_vsync_callback(displaydata->dispman_display, NULL, NULL); + vc_dispmanx_vsync_callback(displaydata->dispman_display, NULL, NULL); - SDL_DestroyCond(data->vsync_cond); - SDL_DestroyMutex(data->vsync_cond_mutex); - } + SDL_DestroyCond(data->vsync_cond); + SDL_DestroyMutex(data->vsync_cond_mutex); + } #if SDL_VIDEO_OPENGL_EGL if (data->egl_surface != EGL_NO_SURFACE) { diff --git a/Engine/lib/sdl/src/video/uikit/SDL_uikitappdelegate.m b/Engine/lib/sdl/src/video/uikit/SDL_uikitappdelegate.m index a942dfde0..15762d2bf 100644 --- a/Engine/lib/sdl/src/video/uikit/SDL_uikitappdelegate.m +++ b/Engine/lib/sdl/src/video/uikit/SDL_uikitappdelegate.m @@ -445,30 +445,7 @@ SDL_LoadLaunchImageNamed(NSString *name, int screenh) #if !TARGET_OS_TV - (void)application:(UIApplication *)application didChangeStatusBarOrientation:(UIInterfaceOrientation)oldStatusBarOrientation { - BOOL isLandscape = UIInterfaceOrientationIsLandscape(application.statusBarOrientation); - SDL_VideoDevice *_this = SDL_GetVideoDevice(); - - if (_this && _this->num_displays > 0) { - SDL_DisplayMode *desktopmode = &_this->displays[0].desktop_mode; - SDL_DisplayMode *currentmode = &_this->displays[0].current_mode; - - /* The desktop display mode should be kept in sync with the screen - * orientation so that updating a window's fullscreen state to - * SDL_WINDOW_FULLSCREEN_DESKTOP keeps the window dimensions in the - * correct orientation. */ - if (isLandscape != (desktopmode->w > desktopmode->h)) { - int height = desktopmode->w; - desktopmode->w = desktopmode->h; - desktopmode->h = height; - } - - /* Same deal with the current mode + SDL_GetCurrentDisplayMode. */ - if (isLandscape != (currentmode->w > currentmode->h)) { - int height = currentmode->w; - currentmode->w = currentmode->h; - currentmode->h = height; - } - } + SDL_OnApplicationDidChangeStatusBarOrientation(); } #endif diff --git a/Engine/lib/sdl/src/video/uikit/SDL_uikitmessagebox.m b/Engine/lib/sdl/src/video/uikit/SDL_uikitmessagebox.m index 7950c8e69..cf2a8f345 100644 --- a/Engine/lib/sdl/src/video/uikit/SDL_uikitmessagebox.m +++ b/Engine/lib/sdl/src/video/uikit/SDL_uikitmessagebox.m @@ -109,6 +109,7 @@ UIKit_ShowMessageBoxAlertController(const SDL_MessageBoxData *messageboxdata, in alertwindow.hidden = YES; } +#if !TARGET_OS_TV /* Force the main SDL window to re-evaluate home indicator state */ SDL_Window *focus = SDL_GetFocusWindow(); if (focus) { @@ -120,6 +121,7 @@ UIKit_ShowMessageBoxAlertController(const SDL_MessageBoxData *messageboxdata, in } } } +#endif /* !TARGET_OS_TV */ *buttonid = messageboxdata->buttons[clickedindex].buttonid; return YES; diff --git a/Engine/lib/sdl/src/video/uikit/SDL_uikitmetalview.m b/Engine/lib/sdl/src/video/uikit/SDL_uikitmetalview.m index 104189d8c..436e7425a 100644 --- a/Engine/lib/sdl/src/video/uikit/SDL_uikitmetalview.m +++ b/Engine/lib/sdl/src/video/uikit/SDL_uikitmetalview.m @@ -49,9 +49,8 @@ { if ((self = [super initWithFrame:frame])) { self.tag = METALVIEW_TAG; - /* Set the desired scale. The default drawableSize of a CAMetalLayer - * is its bounds x its scale so nothing further needs to be done. */ self.layer.contentsScale = scale; + [self updateDrawableSize]; } return self; @@ -61,6 +60,15 @@ - (void)layoutSubviews { [super layoutSubviews]; + [self updateDrawableSize]; +} + +- (void)updateDrawableSize +{ + CGSize size = self.bounds.size; + size.width *= self.layer.contentsScale; + size.height *= self.layer.contentsScale; + ((CAMetalLayer *)self.layer).drawableSize = size; } @end @@ -72,9 +80,9 @@ UIKit_Mtl_AddMetalView(SDL_Window* window) SDL_uikitview *view = (SDL_uikitview*)data.uiwindow.rootViewController.view; CGFloat scale = 1.0; - if ([view isKindOfClass:[SDL_uikitmetalview class]]) { - return (SDL_uikitmetalview *)view; - } + if ([view isKindOfClass:[SDL_uikitmetalview class]]) { + return (SDL_uikitmetalview *)view; + } if (window->flags & SDL_WINDOW_ALLOW_HIGHDPI) { /* Set the scale to the natural scale factor of the screen - then diff --git a/Engine/lib/sdl/src/video/uikit/SDL_uikitmodes.h b/Engine/lib/sdl/src/video/uikit/SDL_uikitmodes.h index a1df0d496..b5c0c650c 100644 --- a/Engine/lib/sdl/src/video/uikit/SDL_uikitmodes.h +++ b/Engine/lib/sdl/src/video/uikit/SDL_uikitmodes.h @@ -45,6 +45,10 @@ extern int UIKit_SetDisplayMode(_THIS, SDL_VideoDisplay * display, SDL_DisplayMo extern void UIKit_QuitModes(_THIS); extern int UIKit_GetDisplayUsableBounds(_THIS, SDL_VideoDisplay * display, SDL_Rect * rect); +#if !TARGET_OS_TV +extern void SDL_OnApplicationDidChangeStatusBarOrientation(void); +#endif + #endif /* SDL_uikitmodes_h_ */ /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/uikit/SDL_uikitmodes.m b/Engine/lib/sdl/src/video/uikit/SDL_uikitmodes.m index 75e256bbf..7ddf1078a 100644 --- a/Engine/lib/sdl/src/video/uikit/SDL_uikitmodes.m +++ b/Engine/lib/sdl/src/video/uikit/SDL_uikitmodes.m @@ -25,6 +25,8 @@ #include "SDL_assert.h" #include "SDL_uikitmodes.h" +#include "../../events/SDL_events_c.h" + @implementation SDL_DisplayData @synthesize uiscreen; @@ -188,6 +190,9 @@ UIKit_InitModes(_THIS) return -1; } } +#if !TARGET_OS_TV + SDL_OnApplicationDidChangeStatusBarOrientation(); +#endif } return 0; @@ -319,6 +324,57 @@ UIKit_QuitModes(_THIS) } } +#if !TARGET_OS_TV +void SDL_OnApplicationDidChangeStatusBarOrientation() +{ + BOOL isLandscape = UIInterfaceOrientationIsLandscape([UIApplication sharedApplication].statusBarOrientation); + SDL_VideoDisplay *display = SDL_GetDisplay(0); + + if (display) { + SDL_DisplayMode *desktopmode = &display->desktop_mode; + SDL_DisplayMode *currentmode = &display->current_mode; + SDL_DisplayOrientation orientation = SDL_ORIENTATION_UNKNOWN; + + /* The desktop display mode should be kept in sync with the screen + * orientation so that updating a window's fullscreen state to + * SDL_WINDOW_FULLSCREEN_DESKTOP keeps the window dimensions in the + * correct orientation. */ + if (isLandscape != (desktopmode->w > desktopmode->h)) { + int height = desktopmode->w; + desktopmode->w = desktopmode->h; + desktopmode->h = height; + } + + /* Same deal with the current mode + SDL_GetCurrentDisplayMode. */ + if (isLandscape != (currentmode->w > currentmode->h)) { + int height = currentmode->w; + currentmode->w = currentmode->h; + currentmode->h = height; + } + + switch ([UIApplication sharedApplication].statusBarOrientation) { + case UIInterfaceOrientationPortrait: + orientation = SDL_ORIENTATION_PORTRAIT; + break; + case UIInterfaceOrientationPortraitUpsideDown: + orientation = SDL_ORIENTATION_PORTRAIT_FLIPPED; + break; + case UIInterfaceOrientationLandscapeLeft: + /* Bug: UIInterfaceOrientationLandscapeLeft/Right are reversed - http://openradar.appspot.com/7216046 */ + orientation = SDL_ORIENTATION_LANDSCAPE_FLIPPED; + break; + case UIInterfaceOrientationLandscapeRight: + /* Bug: UIInterfaceOrientationLandscapeLeft/Right are reversed - http://openradar.appspot.com/7216046 */ + orientation = SDL_ORIENTATION_LANDSCAPE; + break; + default: + break; + } + SDL_SendDisplayEvent(display, SDL_DISPLAYEVENT_ORIENTATION, orientation); + } +} +#endif /* !TARGET_OS_TV */ + #endif /* SDL_VIDEO_DRIVER_UIKIT */ /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/uikit/SDL_uikitopenglview.m b/Engine/lib/sdl/src/video/uikit/SDL_uikitopenglview.m index b0628bfdb..902437632 100644 --- a/Engine/lib/sdl/src/video/uikit/SDL_uikitopenglview.m +++ b/Engine/lib/sdl/src/video/uikit/SDL_uikitopenglview.m @@ -101,7 +101,7 @@ SDL_SetError("sRGB drawables are not supported."); return nil; } - } else if (rBits >= 8 || gBits >= 8 || bBits >= 8) { + } else if (rBits >= 8 || gBits >= 8 || bBits >= 8 || aBits > 0) { /* if user specifically requests rbg888 or some color format higher than 16bpp */ colorFormat = kEAGLColorFormatRGBA8; colorBufferFormat = GL_RGBA8; diff --git a/Engine/lib/sdl/src/video/uikit/SDL_uikitvideo.m b/Engine/lib/sdl/src/video/uikit/SDL_uikitvideo.m index e74339f42..10d1dde31 100644 --- a/Engine/lib/sdl/src/video/uikit/SDL_uikitvideo.m +++ b/Engine/lib/sdl/src/video/uikit/SDL_uikitvideo.m @@ -233,6 +233,17 @@ void SDL_NSLog(const char *text) NSLog(@"%s", text); } +/* + * iOS Tablet detection + * + * This doesn't really have aything to do with the interfaces of the SDL video + * subsystem, but we need to stuff this into an Objective-C source code file. + */ +SDL_bool SDL_IsIPad(void) +{ + return (UI_USER_INTERFACE_IDIOM() == UIUserInterfaceIdiomPad); +} + #endif /* SDL_VIDEO_DRIVER_UIKIT */ /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/uikit/SDL_uikitview.m b/Engine/lib/sdl/src/video/uikit/SDL_uikitview.m index bd60c552f..caabfac07 100644 --- a/Engine/lib/sdl/src/video/uikit/SDL_uikitview.m +++ b/Engine/lib/sdl/src/video/uikit/SDL_uikitview.m @@ -252,34 +252,34 @@ extern int SDL_AppleTVRemoteOpenedAsJoystick; - (void)pressesBegan:(NSSet *)presses withEvent:(UIPressesEvent *)event { - if (!SDL_AppleTVRemoteOpenedAsJoystick) { - for (UIPress *press in presses) { - SDL_Scancode scancode = [self scancodeFromPressType:press.type]; - SDL_SendKeyboardKey(SDL_PRESSED, scancode); - } - } + if (!SDL_AppleTVRemoteOpenedAsJoystick) { + for (UIPress *press in presses) { + SDL_Scancode scancode = [self scancodeFromPressType:press.type]; + SDL_SendKeyboardKey(SDL_PRESSED, scancode); + } + } [super pressesBegan:presses withEvent:event]; } - (void)pressesEnded:(NSSet *)presses withEvent:(UIPressesEvent *)event { - if (!SDL_AppleTVRemoteOpenedAsJoystick) { - for (UIPress *press in presses) { - SDL_Scancode scancode = [self scancodeFromPressType:press.type]; - SDL_SendKeyboardKey(SDL_RELEASED, scancode); - } - } + if (!SDL_AppleTVRemoteOpenedAsJoystick) { + for (UIPress *press in presses) { + SDL_Scancode scancode = [self scancodeFromPressType:press.type]; + SDL_SendKeyboardKey(SDL_RELEASED, scancode); + } + } [super pressesEnded:presses withEvent:event]; } - (void)pressesCancelled:(NSSet *)presses withEvent:(UIPressesEvent *)event { - if (!SDL_AppleTVRemoteOpenedAsJoystick) { - for (UIPress *press in presses) { - SDL_Scancode scancode = [self scancodeFromPressType:press.type]; - SDL_SendKeyboardKey(SDL_RELEASED, scancode); - } - } + if (!SDL_AppleTVRemoteOpenedAsJoystick) { + for (UIPress *press in presses) { + SDL_Scancode scancode = [self scancodeFromPressType:press.type]; + SDL_SendKeyboardKey(SDL_RELEASED, scancode); + } + } [super pressesCancelled:presses withEvent:event]; } diff --git a/Engine/lib/sdl/src/video/uikit/SDL_uikitviewcontroller.m b/Engine/lib/sdl/src/video/uikit/SDL_uikitviewcontroller.m index 296274235..49a39b6bc 100644 --- a/Engine/lib/sdl/src/video/uikit/SDL_uikitviewcontroller.m +++ b/Engine/lib/sdl/src/video/uikit/SDL_uikitviewcontroller.m @@ -74,6 +74,8 @@ SDL_HideHomeIndicatorHintChanged(void *userdata, const char *name, const char *o #if SDL_IPHONE_KEYBOARD UITextField *textField; BOOL rotatingOrientation; + NSString *changeText; + NSString *obligateForBackspace; #endif } @@ -250,10 +252,12 @@ SDL_HideHomeIndicatorHintChanged(void *userdata, const char *name, const char *o /* Set ourselves up as a UITextFieldDelegate */ - (void)initKeyboard { + changeText = nil; + obligateForBackspace = @" "; /* 64 space */ textField = [[UITextField alloc] initWithFrame:CGRectZero]; textField.delegate = self; /* placeholder so there is something to delete! */ - textField.text = @" "; + textField.text = obligateForBackspace; /* set UITextInputTrait properties, mostly to defaults */ textField.autocapitalizationType = UITextAutocapitalizationTypeNone; @@ -267,11 +271,12 @@ SDL_HideHomeIndicatorHintChanged(void *userdata, const char *name, const char *o textField.hidden = YES; keyboardVisible = NO; -#if !TARGET_OS_TV NSNotificationCenter *center = [NSNotificationCenter defaultCenter]; +#if !TARGET_OS_TV [center addObserver:self selector:@selector(keyboardWillShow:) name:UIKeyboardWillShowNotification object:nil]; [center addObserver:self selector:@selector(keyboardWillHide:) name:UIKeyboardWillHideNotification object:nil]; #endif + [center addObserver:self selector:@selector(textFieldTextDidChange:) name:UITextFieldTextDidChangeNotification object:nil]; } - (void)setView:(UIView *)view @@ -310,11 +315,12 @@ SDL_HideHomeIndicatorHintChanged(void *userdata, const char *name, const char *o - (void)deinitKeyboard { -#if !TARGET_OS_TV NSNotificationCenter *center = [NSNotificationCenter defaultCenter]; +#if !TARGET_OS_TV [center removeObserver:self name:UIKeyboardWillShowNotification object:nil]; [center removeObserver:self name:UIKeyboardWillHideNotification object:nil]; #endif + [center removeObserver:self name:UITextFieldTextDidChangeNotification object:nil]; } /* reveal onscreen virtual keyboard */ @@ -354,6 +360,50 @@ SDL_HideHomeIndicatorHintChanged(void *userdata, const char *name, const char *o [self setKeyboardHeight:0]; } +- (void)textFieldTextDidChange:(NSNotification *)notification +{ + if (changeText!=nil && textField.markedTextRange == nil) + { + NSUInteger len = changeText.length; + if (len > 0) { + /* Go through all the characters in the string we've been sent and + * convert them to key presses */ + int i; + for (i = 0; i < len; i++) { + unichar c = [changeText characterAtIndex:i]; + SDL_Scancode code; + Uint16 mod; + + if (c < 127) { + /* Figure out the SDL_Scancode and SDL_keymod for this unichar */ + code = unicharToUIKeyInfoTable[c].code; + mod = unicharToUIKeyInfoTable[c].mod; + } else { + /* We only deal with ASCII right now */ + code = SDL_SCANCODE_UNKNOWN; + mod = 0; + } + + if (mod & KMOD_SHIFT) { + /* If character uses shift, press shift down */ + SDL_SendKeyboardKey(SDL_PRESSED, SDL_SCANCODE_LSHIFT); + } + + /* send a keydown and keyup even for the character */ + SDL_SendKeyboardKey(SDL_PRESSED, code); + SDL_SendKeyboardKey(SDL_RELEASED, code); + + if (mod & KMOD_SHIFT) { + /* If character uses shift, press shift back up */ + SDL_SendKeyboardKey(SDL_RELEASED, SDL_SCANCODE_LSHIFT); + } + } + SDL_SendKeyboardText([changeText UTF8String]); + } + changeText = nil; + } +} + - (void)updateKeyboard { CGAffineTransform t = self.view.transform; @@ -392,49 +442,20 @@ SDL_HideHomeIndicatorHintChanged(void *userdata, const char *name, const char *o - (BOOL)textField:(UITextField *)_textField shouldChangeCharactersInRange:(NSRange)range replacementString:(NSString *)string { NSUInteger len = string.length; - if (len == 0) { - /* it wants to replace text with nothing, ie a delete */ - SDL_SendKeyboardKey(SDL_PRESSED, SDL_SCANCODE_BACKSPACE); - SDL_SendKeyboardKey(SDL_RELEASED, SDL_SCANCODE_BACKSPACE); - } else { - /* go through all the characters in the string we've been sent and - * convert them to key presses */ - int i; - for (i = 0; i < len; i++) { - unichar c = [string characterAtIndex:i]; - Uint16 mod = 0; - SDL_Scancode code; - - if (c < 127) { - /* figure out the SDL_Scancode and SDL_keymod for this unichar */ - code = unicharToUIKeyInfoTable[c].code; - mod = unicharToUIKeyInfoTable[c].mod; - } else { - /* we only deal with ASCII right now */ - code = SDL_SCANCODE_UNKNOWN; - mod = 0; - } - - if (mod & KMOD_SHIFT) { - /* If character uses shift, press shift down */ - SDL_SendKeyboardKey(SDL_PRESSED, SDL_SCANCODE_LSHIFT); - } - - /* send a keydown and keyup even for the character */ - SDL_SendKeyboardKey(SDL_PRESSED, code); - SDL_SendKeyboardKey(SDL_RELEASED, code); - - if (mod & KMOD_SHIFT) { - /* If character uses shift, press shift back up */ - SDL_SendKeyboardKey(SDL_RELEASED, SDL_SCANCODE_LSHIFT); - } + changeText = nil; + if (textField.markedTextRange == nil) { + /* it wants to replace text with nothing, ie a delete */ + SDL_SendKeyboardKey(SDL_PRESSED, SDL_SCANCODE_BACKSPACE); + SDL_SendKeyboardKey(SDL_RELEASED, SDL_SCANCODE_BACKSPACE); } - - SDL_SendKeyboardText([string UTF8String]); + if (textField.text.length < 16) { + textField.text = obligateForBackspace; + } + } else { + changeText = string; } - - return NO; /* don't allow the edit! (keep placeholder text there) */ + return YES; } /* Terminates the editing session */ @@ -498,7 +519,7 @@ UIKit_IsScreenKeyboardShown(_THIS, SDL_Window *window) @autoreleasepool { SDL_uikitviewcontroller *vc = GetWindowViewController(window); if (vc != nil) { - return vc.isKeyboardVisible; + return vc.keyboardVisible; } return SDL_FALSE; } diff --git a/Engine/lib/sdl/src/video/wayland/SDL_waylandevents.c b/Engine/lib/sdl/src/video/wayland/SDL_waylandevents.c index 53453a239..0c953a54c 100644 --- a/Engine/lib/sdl/src/video/wayland/SDL_waylandevents.c +++ b/Engine/lib/sdl/src/video/wayland/SDL_waylandevents.c @@ -40,6 +40,7 @@ #include "pointer-constraints-unstable-v1-client-protocol.h" #include "relative-pointer-unstable-v1-client-protocol.h" +#include "xdg-shell-client-protocol.h" #include "xdg-shell-unstable-v6-client-protocol.h" #include @@ -177,6 +178,8 @@ Wayland_PumpEvents(_THIS) { SDL_VideoData *d = _this->driverdata; + WAYLAND_wl_display_flush(d->display); + if (SDL_IOReady(WAYLAND_wl_display_get_fd(d->display), SDL_FALSE, 0)) { WAYLAND_wl_display_dispatch(d->display); } @@ -263,7 +266,9 @@ ProcessHitTest(struct SDL_WaylandInput *input, uint32_t serial) switch (rc) { case SDL_HITTEST_DRAGGABLE: - if (input->display->shell.zxdg) { + if (input->display->shell.xdg) { + xdg_toplevel_move(window_data->shell_surface.xdg.roleobj.toplevel, input->seat, serial); + } else if (input->display->shell.zxdg) { zxdg_toplevel_v6_move(window_data->shell_surface.zxdg.roleobj.toplevel, input->seat, serial); } else { wl_shell_surface_move(window_data->shell_surface.wl, input->seat, serial); @@ -278,7 +283,9 @@ ProcessHitTest(struct SDL_WaylandInput *input, uint32_t serial) case SDL_HITTEST_RESIZE_BOTTOM: case SDL_HITTEST_RESIZE_BOTTOMLEFT: case SDL_HITTEST_RESIZE_LEFT: - if (input->display->shell.zxdg) { + if (input->display->shell.xdg) { + xdg_toplevel_resize(window_data->shell_surface.xdg.roleobj.toplevel, input->seat, serial, directions_zxdg[rc - SDL_HITTEST_RESIZE_TOPLEFT]); + } else if (input->display->shell.zxdg) { zxdg_toplevel_v6_resize(window_data->shell_surface.zxdg.roleobj.toplevel, input->seat, serial, directions_zxdg[rc - SDL_HITTEST_RESIZE_TOPLEFT]); } else { wl_shell_surface_resize(window_data->shell_surface.wl, input->seat, serial, directions_wl[rc - SDL_HITTEST_RESIZE_TOPLEFT]); @@ -742,7 +749,7 @@ static const struct wl_data_offer_listener data_offer_listener = { static void data_device_handle_data_offer(void *data, struct wl_data_device *wl_data_device, - struct wl_data_offer *id) + struct wl_data_offer *id) { SDL_WaylandDataOffer *data_offer = NULL; @@ -760,7 +767,7 @@ data_device_handle_data_offer(void *data, struct wl_data_device *wl_data_device, static void data_device_handle_enter(void *data, struct wl_data_device *wl_data_device, - uint32_t serial, struct wl_surface *surface, + uint32_t serial, struct wl_surface *surface, wl_fixed_t x, wl_fixed_t y, struct wl_data_offer *id) { SDL_WaylandDataDevice *data_device = data; @@ -803,7 +810,7 @@ data_device_handle_leave(void *data, struct wl_data_device *wl_data_device) static void data_device_handle_motion(void *data, struct wl_data_device *wl_data_device, - uint32_t time, wl_fixed_t x, wl_fixed_t y) + uint32_t time, wl_fixed_t x, wl_fixed_t y) { } @@ -842,7 +849,7 @@ data_device_handle_drop(void *data, struct wl_data_device *wl_data_device) static void data_device_handle_selection(void *data, struct wl_data_device *wl_data_device, - struct wl_data_offer *id) + struct wl_data_offer *id) { SDL_WaylandDataDevice *data_device = data; SDL_WaylandDataOffer *offer = NULL; diff --git a/Engine/lib/sdl/src/video/wayland/SDL_waylandtouch.c b/Engine/lib/sdl/src/video/wayland/SDL_waylandtouch.c index 005b47f58..1cf37c1a2 100644 --- a/Engine/lib/sdl/src/video/wayland/SDL_waylandtouch.c +++ b/Engine/lib/sdl/src/video/wayland/SDL_waylandtouch.c @@ -89,9 +89,9 @@ touch_handle_touch(void *data, */ SDL_TouchID deviceId = 1; - if (SDL_AddTouch(deviceId, "qt_touch_extension") < 0) { - SDL_Log("error: can't add touch %s, %d", __FILE__, __LINE__); - } + if (SDL_AddTouch(deviceId, "qt_touch_extension") < 0) { + SDL_Log("error: can't add touch %s, %d", __FILE__, __LINE__); + } switch (touchState) { case QtWaylandTouchPointPressed: diff --git a/Engine/lib/sdl/src/video/wayland/SDL_waylandtouch.h b/Engine/lib/sdl/src/video/wayland/SDL_waylandtouch.h index 9efc5a54a..eba0da80d 100644 --- a/Engine/lib/sdl/src/video/wayland/SDL_waylandtouch.h +++ b/Engine/lib/sdl/src/video/wayland/SDL_waylandtouch.h @@ -19,13 +19,13 @@ 3. This notice may not be removed or altered from any source distribution. */ +#ifndef SDL_waylandtouch_h_ +#define SDL_waylandtouch_h_ + #include "../../SDL_internal.h" #ifdef SDL_VIDEO_DRIVER_WAYLAND_QT_TOUCH -#ifndef SDL_waylandtouch_h_ -#define SDL_waylandtouch_h_ - #include "SDL_waylandvideo.h" #include #include @@ -347,6 +347,6 @@ qt_windowmanager_open_url(struct qt_windowmanager *qt_windowmanager, uint32_t re QT_WINDOWMANAGER_OPEN_URL, remaining, url); } -#endif /* SDL_waylandtouch_h_ */ - #endif /* SDL_VIDEO_DRIVER_WAYLAND_QT_TOUCH */ + +#endif /* SDL_waylandtouch_h_ */ diff --git a/Engine/lib/sdl/src/video/wayland/SDL_waylandvideo.c b/Engine/lib/sdl/src/video/wayland/SDL_waylandvideo.c index 8401a0884..b6155e76d 100644 --- a/Engine/lib/sdl/src/video/wayland/SDL_waylandvideo.c +++ b/Engine/lib/sdl/src/video/wayland/SDL_waylandvideo.c @@ -45,6 +45,7 @@ #include "SDL_waylanddyn.h" #include +#include "xdg-shell-client-protocol.h" #include "xdg-shell-unstable-v6-client-protocol.h" #define WAYLANDVID_DRIVER_NAME "wayland" @@ -67,10 +68,10 @@ static char * get_classname() { /* !!! FIXME: this is probably wrong, albeit harmless in many common cases. From protocol spec: - "The surface class identifies the general class of applications - to which the surface belongs. A common convention is to use the - file name (or the full path if it is a non-standard location) of - the application's .desktop file as the class." */ + "The surface class identifies the general class of applications + to which the surface belongs. A common convention is to use the + file name (or the full path if it is a non-standard location) of + the application's .desktop file as the class." */ char *spot; #if defined(__LINUX__) || defined(__FREEBSD__) @@ -327,6 +328,17 @@ static const struct zxdg_shell_v6_listener shell_listener_zxdg = { }; +static void +handle_ping_xdg_wm_base(void *data, struct xdg_wm_base *xdg, uint32_t serial) +{ + xdg_wm_base_pong(xdg, serial); +} + +static const struct xdg_wm_base_listener shell_listener_xdg = { + handle_ping_xdg_wm_base +}; + + static void display_handle_global(void *data, struct wl_registry *registry, uint32_t id, const char *interface, uint32_t version) @@ -339,6 +351,9 @@ display_handle_global(void *data, struct wl_registry *registry, uint32_t id, Wayland_add_display(d, id); } else if (strcmp(interface, "wl_seat") == 0) { Wayland_display_add_input(d, id); + } else if (strcmp(interface, "xdg_wm_base") == 0) { + d->shell.xdg = wl_registry_bind(d->registry, id, &xdg_wm_base_interface, 1); + xdg_wm_base_add_listener(d->shell.xdg, &shell_listener_xdg, NULL); } else if (strcmp(interface, "zxdg_shell_v6") == 0) { d->shell.zxdg = wl_registry_bind(d->registry, id, &zxdg_shell_v6_interface, 1); zxdg_shell_v6_add_listener(d->shell.zxdg, &shell_listener_zxdg, NULL); @@ -475,6 +490,9 @@ Wayland_VideoQuit(_THIS) if (data->shell.wl) wl_shell_destroy(data->shell.wl); + if (data->shell.xdg) + xdg_wm_base_destroy(data->shell.xdg); + if (data->shell.zxdg) zxdg_shell_v6_destroy(data->shell.zxdg); diff --git a/Engine/lib/sdl/src/video/wayland/SDL_waylandvideo.h b/Engine/lib/sdl/src/video/wayland/SDL_waylandvideo.h index 6ad68de48..c16c0bdd4 100644 --- a/Engine/lib/sdl/src/video/wayland/SDL_waylandvideo.h +++ b/Engine/lib/sdl/src/video/wayland/SDL_waylandvideo.h @@ -24,6 +24,16 @@ #ifndef SDL_waylandvideo_h_ #define SDL_waylandvideo_h_ + +/* +!!! FIXME: xdg_wm_base is the stable replacement for zxdg_shell_v6. While it's +!!! FIXME: harmless to leave it here, consider deleting the obsolete codepath +!!! FIXME: soon, since Wayland (with xdg_wm_base) will probably be mainline +!!! FIXME: by the time people are relying on this SDL target. It's available +!!! FIXME: in Ubuntu 18.04 (and other distros). +*/ + + #include #include "wayland-util.h" @@ -44,7 +54,7 @@ typedef struct { struct wl_cursor_theme *cursor_theme; struct wl_pointer *pointer; struct { - /* !!! FIXME: add stable xdg_shell from 1.12 */ + struct xdg_wm_base *xdg; struct zxdg_shell_v6 *zxdg; struct wl_shell *wl; } shell; diff --git a/Engine/lib/sdl/src/video/wayland/SDL_waylandwindow.c b/Engine/lib/sdl/src/video/wayland/SDL_waylandwindow.c index 684022a79..aa7299181 100644 --- a/Engine/lib/sdl/src/video/wayland/SDL_waylandwindow.c +++ b/Engine/lib/sdl/src/video/wayland/SDL_waylandwindow.c @@ -33,6 +33,7 @@ #include "SDL_waylanddyn.h" #include "SDL_hints.h" +#include "xdg-shell-client-protocol.h" #include "xdg-shell-unstable-v6-client-protocol.h" /* On modern desktops, we probably will use the xdg-shell protocol instead @@ -78,19 +79,15 @@ handle_configure_wl_shell_surface(void *data, struct wl_shell_surface *shell_sur } } - if (width == window->w && height == window->h) { - return; - } - - window->w = width; - window->h = height; - WAYLAND_wl_egl_window_resize(wind->egl_window, window->w, window->h, 0, 0); - + WAYLAND_wl_egl_window_resize(wind->egl_window, width, height, 0, 0); region = wl_compositor_create_region(wind->waylandData->compositor); - wl_region_add(region, 0, 0, window->w, window->h); + wl_region_add(region, 0, 0, width, height); wl_surface_set_opaque_region(wind->surface, region); wl_region_destroy(region); - SDL_SendWindowEvent(window, SDL_WINDOWEVENT_RESIZED, window->w, window->h); + + SDL_SendWindowEvent(window, SDL_WINDOWEVENT_RESIZED, width, height); + window->w = width; + window->h = height; } static void @@ -113,13 +110,15 @@ handle_configure_zxdg_shell_surface(void *data, struct zxdg_surface_v6 *zxdg, ui SDL_WindowData *wind = (SDL_WindowData *)data; SDL_Window *window = wind->sdlwindow; struct wl_region *region; + + wind->shell_surface.zxdg.initial_configure_seen = SDL_TRUE; + WAYLAND_wl_egl_window_resize(wind->egl_window, window->w, window->h, 0, 0); region = wl_compositor_create_region(wind->waylandData->compositor); wl_region_add(region, 0, 0, window->w, window->h); wl_surface_set_opaque_region(wind->surface, region); wl_region_destroy(region); - SDL_SendWindowEvent(window, SDL_WINDOWEVENT_RESIZED, window->w, window->h); zxdg_surface_v6_ack_configure(zxdg, serial); } @@ -130,10 +129,85 @@ static const struct zxdg_surface_v6_listener shell_surface_listener_zxdg = { static void handle_configure_zxdg_toplevel(void *data, - struct zxdg_toplevel_v6 *zxdg_toplevel_v6, - int32_t width, - int32_t height, - struct wl_array *states) + struct zxdg_toplevel_v6 *zxdg_toplevel_v6, + int32_t width, + int32_t height, + struct wl_array *states) +{ + SDL_WindowData *wind = (SDL_WindowData *)data; + SDL_Window *window = wind->sdlwindow; + + /* wl_shell_surface spec states that this is a suggestion. + Ignore if less than or greater than max/min size. */ + + if (width == 0 || height == 0) { + return; + } + + if (!(window->flags & SDL_WINDOW_FULLSCREEN)) { + if ((window->flags & SDL_WINDOW_RESIZABLE)) { + if (window->max_w > 0) { + width = SDL_min(width, window->max_w); + } + width = SDL_max(width, window->min_w); + + if (window->max_h > 0) { + height = SDL_min(height, window->max_h); + } + height = SDL_max(height, window->min_h); + } else { + return; + } + } + + SDL_SendWindowEvent(window, SDL_WINDOWEVENT_RESIZED, width, height); + window->w = width; + window->h = height; +} + +static void +handle_close_zxdg_toplevel(void *data, struct zxdg_toplevel_v6 *zxdg_toplevel_v6) +{ + SDL_WindowData *window = (SDL_WindowData *)data; + SDL_SendWindowEvent(window->sdlwindow, SDL_WINDOWEVENT_CLOSE, 0, 0); +} + +static const struct zxdg_toplevel_v6_listener toplevel_listener_zxdg = { + handle_configure_zxdg_toplevel, + handle_close_zxdg_toplevel +}; + + + +static void +handle_configure_xdg_shell_surface(void *data, struct xdg_surface *xdg, uint32_t serial) +{ + SDL_WindowData *wind = (SDL_WindowData *)data; + SDL_Window *window = wind->sdlwindow; + struct wl_region *region; + + wind->shell_surface.xdg.initial_configure_seen = SDL_TRUE; + + WAYLAND_wl_egl_window_resize(wind->egl_window, window->w, window->h, 0, 0); + + region = wl_compositor_create_region(wind->waylandData->compositor); + wl_region_add(region, 0, 0, window->w, window->h); + wl_surface_set_opaque_region(wind->surface, region); + wl_region_destroy(region); + xdg_surface_ack_configure(xdg, serial); +} + +static const struct xdg_surface_listener shell_surface_listener_xdg = { + handle_configure_xdg_shell_surface +}; + + +static void +handle_configure_xdg_toplevel(void *data, + struct xdg_toplevel *xdg_toplevel, + int32_t width, + int32_t height, + struct wl_array *states) { SDL_WindowData *wind = (SDL_WindowData *)data; SDL_Window *window = wind->sdlwindow; @@ -165,23 +239,26 @@ handle_configure_zxdg_toplevel(void *data, return; } + SDL_SendWindowEvent(window, SDL_WINDOWEVENT_RESIZED, width, height); window->w = width; window->h = height; } static void -handle_close_zxdg_toplevel(void *data, struct zxdg_toplevel_v6 *zxdg_toplevel_v6) +handle_close_xdg_toplevel(void *data, struct xdg_toplevel *xdg_toplevel) { SDL_WindowData *window = (SDL_WindowData *)data; SDL_SendWindowEvent(window->sdlwindow, SDL_WINDOWEVENT_CLOSE, 0, 0); } -static const struct zxdg_toplevel_v6_listener toplevel_listener_zxdg = { - handle_configure_zxdg_toplevel, - handle_close_zxdg_toplevel +static const struct xdg_toplevel_listener toplevel_listener_xdg = { + handle_configure_xdg_toplevel, + handle_close_xdg_toplevel }; + + #ifdef SDL_VIDEO_DRIVER_WAYLAND_QT_TOUCH static void handle_onscreen_visibility(void *data, @@ -254,7 +331,13 @@ SetFullscreen(_THIS, SDL_Window * window, struct wl_output *output) const SDL_VideoData *viddata = (const SDL_VideoData *) _this->driverdata; SDL_WindowData *wind = window->driverdata; - if (viddata->shell.zxdg) { + if (viddata->shell.xdg) { + if (output) { + xdg_toplevel_set_fullscreen(wind->shell_surface.xdg.roleobj.toplevel, output); + } else { + xdg_toplevel_unset_fullscreen(wind->shell_surface.xdg.roleobj.toplevel); + } + } else if (viddata->shell.zxdg) { if (output) { zxdg_toplevel_v6_set_fullscreen(wind->shell_surface.zxdg.roleobj.toplevel, output); } else { @@ -359,7 +442,8 @@ Wayland_RestoreWindow(_THIS, SDL_Window * window) SDL_WindowData *wind = window->driverdata; const SDL_VideoData *viddata = (const SDL_VideoData *) _this->driverdata; - if (viddata->shell.zxdg) { + if (viddata->shell.xdg) { + } else if (viddata->shell.zxdg) { } else { wl_shell_surface_set_toplevel(wind->shell_surface.wl); } @@ -373,7 +457,9 @@ Wayland_MaximizeWindow(_THIS, SDL_Window * window) SDL_WindowData *wind = window->driverdata; SDL_VideoData *viddata = (SDL_VideoData *) _this->driverdata; - if (viddata->shell.zxdg) { + if (viddata->shell.xdg) { + xdg_toplevel_set_maximized(wind->shell_surface.xdg.roleobj.toplevel); + } else if (viddata->shell.zxdg) { zxdg_toplevel_v6_set_maximized(wind->shell_surface.zxdg.roleobj.toplevel); } else { wl_shell_surface_set_maximized(wind->shell_surface.wl, NULL); @@ -414,7 +500,13 @@ int Wayland_CreateWindow(_THIS, SDL_Window *window) wl_compositor_create_surface(c->compositor); wl_surface_set_user_data(data->surface, data); - if (c->shell.zxdg) { + if (c->shell.xdg) { + data->shell_surface.xdg.surface = xdg_wm_base_get_xdg_surface(c->shell.xdg, data->surface); + /* !!! FIXME: add popup role */ + data->shell_surface.xdg.roleobj.toplevel = xdg_surface_get_toplevel(data->shell_surface.xdg.surface); + xdg_toplevel_add_listener(data->shell_surface.xdg.roleobj.toplevel, &toplevel_listener_xdg, data); + xdg_toplevel_set_app_id(data->shell_surface.xdg.roleobj.toplevel, c->classname); + } else if (c->shell.zxdg) { data->shell_surface.zxdg.surface = zxdg_shell_v6_get_xdg_surface(c->shell.zxdg, data->surface); /* !!! FIXME: add popup role */ data->shell_surface.zxdg.roleobj.toplevel = zxdg_surface_v6_get_toplevel(data->shell_surface.zxdg.surface); @@ -445,7 +537,12 @@ int Wayland_CreateWindow(_THIS, SDL_Window *window) return SDL_SetError("failed to create a window surface"); } - if (c->shell.zxdg) { + if (c->shell.xdg) { + if (data->shell_surface.xdg.surface) { + xdg_surface_set_user_data(data->shell_surface.xdg.surface, data); + xdg_surface_add_listener(data->shell_surface.xdg.surface, &shell_surface_listener_xdg, data); + } + } else if (c->shell.zxdg) { if (data->shell_surface.zxdg.surface) { zxdg_surface_v6_set_user_data(data->shell_surface.zxdg.surface, data); zxdg_surface_v6_add_listener(data->shell_surface.zxdg.surface, &shell_surface_listener_zxdg, data); @@ -477,6 +574,24 @@ int Wayland_CreateWindow(_THIS, SDL_Window *window) wl_surface_commit(data->surface); WAYLAND_wl_display_flush(c->display); + /* we have to wait until the surface gets a "configure" event, or + use of this surface will fail. This is a new rule for xdg_shell. */ + if (c->shell.xdg) { + if (data->shell_surface.xdg.surface) { + while (!data->shell_surface.xdg.initial_configure_seen) { + WAYLAND_wl_display_flush(c->display); + WAYLAND_wl_display_dispatch(c->display); + } + } + } else if (c->shell.zxdg) { + if (data->shell_surface.zxdg.surface) { + while (!data->shell_surface.zxdg.initial_configure_seen) { + WAYLAND_wl_display_flush(c->display); + WAYLAND_wl_display_dispatch(c->display); + } + } + } + return 0; } @@ -500,7 +615,9 @@ void Wayland_SetWindowTitle(_THIS, SDL_Window * window) SDL_VideoData *viddata = (SDL_VideoData *) _this->driverdata; if (window->title != NULL) { - if (viddata->shell.zxdg) { + if (viddata->shell.xdg) { + xdg_toplevel_set_title(wind->shell_surface.xdg.roleobj.toplevel, window->title); + } else if (viddata->shell.zxdg) { zxdg_toplevel_v6_set_title(wind->shell_surface.zxdg.roleobj.toplevel, window->title); } else { wl_shell_surface_set_title(wind->shell_surface.wl, window->title); @@ -519,7 +636,14 @@ void Wayland_DestroyWindow(_THIS, SDL_Window *window) SDL_EGL_DestroySurface(_this, wind->egl_surface); WAYLAND_wl_egl_window_destroy(wind->egl_window); - if (data->shell.zxdg) { + if (data->shell.xdg) { + if (wind->shell_surface.xdg.roleobj.toplevel) { + xdg_toplevel_destroy(wind->shell_surface.xdg.roleobj.toplevel); + } + if (wind->shell_surface.zxdg.surface) { + xdg_surface_destroy(wind->shell_surface.xdg.surface); + } + } else if (data->shell.zxdg) { if (wind->shell_surface.zxdg.roleobj.toplevel) { zxdg_toplevel_v6_destroy(wind->shell_surface.zxdg.roleobj.toplevel); } diff --git a/Engine/lib/sdl/src/video/wayland/SDL_waylandwindow.h b/Engine/lib/sdl/src/video/wayland/SDL_waylandwindow.h index 80d4f3138..69b988923 100644 --- a/Engine/lib/sdl/src/video/wayland/SDL_waylandwindow.h +++ b/Engine/lib/sdl/src/video/wayland/SDL_waylandwindow.h @@ -37,14 +37,24 @@ typedef struct { struct zxdg_toplevel_v6 *toplevel; struct zxdg_popup_v6 *popup; } roleobj; + SDL_bool initial_configure_seen; } SDL_zxdg_shell_surface; +typedef struct { + struct xdg_surface *surface; + union { + struct xdg_toplevel *toplevel; + struct xdg_popup *popup; + } roleobj; + SDL_bool initial_configure_seen; +} SDL_xdg_shell_surface; + typedef struct { SDL_Window *sdlwindow; SDL_VideoData *waylandData; struct wl_surface *surface; union { - /* !!! FIXME: add stable xdg_shell from 1.12 */ + SDL_xdg_shell_surface xdg; SDL_zxdg_shell_surface zxdg; struct wl_shell_surface *wl; } shell_surface; diff --git a/Engine/lib/sdl/src/video/windows/SDL_windowsevents.c b/Engine/lib/sdl/src/video/windows/SDL_windowsevents.c index a5fd00689..e961cf568 100644 --- a/Engine/lib/sdl/src/video/windows/SDL_windowsevents.c +++ b/Engine/lib/sdl/src/video/windows/SDL_windowsevents.c @@ -228,9 +228,7 @@ WIN_CheckWParamMouseButton(SDL_bool bwParamMousePressed, SDL_bool bSDLMousePress /* Ignore the button click for activation */ if (!bwParamMousePressed) { data->focus_click_pending &= ~SDL_BUTTON(button); - if (!data->focus_click_pending) { - WIN_UpdateClipCursor(data->window); - } + WIN_UpdateClipCursor(data->window); } if (WIN_ShouldIgnoreFocusClick()) { return; @@ -416,11 +414,23 @@ WIN_WindowProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam) } break; + case WM_NCACTIVATE: + { + /* Don't immediately clip the cursor in case we're clicking minimize/maximize buttons */ + data->skip_update_clipcursor = SDL_TRUE; + } + break; + case WM_ACTIVATE: { POINT cursorPos; BOOL minimized; + /* Don't mark the window as shown if it's activated before being shown */ + if (!IsWindowVisible(hwnd)) { + break; + } + minimized = HIWORD(wParam); if (!minimized && (LOWORD(wParam) != WA_INACTIVE)) { if (LOWORD(wParam) == WA_CLICKACTIVE) { @@ -460,6 +470,8 @@ WIN_WindowProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam) SDL_ToggleModState(KMOD_CAPS, (GetKeyState(VK_CAPITAL) & 0x0001) != 0); SDL_ToggleModState(KMOD_NUM, (GetKeyState(VK_NUMLOCK) & 0x0001) != 0); } else { + RECT rect; + data->in_window_deactivation = SDL_TRUE; if (SDL_GetKeyboardFocus() == data->window) { @@ -467,7 +479,10 @@ WIN_WindowProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam) WIN_ResetDeadKeys(); } - ClipCursor(NULL); + if (GetClipCursor(&rect) && SDL_memcmp(&rect, &data->cursor_clipped_rect, sizeof(rect) == 0)) { + ClipCursor(NULL); + SDL_zero(data->cursor_clipped_rect); + } data->in_window_deactivation = SDL_FALSE; } @@ -648,7 +663,7 @@ WIN_WindowProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam) break; case WM_UNICHAR: - if ( wParam == UNICODE_NOCHAR ) { + if (wParam == UNICODE_NOCHAR) { returnCode = 1; break; } @@ -656,8 +671,8 @@ WIN_WindowProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam) case WM_CHAR: { char text[5]; - if ( WIN_ConvertUTF32toUTF8( (UINT32)wParam, text ) ) { - SDL_SendKeyboardText( text ); + if (WIN_ConvertUTF32toUTF8((UINT32)wParam, text)) { + SDL_SendKeyboardText(text); } } returnCode = 0; @@ -1022,6 +1037,20 @@ WIN_WindowProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam) } } +static void WIN_UpdateClipCursorForWindows() +{ + SDL_VideoDevice *_this = SDL_GetVideoDevice(); + SDL_Window *window; + + if (_this) { + for (window = _this->windows; window; window = window->next) { + if (window->driverdata) { + WIN_UpdateClipCursor(window); + } + } + } +} + /* A message hook called before TranslateMessage() */ static SDL_WindowsMessageHook g_WindowsMessageHook = NULL; static void *g_WindowsMessageHookData = NULL; @@ -1067,6 +1096,9 @@ WIN_PumpEvents(_THIS) if ((keystate[SDL_SCANCODE_RSHIFT] == SDL_PRESSED) && !(GetKeyState(VK_RSHIFT) & 0x8000)) { SDL_SendKeyboardKey(SDL_RELEASED, SDL_SCANCODE_RSHIFT); } + + /* Update the clipping rect in case someone else has stolen it */ + WIN_UpdateClipCursorForWindows(); } /* to work around #3931, a bug introduced in Win10 Fall Creators Update (build nr. 16299) @@ -1094,9 +1126,9 @@ IsWin10FCUorNewer(void) SDL_zero(info); info.dwOSVersionInfoSize = sizeof(info); if (getVersionPtr(&info) == 0) { /* STATUS_SUCCESS == 0 */ - if ( (info.dwMajorVersion == 10 && info.dwMinorVersion == 0 && info.dwBuildNumber >= 16299) - || (info.dwMajorVersion == 10 && info.dwMinorVersion > 0) - || (info.dwMajorVersion > 10) ) + if ((info.dwMajorVersion == 10 && info.dwMinorVersion == 0 && info.dwBuildNumber >= 16299) || + (info.dwMajorVersion == 10 && info.dwMinorVersion > 0) || + (info.dwMajorVersion > 10)) { return SDL_TRUE; } diff --git a/Engine/lib/sdl/src/video/windows/SDL_windowsmessagebox.c b/Engine/lib/sdl/src/video/windows/SDL_windowsmessagebox.c index 924b4121b..9ddb9e22f 100644 --- a/Engine/lib/sdl/src/video/windows/SDL_windowsmessagebox.c +++ b/Engine/lib/sdl/src/video/windows/SDL_windowsmessagebox.c @@ -22,16 +22,52 @@ #if SDL_VIDEO_DRIVER_WINDOWS +#ifdef HAVE_LIMITS_H +#include +#else +#ifndef SIZE_MAX +#define SIZE_MAX ((size_t)-1) +#endif +#endif + #include "../../core/windows/SDL_windows.h" #include "SDL_assert.h" #include "SDL_windowsvideo.h" - +#include "SDL_windowstaskdialog.h" #ifndef SS_EDITCONTROL #define SS_EDITCONTROL 0x2000 #endif +#ifndef IDOK +#define IDOK 1 +#endif + +#ifndef IDCANCEL +#define IDCANCEL 2 +#endif + +/* Custom dialog return codes */ +#define IDCLOSED 20 +#define IDINVALPTRINIT 50 +#define IDINVALPTRCOMMAND 51 +#define IDINVALPTRSETFOCUS 52 +#define IDINVALPTRDLGITEM 53 +/* First button ID */ +#define IDBUTTONINDEX0 100 + +#define DLGITEMTYPEBUTTON 0x0080 +#define DLGITEMTYPESTATIC 0x0082 + +/* Windows only sends the lower 16 bits of the control ID when a button + * gets clicked. There are also some predefined and custom IDs that lower + * the available number further. 2^16 - 101 buttons should be enough for + * everyone, no need to make the code more complex. + */ +#define MAX_BUTTONS (0xffff - 100) + + /* Display a Windows message box */ #pragma pack(push, 1) @@ -70,15 +106,79 @@ typedef struct Uint8 *data; size_t size; size_t used; + WORD numbuttons; } WIN_DialogData; +static SDL_bool GetButtonIndex(const SDL_MessageBoxData *messageboxdata, Uint32 flags, size_t *i) +{ + for (*i = 0; *i < (size_t)messageboxdata->numbuttons; ++*i) { + if (messageboxdata->buttons[*i].flags & flags) { + return SDL_TRUE; + } + } + return SDL_FALSE; +} static INT_PTR MessageBoxDialogProc(HWND hDlg, UINT iMessage, WPARAM wParam, LPARAM lParam) { + const SDL_MessageBoxData *messageboxdata; + size_t buttonindex; + switch ( iMessage ) { + case WM_INITDIALOG: + if (lParam == 0) { + EndDialog(hDlg, IDINVALPTRINIT); + return TRUE; + } + messageboxdata = (const SDL_MessageBoxData *)lParam; + SetWindowLongPtr(hDlg, GWLP_USERDATA, lParam); + + if (GetButtonIndex(messageboxdata, SDL_MESSAGEBOX_BUTTON_RETURNKEY_DEFAULT, &buttonindex)) { + /* Focus on the first default return-key button */ + HWND buttonctl = GetDlgItem(hDlg, (int)(IDBUTTONINDEX0 + buttonindex)); + if (buttonctl == NULL) { + EndDialog(hDlg, IDINVALPTRDLGITEM); + } + PostMessage(hDlg, WM_NEXTDLGCTL, (WPARAM)buttonctl, TRUE); + } else { + /* Give the focus to the dialog window instead */ + SetFocus(hDlg); + } + return FALSE; + case WM_SETFOCUS: + messageboxdata = (const SDL_MessageBoxData *)GetWindowLongPtr(hDlg, GWLP_USERDATA); + if (messageboxdata == NULL) { + EndDialog(hDlg, IDINVALPTRSETFOCUS); + return TRUE; + } + + /* Let the default button be focused if there is one. Otherwise, prevent any initial focus. */ + if (GetButtonIndex(messageboxdata, SDL_MESSAGEBOX_BUTTON_RETURNKEY_DEFAULT, &buttonindex)) { + return FALSE; + } + return TRUE; case WM_COMMAND: + messageboxdata = (const SDL_MessageBoxData *)GetWindowLongPtr(hDlg, GWLP_USERDATA); + if (messageboxdata == NULL) { + EndDialog(hDlg, IDINVALPTRCOMMAND); + return TRUE; + } + /* Return the ID of the button that was pushed */ - EndDialog(hDlg, LOWORD(wParam)); + if (wParam == IDOK) { + if (GetButtonIndex(messageboxdata, SDL_MESSAGEBOX_BUTTON_RETURNKEY_DEFAULT, &buttonindex)) { + EndDialog(hDlg, IDBUTTONINDEX0 + buttonindex); + } + } else if (wParam == IDCANCEL) { + if (GetButtonIndex(messageboxdata, SDL_MESSAGEBOX_BUTTON_ESCAPEKEY_DEFAULT, &buttonindex)) { + EndDialog(hDlg, IDBUTTONINDEX0 + buttonindex); + } else { + /* Closing of window was requested by user or system. It would be rude not to comply. */ + EndDialog(hDlg, IDCLOSED); + } + } else if (wParam >= IDBUTTONINDEX0 && (int)wParam - IDBUTTONINDEX0 < messageboxdata->numbuttons) { + EndDialog(hDlg, wParam); + } return TRUE; default: @@ -89,15 +189,30 @@ static INT_PTR MessageBoxDialogProc(HWND hDlg, UINT iMessage, WPARAM wParam, LPA static SDL_bool ExpandDialogSpace(WIN_DialogData *dialog, size_t space) { + /* Growing memory in 64 KiB steps. */ + const size_t sizestep = 0x10000; size_t size = dialog->size; if (size == 0) { - size = space; - } else { - while ((dialog->used + space) > size) { - size *= 2; + /* Start with 4 KiB or a multiple of 64 KiB to fit the data. */ + size = 0x1000; + if (SIZE_MAX - sizestep < space) { + size = space; + } else if (space > size) { + size = (space + sizestep) & ~(sizestep - 1); } + } else if (SIZE_MAX - dialog->used < space) { + SDL_OutOfMemory(); + return SDL_FALSE; + } else if (SIZE_MAX - (dialog->used + space) < sizestep) { + /* Close to the maximum. */ + size = dialog->used + space; + } else if (size < dialog->used + space) { + /* Round up to the next 64 KiB block. */ + size = dialog->used + space; + size += sizestep - size % sizestep; } + if (size > dialog->size) { void *data = SDL_realloc(dialog->data, size); if (!data) { @@ -175,7 +290,7 @@ static void Vec2ToDLU(short *x, short *y) } -static SDL_bool AddDialogControl(WIN_DialogData *dialog, WORD type, DWORD style, DWORD exStyle, int x, int y, int w, int h, int id, const char *caption) +static SDL_bool AddDialogControl(WIN_DialogData *dialog, WORD type, DWORD style, DWORD exStyle, int x, int y, int w, int h, int id, const char *caption, WORD ordinal) { DLGITEMTEMPLATEEX item; WORD marker = 0xFFFF; @@ -205,32 +320,54 @@ static SDL_bool AddDialogControl(WIN_DialogData *dialog, WORD type, DWORD style, if (!AddDialogData(dialog, &type, sizeof(type))) { return SDL_FALSE; } - if (!AddDialogString(dialog, caption)) { - return SDL_FALSE; + if (type == DLGITEMTYPEBUTTON || (type == DLGITEMTYPESTATIC && caption != NULL)) { + if (!AddDialogString(dialog, caption)) { + return SDL_FALSE; + } + } else { + if (!AddDialogData(dialog, &marker, sizeof(marker))) { + return SDL_FALSE; + } + if (!AddDialogData(dialog, &ordinal, sizeof(ordinal))) { + return SDL_FALSE; + } } if (!AddDialogData(dialog, &extraData, sizeof(extraData))) { return SDL_FALSE; } + if (type == DLGITEMTYPEBUTTON) { + dialog->numbuttons++; + } ++dialog->lpDialog->cDlgItems; return SDL_TRUE; } -static SDL_bool AddDialogStatic(WIN_DialogData *dialog, int x, int y, int w, int h, const char *text) +static SDL_bool AddDialogStaticText(WIN_DialogData *dialog, int x, int y, int w, int h, const char *text) { - DWORD style = WS_VISIBLE | WS_CHILD | SS_LEFT | SS_NOPREFIX | SS_EDITCONTROL; - return AddDialogControl(dialog, 0x0082, style, 0, x, y, w, h, -1, text); + DWORD style = WS_VISIBLE | WS_CHILD | SS_LEFT | SS_NOPREFIX | SS_EDITCONTROL | WS_GROUP; + return AddDialogControl(dialog, DLGITEMTYPESTATIC, style, 0, x, y, w, h, -1, text, 0); +} + +static SDL_bool AddDialogStaticIcon(WIN_DialogData *dialog, int x, int y, int w, int h, Uint16 ordinal) +{ + DWORD style = WS_VISIBLE | WS_CHILD | SS_ICON | WS_GROUP; + return AddDialogControl(dialog, DLGITEMTYPESTATIC, style, 0, x, y, w, h, -2, NULL, ordinal); } static SDL_bool AddDialogButton(WIN_DialogData *dialog, int x, int y, int w, int h, const char *text, int id, SDL_bool isDefault) { - DWORD style = WS_VISIBLE | WS_CHILD; + DWORD style = WS_VISIBLE | WS_CHILD | WS_TABSTOP; if (isDefault) { style |= BS_DEFPUSHBUTTON; } else { style |= BS_PUSHBUTTON; } - return AddDialogControl(dialog, 0x0080, style, 0, x, y, w, h, id, text); + /* The first button marks the start of the group. */ + if (dialog->numbuttons == 0) { + style |= WS_GROUP; + } + return AddDialogControl(dialog, DLGITEMTYPEBUTTON, style, 0, x, y, w, h, IDBUTTONINDEX0 + dialog->numbuttons, text, 0); } static void FreeDialogData(WIN_DialogData *dialog) @@ -341,17 +478,87 @@ static WIN_DialogData *CreateDialogData(int w, int h, const char *caption) return dialog; } -int -WIN_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) +/* Escaping ampersands is necessary to disable mnemonics in dialog controls. + * The caller provides a char** for dst and a size_t* for dstlen where the + * address of the work buffer and its size will be stored. Their values must be + * NULL and 0 on the first call. src is the string to be escaped. On error, the + * function returns NULL and, on success, returns a pointer to the escaped + * sequence as a read-only string that is valid until the next call or until the + * work buffer is freed. Once all strings have been processed, it's the caller's + * responsibilty to free the work buffer with SDL_free, even on errors. + */ +static const char *EscapeAmpersands(char **dst, size_t *dstlen, const char *src) +{ + char *newdst; + size_t ampcount = 0; + size_t srclen = 0; + + if (src == NULL) { + return NULL; + } + + while (src[srclen]) { + if (src[srclen] == '&') { + ampcount++; + } + srclen++; + } + srclen++; + + if (ampcount == 0) { + /* Nothing to do. */ + return src; + } + if (SIZE_MAX - srclen < ampcount) { + return NULL; + } + if (*dst == NULL || *dstlen < srclen + ampcount) { + /* Allocating extra space in case the next strings are a bit longer. */ + size_t extraspace = SIZE_MAX - (srclen + ampcount); + if (extraspace > 512) { + extraspace = 512; + } + *dstlen = srclen + ampcount + extraspace; + SDL_free(*dst); + *dst = NULL; + newdst = SDL_malloc(*dstlen); + if (newdst == NULL) { + return NULL; + } + *dst = newdst; + } else { + newdst = *dst; + } + + /* The escape character is the ampersand itself. */ + while (srclen--) { + if (*src == '&') { + *newdst++ = '&'; + } + *newdst++ = *src++; + } + + return *dst; +} + +/* This function is called if a Task Dialog is unsupported. */ +static int +WIN_ShowOldMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) { WIN_DialogData *dialog; - int i, x, y; + int i, x, y, retval; const SDL_MessageBoxButtonData *buttons = messageboxdata->buttons; HFONT DialogFont; SIZE Size; RECT TextSize; wchar_t* wmessage; TEXTMETRIC TM; + HDC FontDC; + INT_PTR result; + char *ampescape = NULL; + size_t ampescapesize = 0; + Uint16 defbuttoncount = 0; + Uint16 icon = 0; HWND ParentWindow = NULL; @@ -359,7 +566,25 @@ WIN_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) const int ButtonHeight = 26; const int TextMargin = 16; const int ButtonMargin = 12; + const int IconWidth = GetSystemMetrics(SM_CXICON); + const int IconHeight = GetSystemMetrics(SM_CYICON); + const int IconMargin = 20; + if (messageboxdata->numbuttons > MAX_BUTTONS) { + return SDL_SetError("Number of butons exceeds limit of %d", MAX_BUTTONS); + } + + switch (messageboxdata->flags) { + case SDL_MESSAGEBOX_ERROR: + icon = (Uint16)(size_t)IDI_ERROR; + break; + case SDL_MESSAGEBOX_WARNING: + icon = (Uint16)(size_t)IDI_WARNING; + break; + case SDL_MESSAGEBOX_INFORMATION: + icon = (Uint16)(size_t)IDI_INFORMATION; + break; + } /* Jan 25th, 2013 - dant@fleetsa.com * @@ -393,7 +618,7 @@ WIN_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) * In order to get text dimensions we need to have a DC with the desired font. * I'm assuming a dialog box in SDL is rare enough we can to the create. */ - HDC FontDC = CreateCompatibleDC(0); + FontDC = CreateCompatibleDC(0); { /* Create a duplicate of the font used in system message boxes. */ @@ -428,11 +653,13 @@ WIN_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) /* Measure the *pixel* size of the string. */ wmessage = WIN_UTF8ToString(messageboxdata->message); SDL_zero(TextSize); - DrawText(FontDC, wmessage, -1, &TextSize, DT_CALCRECT); + DrawText(FontDC, wmessage, -1, &TextSize, DT_CALCRECT | DT_LEFT | DT_NOPREFIX | DT_EDITCONTROL); - /* Add some padding for hangs, etc. */ - TextSize.right += 2; - TextSize.bottom += 2; + /* Add margins and some padding for hangs, etc. */ + TextSize.left += TextMargin; + TextSize.right += TextMargin + 2; + TextSize.top += TextMargin; + TextSize.bottom += TextMargin + 2; /* Done with the DC, and the string */ DeleteDC(FontDC); @@ -444,10 +671,22 @@ WIN_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) Size.cx += TextMargin * 2; Size.cy += TextMargin * 2; + /* Make dialog wider and shift text over for the icon. */ + if (icon) { + Size.cx += IconMargin + IconWidth; + TextSize.left += IconMargin + IconWidth; + TextSize.right += IconMargin + IconWidth; + } + /* Ensure the size is wide enough for all of the buttons. */ if (Size.cx < messageboxdata->numbuttons * (ButtonWidth + ButtonMargin) + ButtonMargin) Size.cx = messageboxdata->numbuttons * (ButtonWidth + ButtonMargin) + ButtonMargin; + /* Reset the height to the icon size if it is actually bigger than the text. */ + if (icon && Size.cy < IconMargin * 2 + IconHeight) { + Size.cy = IconMargin * 2 + IconHeight; + } + /* Add vertical space for the buttons and border. */ Size.cy += ButtonHeight + TextMargin; @@ -456,7 +695,12 @@ WIN_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) return -1; } - if (!AddDialogStatic(dialog, TextMargin, TextMargin, TextSize.right - TextSize.left, TextSize.bottom - TextSize.top, messageboxdata->message)) { + if (icon && ! AddDialogStaticIcon(dialog, IconMargin, IconMargin, IconWidth, IconHeight, icon)) { + FreeDialogData(dialog); + return -1; + } + + if (!AddDialogStaticText(dialog, TextSize.left, TextSize.top, TextSize.right - TextSize.left, TextSize.bottom - TextSize.top, messageboxdata->message)) { FreeDialogData(dialog); return -1; } @@ -465,19 +709,25 @@ WIN_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) x = Size.cx - (ButtonWidth + ButtonMargin) * messageboxdata->numbuttons; y = Size.cy - ButtonHeight - ButtonMargin; for (i = messageboxdata->numbuttons - 1; i >= 0; --i) { - SDL_bool isDefault; + SDL_bool isdefault = SDL_FALSE; + const char *buttontext; if (buttons[i].flags & SDL_MESSAGEBOX_BUTTON_RETURNKEY_DEFAULT) { - isDefault = SDL_TRUE; - } else { - isDefault = SDL_FALSE; + defbuttoncount++; + if (defbuttoncount == 1) { + isdefault = SDL_TRUE; + } } - if (!AddDialogButton(dialog, x, y, ButtonWidth, ButtonHeight, buttons[i].text, buttons[i].buttonid, isDefault)) { + + buttontext = EscapeAmpersands(&escape, &escapesize, buttons[i].text); + if (buttontext == NULL || !AddDialogButton(dialog, x, y, ButtonWidth, ButtonHeight, buttontext, buttons[i].buttonid, isdefault)) { FreeDialogData(dialog); + SDL_free(ampescape); return -1; } x += ButtonWidth + ButtonMargin; } + SDL_free(ampescape); /* If we have a parent window, get the Instance and HWND for them * so that our little dialog gets exclusive focus at all times. */ @@ -485,10 +735,169 @@ WIN_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) ParentWindow = ((SDL_WindowData*)messageboxdata->window->driverdata)->hwnd; } - *buttonid = (int)DialogBoxIndirect(NULL, (DLGTEMPLATE*)dialog->lpDialog, ParentWindow, (DLGPROC)MessageBoxDialogProc); + result = DialogBoxIndirectParam(NULL, (DLGTEMPLATE*)dialog->lpDialog, ParentWindow, (DLGPROC)MessageBoxDialogProc, (LPARAM)messageboxdata); + if (result >= IDBUTTONINDEX0 && result - IDBUTTONINDEX0 < messageboxdata->numbuttons) { + *buttonid = messageboxdata->buttons[(messageboxdata->numbuttons - 1) - (result - IDBUTTONINDEX0)].buttonid; + retval = 0; + } else if (result == IDCLOSED) { + /* Dialog window closed by user or system. */ + /* This could use a special return code. */ + retval = 0; + *buttonid = -1; + } else { + if (result == 0) { + SDL_SetError("Invalid parent window handle"); + } else if (result == -1) { + SDL_SetError("The message box encountered an error."); + } else if (result == IDINVALPTRINIT || result == IDINVALPTRSETFOCUS || result == IDINVALPTRCOMMAND) { + SDL_SetError("Invalid message box pointer in dialog procedure"); + } else if (result == IDINVALPTRDLGITEM) { + SDL_SetError("Couldn't find dialog control of the default enter-key button"); + } else { + SDL_SetError("An unknown error occured"); + } + retval = -1; + } FreeDialogData(dialog); - return 0; + return retval; +} + +/* TaskDialogIndirect procedure + * This is because SDL targets Windows XP (0x501), so this is not defined in the platform SDK. + */ +typedef HRESULT(FAR WINAPI *TASKDIALOGINDIRECTPROC)(const TASKDIALOGCONFIG *pTaskConfig, int *pnButton, int *pnRadioButton, BOOL *pfVerificationFlagChecked); + +int +WIN_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid) +{ + HWND ParentWindow = NULL; + wchar_t *wmessage; + wchar_t *wtitle; + TASKDIALOGCONFIG TaskConfig; + TASKDIALOG_BUTTON *pButtons; + TASKDIALOG_BUTTON *pButton; + HMODULE hComctl32; + TASKDIALOGINDIRECTPROC pfnTaskDialogIndirect; + HRESULT hr; + char *ampescape = NULL; + size_t ampescapesize = 0; + int nButton; + int nCancelButton; + int i; + + if (SIZE_MAX / sizeof(TASKDIALOG_BUTTON) < messageboxdata->numbuttons) { + return SDL_OutOfMemory(); + } + + /* If we cannot load comctl32.dll use the old messagebox! */ + hComctl32 = LoadLibrary(TEXT("Comctl32.dll")); + if (hComctl32 == NULL) { + return WIN_ShowOldMessageBox(messageboxdata, buttonid); + } + + /* If TaskDialogIndirect doesn't exist use the old messagebox! + This will fail prior to Windows Vista. + The manifest file in the application may require targeting version 6 of comctl32.dll, even + when we use LoadLibrary here! + If you don't want to bother with manifests, put this #pragma in your app's source code somewhere: + pragma comment(linker,"\"/manifestdependency:type='win32' name='Microsoft.Windows.Common-Controls' version='6.0.0.0' processorArchitecture='*' publicKeyToken='6595b64144ccf1df' language='*'\"") + */ + pfnTaskDialogIndirect = (TASKDIALOGINDIRECTPROC) GetProcAddress(hComctl32, "TaskDialogIndirect"); + if (pfnTaskDialogIndirect == NULL) { + FreeLibrary(hComctl32); + return WIN_ShowOldMessageBox(messageboxdata, buttonid); + } + + /* If we have a parent window, get the Instance and HWND for them + so that our little dialog gets exclusive focus at all times. */ + if (messageboxdata->window) { + ParentWindow = ((SDL_WindowData *) messageboxdata->window->driverdata)->hwnd; + } + + wmessage = WIN_UTF8ToString(messageboxdata->message); + wtitle = WIN_UTF8ToString(messageboxdata->title); + + SDL_zero(TaskConfig); + TaskConfig.cbSize = sizeof (TASKDIALOGCONFIG); + TaskConfig.hwndParent = ParentWindow; + TaskConfig.dwFlags = TDF_SIZE_TO_CONTENT; + TaskConfig.pszWindowTitle = wtitle; + if (messageboxdata->flags & SDL_MESSAGEBOX_ERROR) { + TaskConfig.pszMainIcon = TD_ERROR_ICON; + } else if (messageboxdata->flags & SDL_MESSAGEBOX_WARNING) { + TaskConfig.pszMainIcon = TD_WARNING_ICON; + } else if (messageboxdata->flags & SDL_MESSAGEBOX_INFORMATION) { + TaskConfig.pszMainIcon = TD_INFORMATION_ICON; + } else { + TaskConfig.pszMainIcon = NULL; + } + + TaskConfig.pszContent = wmessage; + TaskConfig.cButtons = messageboxdata->numbuttons; + pButtons = SDL_malloc(sizeof (TASKDIALOG_BUTTON) * messageboxdata->numbuttons); + TaskConfig.nDefaultButton = 0; + nCancelButton = 0; + for (i = 0; i < messageboxdata->numbuttons; i++) + { + const char *buttontext; + pButton = &pButtons[messageboxdata->numbuttons-1-i]; + if (messageboxdata->buttons[i].flags & SDL_MESSAGEBOX_BUTTON_ESCAPEKEY_DEFAULT) { + nCancelButton = messageboxdata->buttons[i].buttonid; + pButton->nButtonID = 2; + } else { + pButton->nButtonID = messageboxdata->buttons[i].buttonid + 1; + if (pButton->nButtonID >= 2) { + pButton->nButtonID++; + } + } + buttontext = EscapeAmpersands(&escape, &escapesize, messageboxdata->buttons[i].text); + if (buttontext == NULL) { + int j; + FreeLibrary(hComctl32); + SDL_free(ampescape); + SDL_free(wmessage); + SDL_free(wtitle); + for (j = 0; j < i; j++) { + SDL_free((wchar_t *) pButtons[j].pszButtonText); + } + SDL_free(pButtons); + return -1; + } + pButton->pszButtonText = WIN_UTF8ToString(buttontext); + if (messageboxdata->buttons[i].flags & SDL_MESSAGEBOX_BUTTON_RETURNKEY_DEFAULT) { + TaskConfig.nDefaultButton = pButton->nButtonID; + } + } + TaskConfig.pButtons = pButtons; + + /* Show the Task Dialog */ + hr = pfnTaskDialogIndirect(&TaskConfig, &nButton, NULL, NULL); + + /* Free everything */ + FreeLibrary(hComctl32); + SDL_free(ampescape); + SDL_free(wmessage); + SDL_free(wtitle); + for (i = 0; i < messageboxdata->numbuttons; i++) { + SDL_free((wchar_t *) pButtons[i].pszButtonText); + } + SDL_free(pButtons); + + /* Check the Task Dialog was successful and give the result */ + if (SUCCEEDED(hr)) { + if (nButton == 2) { + *buttonid = nCancelButton; + } else if (nButton > 2) { + *buttonid = nButton-1-1; + } else { + *buttonid = nButton-1; + } + return 0; + } + + /* We failed showing the Task Dialog, use the old message box! */ + return WIN_ShowOldMessageBox(messageboxdata, buttonid); } #endif /* SDL_VIDEO_DRIVER_WINDOWS */ diff --git a/Engine/lib/sdl/src/video/windows/SDL_windowsmouse.c b/Engine/lib/sdl/src/video/windows/SDL_windowsmouse.c index 1ddeae24b..eff31605b 100644 --- a/Engine/lib/sdl/src/video/windows/SDL_windowsmouse.c +++ b/Engine/lib/sdl/src/video/windows/SDL_windowsmouse.c @@ -304,8 +304,6 @@ WIN_InitMouse(_THIS) mouse->GetGlobalMouseState = WIN_GetGlobalMouseState; SDL_SetDefaultCursor(WIN_CreateDefaultCursor()); - - SDL_SetDoubleClickTime(GetDoubleClickTime()); } void diff --git a/Engine/lib/sdl/src/video/windows/SDL_windowsopengl.h b/Engine/lib/sdl/src/video/windows/SDL_windowsopengl.h index 75b4898c3..87044117c 100644 --- a/Engine/lib/sdl/src/video/windows/SDL_windowsopengl.h +++ b/Engine/lib/sdl/src/video/windows/SDL_windowsopengl.h @@ -33,31 +33,31 @@ struct SDL_GLDriverData SDL_bool HAS_WGL_ARB_create_context_robustness; SDL_bool HAS_WGL_ARB_create_context_no_error; - /* Max version of OpenGL ES context that can be created if the - implementation supports WGL_EXT_create_context_es2_profile. - major = minor = 0 when unsupported. - */ - struct { - int major; - int minor; - } es_profile_max_supported_version; + /* Max version of OpenGL ES context that can be created if the + implementation supports WGL_EXT_create_context_es2_profile. + major = minor = 0 when unsupported. + */ + struct { + int major; + int minor; + } es_profile_max_supported_version; - void *(WINAPI * wglGetProcAddress) (const char *proc); - HGLRC(WINAPI * wglCreateContext) (HDC hdc); - BOOL(WINAPI * wglDeleteContext) (HGLRC hglrc); - BOOL(WINAPI * wglMakeCurrent) (HDC hdc, HGLRC hglrc); - BOOL(WINAPI * wglShareLists) (HGLRC hglrc1, HGLRC hglrc2); - BOOL(WINAPI * wglChoosePixelFormatARB) (HDC hdc, - const int *piAttribIList, - const FLOAT * pfAttribFList, - UINT nMaxFormats, - int *piFormats, - UINT * nNumFormats); - BOOL(WINAPI * wglGetPixelFormatAttribivARB) (HDC hdc, int iPixelFormat, - int iLayerPlane, - UINT nAttributes, - const int *piAttributes, - int *piValues); + void *(WINAPI * wglGetProcAddress) (const char *proc); + HGLRC(WINAPI * wglCreateContext) (HDC hdc); + BOOL(WINAPI * wglDeleteContext) (HGLRC hglrc); + BOOL(WINAPI * wglMakeCurrent) (HDC hdc, HGLRC hglrc); + BOOL(WINAPI * wglShareLists) (HGLRC hglrc1, HGLRC hglrc2); + BOOL(WINAPI * wglChoosePixelFormatARB) (HDC hdc, + const int *piAttribIList, + const FLOAT * pfAttribFList, + UINT nMaxFormats, + int *piFormats, + UINT * nNumFormats); + BOOL(WINAPI * wglGetPixelFormatAttribivARB) (HDC hdc, int iPixelFormat, + int iLayerPlane, + UINT nAttributes, + const int *piAttributes, + int *piValues); BOOL (WINAPI * wglSwapIntervalEXT) (int interval); int (WINAPI * wglGetSwapIntervalEXT) (void); }; diff --git a/Engine/lib/sdl/src/video/windows/SDL_windowstaskdialog.h b/Engine/lib/sdl/src/video/windows/SDL_windowstaskdialog.h new file mode 100644 index 000000000..a2a9e8a4b --- /dev/null +++ b/Engine/lib/sdl/src/video/windows/SDL_windowstaskdialog.h @@ -0,0 +1,156 @@ +/* + Simple DirectMedia Layer + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this 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 + +typedef HRESULT(CALLBACK *PFTASKDIALOGCALLBACK)(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam, LONG_PTR lpRefData); + +enum _TASKDIALOG_FLAGS +{ + TDF_ENABLE_HYPERLINKS = 0x0001, + TDF_USE_HICON_MAIN = 0x0002, + TDF_USE_HICON_FOOTER = 0x0004, + TDF_ALLOW_DIALOG_CANCELLATION = 0x0008, + TDF_USE_COMMAND_LINKS = 0x0010, + TDF_USE_COMMAND_LINKS_NO_ICON = 0x0020, + TDF_EXPAND_FOOTER_AREA = 0x0040, + TDF_EXPANDED_BY_DEFAULT = 0x0080, + TDF_VERIFICATION_FLAG_CHECKED = 0x0100, + TDF_SHOW_PROGRESS_BAR = 0x0200, + TDF_SHOW_MARQUEE_PROGRESS_BAR = 0x0400, + TDF_CALLBACK_TIMER = 0x0800, + TDF_POSITION_RELATIVE_TO_WINDOW = 0x1000, + TDF_RTL_LAYOUT = 0x2000, + TDF_NO_DEFAULT_RADIO_BUTTON = 0x4000, + TDF_CAN_BE_MINIMIZED = 0x8000, + //#if (NTDDI_VERSION >= NTDDI_WIN8) + TDF_NO_SET_FOREGROUND = 0x00010000, // Don't call SetForegroundWindow() when activating the dialog + //#endif // (NTDDI_VERSION >= NTDDI_WIN8) + TDF_SIZE_TO_CONTENT = 0x01000000 // used by ShellMessageBox to emulate MessageBox sizing behavior +}; +typedef int TASKDIALOG_FLAGS; // Note: _TASKDIALOG_FLAGS is an int + +typedef enum _TASKDIALOG_MESSAGES +{ + TDM_NAVIGATE_PAGE = WM_USER + 101, + TDM_CLICK_BUTTON = WM_USER + 102, // wParam = Button ID + TDM_SET_MARQUEE_PROGRESS_BAR = WM_USER + 103, // wParam = 0 (nonMarque) wParam != 0 (Marquee) + TDM_SET_PROGRESS_BAR_STATE = WM_USER + 104, // wParam = new progress state + TDM_SET_PROGRESS_BAR_RANGE = WM_USER + 105, // lParam = MAKELPARAM(nMinRange, nMaxRange) + TDM_SET_PROGRESS_BAR_POS = WM_USER + 106, // wParam = new position + TDM_SET_PROGRESS_BAR_MARQUEE = WM_USER + 107, // wParam = 0 (stop marquee), wParam != 0 (start marquee), lparam = speed (milliseconds between repaints) + TDM_SET_ELEMENT_TEXT = WM_USER + 108, // wParam = element (TASKDIALOG_ELEMENTS), lParam = new element text (LPCWSTR) + TDM_CLICK_RADIO_BUTTON = WM_USER + 110, // wParam = Radio Button ID + TDM_ENABLE_BUTTON = WM_USER + 111, // lParam = 0 (disable), lParam != 0 (enable), wParam = Button ID + TDM_ENABLE_RADIO_BUTTON = WM_USER + 112, // lParam = 0 (disable), lParam != 0 (enable), wParam = Radio Button ID + TDM_CLICK_VERIFICATION = WM_USER + 113, // wParam = 0 (unchecked), 1 (checked), lParam = 1 (set key focus) + TDM_UPDATE_ELEMENT_TEXT = WM_USER + 114, // wParam = element (TASKDIALOG_ELEMENTS), lParam = new element text (LPCWSTR) + TDM_SET_BUTTON_ELEVATION_REQUIRED_STATE = WM_USER + 115, // wParam = Button ID, lParam = 0 (elevation not required), lParam != 0 (elevation required) + TDM_UPDATE_ICON = WM_USER + 116 // wParam = icon element (TASKDIALOG_ICON_ELEMENTS), lParam = new icon (hIcon if TDF_USE_HICON_* was set, PCWSTR otherwise) +} TASKDIALOG_MESSAGES; + +typedef enum _TASKDIALOG_NOTIFICATIONS +{ + TDN_CREATED = 0, + TDN_NAVIGATED = 1, + TDN_BUTTON_CLICKED = 2, // wParam = Button ID + TDN_HYPERLINK_CLICKED = 3, // lParam = (LPCWSTR)pszHREF + TDN_TIMER = 4, // wParam = Milliseconds since dialog created or timer reset + TDN_DESTROYED = 5, + TDN_RADIO_BUTTON_CLICKED = 6, // wParam = Radio Button ID + TDN_DIALOG_CONSTRUCTED = 7, + TDN_VERIFICATION_CLICKED = 8, // wParam = 1 if checkbox checked, 0 if not, lParam is unused and always 0 + TDN_HELP = 9, + TDN_EXPANDO_BUTTON_CLICKED = 10 // wParam = 0 (dialog is now collapsed), wParam != 0 (dialog is now expanded) +} TASKDIALOG_NOTIFICATIONS; + +typedef struct _TASKDIALOG_BUTTON +{ + int nButtonID; + PCWSTR pszButtonText; +} TASKDIALOG_BUTTON; + +typedef enum _TASKDIALOG_ELEMENTS +{ + TDE_CONTENT, + TDE_EXPANDED_INFORMATION, + TDE_FOOTER, + TDE_MAIN_INSTRUCTION +} TASKDIALOG_ELEMENTS; + +typedef enum _TASKDIALOG_ICON_ELEMENTS +{ + TDIE_ICON_MAIN, + TDIE_ICON_FOOTER +} TASKDIALOG_ICON_ELEMENTS; + +#define TD_WARNING_ICON MAKEINTRESOURCEW(-1) +#define TD_ERROR_ICON MAKEINTRESOURCEW(-2) +#define TD_INFORMATION_ICON MAKEINTRESOURCEW(-3) +#define TD_SHIELD_ICON MAKEINTRESOURCEW(-4) + +enum _TASKDIALOG_COMMON_BUTTON_FLAGS +{ + TDCBF_OK_BUTTON = 0x0001, // selected control return value IDOK + TDCBF_YES_BUTTON = 0x0002, // selected control return value IDYES + TDCBF_NO_BUTTON = 0x0004, // selected control return value IDNO + TDCBF_CANCEL_BUTTON = 0x0008, // selected control return value IDCANCEL + TDCBF_RETRY_BUTTON = 0x0010, // selected control return value IDRETRY + TDCBF_CLOSE_BUTTON = 0x0020 // selected control return value IDCLOSE +}; +typedef int TASKDIALOG_COMMON_BUTTON_FLAGS; // Note: _TASKDIALOG_COMMON_BUTTON_FLAGS is an int + +typedef struct _TASKDIALOGCONFIG +{ + UINT cbSize; + HWND hwndParent; // incorrectly named, this is the owner window, not a parent. + HINSTANCE hInstance; // used for MAKEINTRESOURCE() strings + TASKDIALOG_FLAGS dwFlags; // TASKDIALOG_FLAGS (TDF_XXX) flags + TASKDIALOG_COMMON_BUTTON_FLAGS dwCommonButtons; // TASKDIALOG_COMMON_BUTTON (TDCBF_XXX) flags + PCWSTR pszWindowTitle; // string or MAKEINTRESOURCE() + union + { + HICON hMainIcon; + PCWSTR pszMainIcon; + } /*DUMMYUNIONNAME*/; + PCWSTR pszMainInstruction; + PCWSTR pszContent; + UINT cButtons; + const TASKDIALOG_BUTTON *pButtons; + int nDefaultButton; + UINT cRadioButtons; + const TASKDIALOG_BUTTON *pRadioButtons; + int nDefaultRadioButton; + PCWSTR pszVerificationText; + PCWSTR pszExpandedInformation; + PCWSTR pszExpandedControlText; + PCWSTR pszCollapsedControlText; + union + { + HICON hFooterIcon; + PCWSTR pszFooterIcon; + } /*DUMMYUNIONNAME2*/; + PCWSTR pszFooter; + PFTASKDIALOGCALLBACK pfCallback; + LONG_PTR lpCallbackData; + UINT cxWidth; // width of the Task Dialog's client area in DLU's. If 0, Task Dialog will calculate the ideal width. +} TASKDIALOGCONFIG; + +#include diff --git a/Engine/lib/sdl/src/video/windows/SDL_windowsvideo.c b/Engine/lib/sdl/src/video/windows/SDL_windowsvideo.c index 8d45b721e..358ab2356 100644 --- a/Engine/lib/sdl/src/video/windows/SDL_windowsvideo.c +++ b/Engine/lib/sdl/src/video/windows/SDL_windowsvideo.c @@ -63,6 +63,15 @@ UpdateWindowFrameUsableWhileCursorHidden(void *userdata, const char *name, const } } +static void WIN_SuspendScreenSaver(_THIS) +{ + if (_this->suspend_screensaver) { + SetThreadExecutionState(ES_CONTINUOUS | ES_DISPLAY_REQUIRED); + } else { + SetThreadExecutionState(ES_CONTINUOUS); + } +} + /* Windows driver bootstrap functions */ @@ -136,6 +145,7 @@ WIN_CreateDevice(int devindex) device->GetDisplayModes = WIN_GetDisplayModes; device->SetDisplayMode = WIN_SetDisplayMode; device->PumpEvents = WIN_PumpEvents; + device->SuspendScreenSaver = WIN_SuspendScreenSaver; device->CreateSDLWindow = WIN_CreateWindow; device->CreateSDLWindowFrom = WIN_CreateWindowFrom; @@ -164,6 +174,7 @@ WIN_CreateDevice(int devindex) device->DestroyWindowFramebuffer = WIN_DestroyWindowFramebuffer; device->OnWindowEnter = WIN_OnWindowEnter; device->SetWindowHitTest = WIN_SetWindowHitTest; + device->AcceptDragAndDrop = WIN_AcceptDragAndDrop; device->shape_driver.CreateShaper = Win32_CreateShaper; device->shape_driver.SetWindowShape = Win32_SetWindowShape; diff --git a/Engine/lib/sdl/src/video/windows/SDL_windowsvulkan.c b/Engine/lib/sdl/src/video/windows/SDL_windowsvulkan.c index c4b34f0db..6bb8f2a42 100644 --- a/Engine/lib/sdl/src/video/windows/SDL_windowsvulkan.c +++ b/Engine/lib/sdl/src/video/windows/SDL_windowsvulkan.c @@ -40,7 +40,7 @@ int WIN_Vulkan_LoadLibrary(_THIS, const char *path) { VkExtensionProperties *extensions = NULL; Uint32 extensionCount = 0; - Uint32 i; + Uint32 i; SDL_bool hasSurfaceExtension = SDL_FALSE; SDL_bool hasWin32SurfaceExtension = SDL_FALSE; PFN_vkGetInstanceProcAddr vkGetInstanceProcAddr = NULL; diff --git a/Engine/lib/sdl/src/video/windows/SDL_windowswindow.c b/Engine/lib/sdl/src/video/windows/SDL_windowswindow.c index b08244389..45463c4c7 100644 --- a/Engine/lib/sdl/src/video/windows/SDL_windowswindow.c +++ b/Engine/lib/sdl/src/video/windows/SDL_windowswindow.c @@ -97,6 +97,11 @@ GetWindowStyle(SDL_Window * window) if (window->flags & SDL_WINDOW_RESIZABLE) { style |= STYLE_RESIZABLE; } + + /* Need to set initialize minimize style, or when we call ShowWindow with WS_MINIMIZE it will activate a random window */ + if (window->flags & SDL_WINDOW_MINIMIZED) { + style |= WS_MINIMIZE; + } } return style; } @@ -215,8 +220,6 @@ SetupWindowData(_THIS, SDL_Window * window, HWND hwnd, HWND parent, SDL_bool cre if ((window->windowed.w && window->windowed.w != w) || (window->windowed.h && window->windowed.h != h)) { /* We tried to create a window larger than the desktop and Windows didn't allow it. Override! */ int x, y; - int w, h; - /* Figure out what the window area will be */ WIN_AdjustWindowRect(window, &x, &y, &w, &h, SDL_FALSE); SetWindowPos(hwnd, HWND_NOTOPMOST, x, y, w, h, SWP_NOCOPYBITS | SWP_NOZORDER | SWP_NOACTIVATE); @@ -287,9 +290,6 @@ SetupWindowData(_THIS, SDL_Window * window, HWND hwnd, HWND parent, SDL_bool cre videodata->RegisterTouchWindow(hwnd, (TWF_FINETOUCH|TWF_WANTPALM)); } - /* Enable dropping files */ - DragAcceptFiles(hwnd, TRUE); - data->initializing = SDL_FALSE; /* All done! */ @@ -335,6 +335,10 @@ WIN_CreateWindow(_THIS, SDL_Window * window) /* Inform Windows of the frame change so we can respond to WM_NCCALCSIZE */ SetWindowPos(hwnd, NULL, 0, 0, 0, 0, SWP_FRAMECHANGED | SWP_NOSIZE | SWP_NOZORDER | SWP_NOMOVE | SWP_NOACTIVATE); + if (window->flags & SDL_WINDOW_MINIMIZED) { + ShowWindow(hwnd, SW_SHOWMINNOACTIVE); + } + if (!(window->flags & SDL_WINDOW_OPENGL)) { return 0; } @@ -407,13 +411,11 @@ WIN_CreateWindowFrom(_THIS, SDL_Window * window, const void *data) SDL_sscanf(hint, "%p", (void**)&otherWindow); /* Do some error checking on the pointer */ - if (otherWindow != NULL && otherWindow->magic == &_this->window_magic) - { + if (otherWindow != NULL && otherWindow->magic == &_this->window_magic) { /* If the otherWindow has SDL_WINDOW_OPENGL set, set it for the new window as well */ - if (otherWindow->flags & SDL_WINDOW_OPENGL) - { + if (otherWindow->flags & SDL_WINDOW_OPENGL) { window->flags |= SDL_WINDOW_OPENGL; - if(!WIN_GL_SetPixelFormatFrom(_this, otherWindow, window)) { + if (!WIN_GL_SetPixelFormatFrom(_this, otherWindow, window)) { return -1; } } @@ -546,8 +548,17 @@ WIN_GetWindowBordersSize(_THIS, SDL_Window * window, int *top, int *left, int *b void WIN_ShowWindow(_THIS, SDL_Window * window) { - HWND hwnd = ((SDL_WindowData *) window->driverdata)->hwnd; - ShowWindow(hwnd, SW_SHOW); + DWORD style; + HWND hwnd; + int nCmdShow; + + hwnd = ((SDL_WindowData *)window->driverdata)->hwnd; + nCmdShow = SW_SHOW; + style = GetWindowLong(hwnd, GWL_EXSTYLE); + if (style & WS_EX_NOACTIVATE) { + nCmdShow = SW_SHOWNOACTIVATE; + } + ShowWindow(hwnd, nCmdShow); } void @@ -891,8 +902,13 @@ WIN_UpdateClipCursor(SDL_Window *window) { SDL_WindowData *data = (SDL_WindowData *) window->driverdata; SDL_Mouse *mouse = SDL_GetMouse(); + RECT rect; - if (data->focus_click_pending) { + if (data->in_title_click || data->focus_click_pending) { + return; + } + if (data->skip_update_clipcursor) { + data->skip_update_clipcursor = SDL_FALSE; return; } @@ -900,7 +916,6 @@ WIN_UpdateClipCursor(SDL_Window *window) (window->flags & SDL_WINDOW_INPUT_FOCUS)) { if (mouse->relative_mode && !mouse->relative_mode_warp) { LONG cx, cy; - RECT rect; GetWindowRect(data->hwnd, &rect); cx = (rect.left + rect.right) / 2; @@ -912,17 +927,21 @@ WIN_UpdateClipCursor(SDL_Window *window) rect.top = cy - 1; rect.bottom = cy + 1; - ClipCursor(&rect); + if (ClipCursor(&rect)) { + data->cursor_clipped_rect = rect; + } } else { - RECT rect; if (GetClientRect(data->hwnd, &rect) && !IsRectEmpty(&rect)) { ClientToScreen(data->hwnd, (LPPOINT) & rect); ClientToScreen(data->hwnd, (LPPOINT) & rect + 1); - ClipCursor(&rect); + if (ClipCursor(&rect)) { + data->cursor_clipped_rect = rect; + } } } - } else { + } else if (GetClipCursor(&rect) && SDL_memcmp(&rect, &data->cursor_clipped_rect, sizeof(rect)) == 0) { ClipCursor(NULL); + SDL_zero(data->cursor_clipped_rect); } } @@ -965,6 +984,13 @@ WIN_SetWindowOpacity(_THIS, SDL_Window * window, float opacity) return 0; } +void +WIN_AcceptDragAndDrop(SDL_Window * window, SDL_bool accept) +{ + const SDL_WindowData *data = (SDL_WindowData *) window->driverdata; + DragAcceptFiles(data->hwnd, accept ? TRUE : FALSE); +} + #endif /* SDL_VIDEO_DRIVER_WINDOWS */ /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/windows/SDL_windowswindow.h b/Engine/lib/sdl/src/video/windows/SDL_windowswindow.h index 0325abb8e..b738c3494 100644 --- a/Engine/lib/sdl/src/video/windows/SDL_windowswindow.h +++ b/Engine/lib/sdl/src/video/windows/SDL_windowswindow.h @@ -44,8 +44,10 @@ typedef struct SDL_bool in_border_change; SDL_bool in_title_click; Uint8 focus_click_pending; + SDL_bool skip_update_clipcursor; SDL_bool windowed_mode_was_maximized; SDL_bool in_window_deactivation; + RECT cursor_clipped_rect; struct SDL_VideoData *videodata; #if SDL_VIDEO_OPENGL_EGL EGLSurface egl_surface; @@ -78,6 +80,7 @@ extern SDL_bool WIN_GetWindowWMInfo(_THIS, SDL_Window * window, extern void WIN_OnWindowEnter(_THIS, SDL_Window * window); extern void WIN_UpdateClipCursor(SDL_Window *window); extern int WIN_SetWindowHitTest(SDL_Window *window, SDL_bool enabled); +extern void WIN_AcceptDragAndDrop(SDL_Window * window, SDL_bool accept); #endif /* SDL_windowswindow_h_ */ diff --git a/Engine/lib/sdl/src/video/x11/SDL_x11framebuffer.h b/Engine/lib/sdl/src/video/x11/SDL_x11framebuffer.h index 61bb0c55c..6a3178869 100644 --- a/Engine/lib/sdl/src/video/x11/SDL_x11framebuffer.h +++ b/Engine/lib/sdl/src/video/x11/SDL_x11framebuffer.h @@ -18,6 +18,10 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + +#ifndef SDL_x11framebuffer_h_ +#define SDL_x11framebuffer_h_ + #include "../../SDL_internal.h" @@ -28,4 +32,6 @@ extern int X11_UpdateWindowFramebuffer(_THIS, SDL_Window * window, const SDL_Rect * rects, int numrects); extern void X11_DestroyWindowFramebuffer(_THIS, SDL_Window * window); +#endif /* SDL_x11framebuffer_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/x11/SDL_x11keyboard.c b/Engine/lib/sdl/src/video/x11/SDL_x11keyboard.c index d667c7b22..a57adf9bb 100644 --- a/Engine/lib/sdl/src/video/x11/SDL_x11keyboard.c +++ b/Engine/lib/sdl/src/video/x11/SDL_x11keyboard.c @@ -266,7 +266,7 @@ X11_InitKeyboard(_THIS) int best_distance; int best_index; int distance; - BOOL xkb_repeat = 0; + Bool xkb_repeat = 0; X11_XAutoRepeatOn(data->display); @@ -292,9 +292,7 @@ X11_InitKeyboard(_THIS) char *prev_locale = setlocale(LC_ALL, NULL); char *prev_xmods = X11_XSetLocaleModifiers(NULL); const char *new_xmods = ""; -#if defined(HAVE_IBUS_IBUS_H) || defined(HAVE_FCITX_FRONTEND_H) const char *env_xmods = SDL_getenv("XMODIFIERS"); -#endif SDL_bool has_dbus_ime_support = SDL_FALSE; if (prev_locale) { @@ -309,16 +307,12 @@ X11_InitKeyboard(_THIS) when it is used via XIM which causes issues. Prevent this by forcing @im=none if XMODIFIERS contains @im=ibus. IBus can still be used via the DBus implementation, which also has support for pre-editing. */ -#ifdef HAVE_IBUS_IBUS_H if (env_xmods && SDL_strstr(env_xmods, "@im=ibus") != NULL) { has_dbus_ime_support = SDL_TRUE; } -#endif -#ifdef HAVE_FCITX_FRONTEND_H if (env_xmods && SDL_strstr(env_xmods, "@im=fcitx") != NULL) { has_dbus_ime_support = SDL_TRUE; } -#endif if (has_dbus_ime_support || !xkb_repeat) { new_xmods = "@im=none"; } diff --git a/Engine/lib/sdl/src/video/x11/SDL_x11messagebox.c b/Engine/lib/sdl/src/video/x11/SDL_x11messagebox.c index fe6ab199e..70a472ab4 100644 --- a/Engine/lib/sdl/src/video/x11/SDL_x11messagebox.c +++ b/Engine/lib/sdl/src/video/x11/SDL_x11messagebox.c @@ -44,7 +44,6 @@ #endif #define MAX_BUTTONS 8 /* Maximum number of buttons supported */ -#define MAX_TEXT_LINES 32 /* Maximum number of text lines supported */ #define MIN_BUTTON_WIDTH 64 /* Minimum button width */ #define MIN_DIALOG_WIDTH 200 /* Minimum dialog width */ #define MIN_DIALOG_HEIGHT 100 /* Minimum dialog height */ @@ -101,7 +100,7 @@ typedef struct SDL_MessageBoxDataX11 int xtext, ytext; /* Text position to start drawing at. */ int numlines; /* Count of Text lines. */ int text_height; /* Height for text lines. */ - TextLineData linedata[ MAX_TEXT_LINES ]; + TextLineData *linedata; int *pbuttonid; /* Pointer to user return buttonid value. */ @@ -223,6 +222,18 @@ X11_MessageBoxInit( SDL_MessageBoxDataX11 *data, const SDL_MessageBoxData * mess return 0; } +static int +CountLinesOfText(const char *text) +{ + int retval = 0; + while (text && *text) { + const char *lf = SDL_strchr(text, '\n'); + retval++; /* even without an endline, this counts as a line. */ + text = lf ? lf + 1 : NULL; + } + return retval; +} + /* Calculate and initialize text and button locations. */ static int X11_MessageBoxInitPositions( SDL_MessageBoxDataX11 *data ) @@ -237,29 +248,35 @@ X11_MessageBoxInitPositions( SDL_MessageBoxDataX11 *data ) /* Go over text and break linefeeds into separate lines. */ if ( messageboxdata->message && messageboxdata->message[ 0 ] ) { const char *text = messageboxdata->message; - TextLineData *plinedata = data->linedata; + const int linecount = CountLinesOfText(text); + TextLineData *plinedata = (TextLineData *) SDL_malloc(sizeof (TextLineData) * linecount); - for ( i = 0; i < MAX_TEXT_LINES; i++, plinedata++ ) { + if (!plinedata) { + return SDL_OutOfMemory(); + } + + data->linedata = plinedata; + data->numlines = linecount; + + for ( i = 0; i < linecount; i++, plinedata++ ) { + const char *lf = SDL_strchr( text, '\n' ); + const int length = lf ? ( lf - text ) : SDL_strlen( text ); int height; - char *lf = SDL_strchr( ( char * )text, '\n' ); - data->numlines++; - - /* Only grab length up to lf if it exists and isn't the last line. */ - plinedata->length = ( lf && ( i < MAX_TEXT_LINES - 1 ) ) ? ( lf - text ) : SDL_strlen( text ); plinedata->text = text; - GetTextWidthHeight( data, text, plinedata->length, &plinedata->width, &height ); + GetTextWidthHeight( data, text, length, &plinedata->width, &height ); /* Text and widths are the largest we've ever seen. */ data->text_height = IntMax( data->text_height, height ); text_width_max = IntMax( text_width_max, plinedata->width ); + plinedata->length = length; if (lf && (lf > text) && (lf[-1] == '\r')) { plinedata->length--; } - text += plinedata->length + 1; + text += length + 1; /* Break if there are no more linefeeds. */ if ( !lf ) @@ -369,6 +386,8 @@ X11_MessageBoxShutdown( SDL_MessageBoxDataX11 *data ) X11_XCloseDisplay( data->display ); data->display = NULL; } + + SDL_free(data->linedata); } /* Create and set up our X11 dialog box indow. */ diff --git a/Engine/lib/sdl/src/video/x11/SDL_x11messagebox.h b/Engine/lib/sdl/src/video/x11/SDL_x11messagebox.h index cab407b2d..6515983fa 100644 --- a/Engine/lib/sdl/src/video/x11/SDL_x11messagebox.h +++ b/Engine/lib/sdl/src/video/x11/SDL_x11messagebox.h @@ -19,10 +19,15 @@ 3. This notice may not be removed or altered from any source distribution. */ +#ifndef SDL_x11messagebox_h_ +#define SDL_x11messagebox_h_ + #if SDL_VIDEO_DRIVER_X11 extern int X11_ShowMessageBox(const SDL_MessageBoxData *messageboxdata, int *buttonid); #endif /* SDL_VIDEO_DRIVER_X11 */ +#endif /* SDL_x11messagebox_h_ */ + /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/x11/SDL_x11opengl.h b/Engine/lib/sdl/src/video/x11/SDL_x11opengl.h index 1a26ea099..7331b71ff 100644 --- a/Engine/lib/sdl/src/video/x11/SDL_x11opengl.h +++ b/Engine/lib/sdl/src/video/x11/SDL_x11opengl.h @@ -38,14 +38,14 @@ struct SDL_GLDriverData SDL_bool HAS_GLX_ARB_create_context_robustness; SDL_bool HAS_GLX_ARB_create_context_no_error; - /* Max version of OpenGL ES context that can be created if the - implementation supports GLX_EXT_create_context_es2_profile. - major = minor = 0 when unsupported. - */ - struct { - int major; - int minor; - } es_profile_max_supported_version; + /* Max version of OpenGL ES context that can be created if the + implementation supports GLX_EXT_create_context_es2_profile. + major = minor = 0 when unsupported. + */ + struct { + int major; + int minor; + } es_profile_max_supported_version; Bool (*glXQueryExtension) (Display*,int*,int*); void *(*glXGetProcAddress) (const GLubyte*); diff --git a/Engine/lib/sdl/src/video/x11/SDL_x11sym.h b/Engine/lib/sdl/src/video/x11/SDL_x11sym.h index a07a0309c..670999215 100644 --- a/Engine/lib/sdl/src/video/x11/SDL_x11sym.h +++ b/Engine/lib/sdl/src/video/x11/SDL_x11sym.h @@ -45,7 +45,7 @@ SDL_X11_SYM(Pixmap,XCreateBitmapFromData,(Display *dpy,Drawable d,_Xconst char * SDL_X11_SYM(Colormap,XCreateColormap,(Display* a,Window b,Visual* c,int d),(a,b,c,d),return) SDL_X11_SYM(Cursor,XCreatePixmapCursor,(Display* a,Pixmap b,Pixmap c,XColor* d,XColor* e,unsigned int f,unsigned int g),(a,b,c,d,e,f,g),return) SDL_X11_SYM(Cursor,XCreateFontCursor,(Display* a,unsigned int b),(a,b),return) -SDL_X11_SYM(XFontSet,XCreateFontSet,(Display* a, _Xconst char* b, char*** c, int* d, char** e),(a,b,c,d,e),return) +SDL_X11_SYM(XFontSet,XCreateFontSet,(Display* a, _Xconst char* b, char*** c, int* d, char** e),(a,b,c,d,e),return) SDL_X11_SYM(GC,XCreateGC,(Display* a,Drawable b,unsigned long c,XGCValues* d),(a,b,c,d),return) SDL_X11_SYM(XImage*,XCreateImage,(Display* a,Visual* b,unsigned int c,int d,int e,char* f,unsigned int g,unsigned int h,int i,int j),(a,b,c,d,e,f,g,h,i,j),return) SDL_X11_SYM(Window,XCreateWindow,(Display* a,Window b,int c,int d,unsigned int e,unsigned int f,unsigned int g,int h,unsigned int i,Visual* j,unsigned long k,XSetWindowAttributes* l),(a,b,c,d,e,f,g,h,i,j,k,l),return) @@ -180,7 +180,7 @@ SDL_X11_SYM(Status,XkbGetUpdatedMap,(Display* a,unsigned int b,XkbDescPtr c),(a, SDL_X11_SYM(XkbDescPtr,XkbGetMap,(Display* a,unsigned int b,unsigned int c),(a,b,c),return) SDL_X11_SYM(void,XkbFreeClientMap,(XkbDescPtr a,unsigned int b, Bool c),(a,b,c),) SDL_X11_SYM(void,XkbFreeKeyboard,(XkbDescPtr a,unsigned int b, Bool c),(a,b,c),) -SDL_X11_SYM(BOOL,XkbSetDetectableAutoRepeat,(Display* a, BOOL b, BOOL* c),(a,b,c),return) +SDL_X11_SYM(Bool,XkbSetDetectableAutoRepeat,(Display* a, Bool b, Bool* c),(a,b,c),return) #endif #if NeedWidePrototypes @@ -201,7 +201,7 @@ SDL_X11_SYM(void,XUnsetICFocus,(XIC a),(a),) SDL_X11_SYM(XIM,XOpenIM,(Display* a,struct _XrmHashBucketRec* b,char* c,char* d),(a,b,c,d),return) SDL_X11_SYM(Status,XCloseIM,(XIM a),(a),return) SDL_X11_SYM(void,Xutf8DrawString,(Display *a, Drawable b, XFontSet c, GC d, int e, int f, _Xconst char *g, int h),(a,b,c,d,e,f,g,h),) -SDL_X11_SYM(int,Xutf8TextExtents,(XFontSet a, _Xconst char* b, int c, XRectangle* d, XRectangle* e),(a,b,c,d,e),return) +SDL_X11_SYM(int,Xutf8TextExtents,(XFontSet a, _Xconst char* b, int c, XRectangle* d, XRectangle* e),(a,b,c,d,e),return) SDL_X11_SYM(char*,XSetLocaleModifiers,(const char *a),(a),return) SDL_X11_SYM(char*,Xutf8ResetIC,(XIC a),(a),return) #endif diff --git a/Engine/lib/sdl/src/video/x11/SDL_x11video.c b/Engine/lib/sdl/src/video/x11/SDL_x11video.c index b8f8edff7..b3b1a70dd 100644 --- a/Engine/lib/sdl/src/video/x11/SDL_x11video.c +++ b/Engine/lib/sdl/src/video/x11/SDL_x11video.c @@ -260,6 +260,7 @@ X11_CreateDevice(int devindex) device->DestroyWindowFramebuffer = X11_DestroyWindowFramebuffer; device->GetWindowWMInfo = X11_GetWindowWMInfo; device->SetWindowHitTest = X11_SetWindowHitTest; + device->AcceptDragAndDrop = X11_AcceptDragAndDrop; device->shape_driver.CreateShaper = X11_CreateShaper; device->shape_driver.SetWindowShape = X11_SetWindowShape; diff --git a/Engine/lib/sdl/src/video/x11/SDL_x11window.c b/Engine/lib/sdl/src/video/x11/SDL_x11window.c index be03aa6de..0a254b0ff 100644 --- a/Engine/lib/sdl/src/video/x11/SDL_x11window.c +++ b/Engine/lib/sdl/src/video/x11/SDL_x11window.c @@ -390,7 +390,6 @@ X11_CreateWindow(_THIS, SDL_Window * window) const char *wintype_name = NULL; long compositor = 1; Atom _NET_WM_PID; - Atom XdndAware, xdnd_version = 5; long fevent = 0; #if SDL_VIDEO_OPENGL_GLX || SDL_VIDEO_OPENGL_EGL @@ -651,11 +650,6 @@ X11_CreateWindow(_THIS, SDL_Window * window) PropertyChangeMask | StructureNotifyMask | KeymapStateMask | fevent)); - XdndAware = X11_XInternAtom(display, "XdndAware", False); - X11_XChangeProperty(display, w, XdndAware, XA_ATOM, 32, - PropModeReplace, - (unsigned char*)&xdnd_version, 1); - X11_XFlush(display); return 0; @@ -1604,6 +1598,22 @@ X11_SetWindowHitTest(SDL_Window *window, SDL_bool enabled) return 0; /* just succeed, the real work is done elsewhere. */ } +void +X11_AcceptDragAndDrop(SDL_Window * window, SDL_bool accept) +{ + SDL_WindowData *data = (SDL_WindowData *) window->driverdata; + Display *display = data->videodata->display; + Atom XdndAware = X11_XInternAtom(display, "XdndAware", False); + + if (accept) { + Atom xdnd_version = 5; + X11_XChangeProperty(display, data->xwindow, XdndAware, XA_ATOM, 32, + PropModeReplace, (unsigned char*)&xdnd_version, 1); + } else { + X11_XDeleteProperty(display, data->xwindow, XdndAware); + } +} + #endif /* SDL_VIDEO_DRIVER_X11 */ /* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/src/video/x11/SDL_x11window.h b/Engine/lib/sdl/src/video/x11/SDL_x11window.h index 7c4c6c5e5..6ee80169f 100644 --- a/Engine/lib/sdl/src/video/x11/SDL_x11window.h +++ b/Engine/lib/sdl/src/video/x11/SDL_x11window.h @@ -104,6 +104,7 @@ extern void X11_DestroyWindow(_THIS, SDL_Window * window); extern SDL_bool X11_GetWindowWMInfo(_THIS, SDL_Window * window, struct SDL_SysWMinfo *info); extern int X11_SetWindowHitTest(SDL_Window *window, SDL_bool enabled); +extern void X11_AcceptDragAndDrop(SDL_Window * window, SDL_bool accept); #endif /* SDL_x11window_h_ */ diff --git a/Engine/lib/sdl/src/video/yuv2rgb/yuv_rgb_std_func.h b/Engine/lib/sdl/src/video/yuv2rgb/yuv_rgb_std_func.h index bf4f48eab..f0ab5c660 100644 --- a/Engine/lib/sdl/src/video/yuv2rgb/yuv_rgb_std_func.h +++ b/Engine/lib/sdl/src/video/yuv2rgb/yuv_rgb_std_func.h @@ -77,20 +77,20 @@ void STD_FUNCTION_NAME( { const YUV2RGBParam *const param = &(YUV2RGB[yuv_type]); #if YUV_FORMAT == YUV_FORMAT_420 - const int y_pixel_stride = 1; - const int uv_pixel_stride = 1; - const int uv_x_sample_interval = 2; - const int uv_y_sample_interval = 2; + #define y_pixel_stride 1 + #define uv_pixel_stride 1 + #define uv_x_sample_interval 2 + #define uv_y_sample_interval 2 #elif YUV_FORMAT == YUV_FORMAT_422 - const int y_pixel_stride = 2; - const int uv_pixel_stride = 4; - const int uv_x_sample_interval = 2; - const int uv_y_sample_interval = 1; + #define y_pixel_stride 2 + #define uv_pixel_stride 4 + #define uv_x_sample_interval 2 + #define uv_y_sample_interval 1 #elif YUV_FORMAT == YUV_FORMAT_NV12 - const int y_pixel_stride = 1; - const int uv_pixel_stride = 2; - const int uv_x_sample_interval = 2; - const int uv_y_sample_interval = 2; + #define y_pixel_stride 1 + #define uv_pixel_stride 2 + #define uv_x_sample_interval 2 + #define uv_y_sample_interval 2 #endif uint32_t x, y; @@ -101,9 +101,12 @@ void STD_FUNCTION_NAME( *u_ptr=U+(y/uv_y_sample_interval)*UV_stride, *v_ptr=V+(y/uv_y_sample_interval)*UV_stride; - uint8_t *rgb_ptr1=RGB+y*RGB_stride, - *rgb_ptr2=RGB+(y+1)*RGB_stride; - + uint8_t *rgb_ptr1=RGB+y*RGB_stride; + + #if uv_y_sample_interval > 1 + uint8_t *rgb_ptr2=RGB+(y+1)*RGB_stride; + #endif + for(x=0; x<(width-(uv_x_sample_interval-1)); x+=uv_x_sample_interval) { // Compute U and V contributions, common to the four pixels @@ -123,13 +126,13 @@ void STD_FUNCTION_NAME( y_tmp = ((y_ptr1[y_pixel_stride]-param->y_shift)*param->y_factor); PACK_PIXEL(rgb_ptr1); - if (uv_y_sample_interval > 1) { - y_tmp = ((y_ptr2[0]-param->y_shift)*param->y_factor); - PACK_PIXEL(rgb_ptr2); + #if uv_y_sample_interval > 1 + y_tmp = ((y_ptr2[0]-param->y_shift)*param->y_factor); + PACK_PIXEL(rgb_ptr2); - y_tmp = ((y_ptr2[y_pixel_stride]-param->y_shift)*param->y_factor); - PACK_PIXEL(rgb_ptr2); - } + y_tmp = ((y_ptr2[y_pixel_stride]-param->y_shift)*param->y_factor); + PACK_PIXEL(rgb_ptr2); + #endif y_ptr1+=2*y_pixel_stride; y_ptr2+=2*y_pixel_stride; @@ -154,10 +157,10 @@ void STD_FUNCTION_NAME( int32_t y_tmp = ((y_ptr1[0]-param->y_shift)*param->y_factor); PACK_PIXEL(rgb_ptr1); - if (uv_y_sample_interval > 1) { - y_tmp = ((y_ptr2[0]-param->y_shift)*param->y_factor); - PACK_PIXEL(rgb_ptr2); - } + #if uv_y_sample_interval > 1 + y_tmp = ((y_ptr2[0]-param->y_shift)*param->y_factor); + PACK_PIXEL(rgb_ptr2); + #endif } } @@ -212,6 +215,11 @@ void STD_FUNCTION_NAME( PACK_PIXEL(rgb_ptr1); } } + + #undef y_pixel_stride + #undef uv_pixel_stride + #undef uv_x_sample_interval + #undef uv_y_sample_interval } #undef STD_FUNCTION_NAME diff --git a/Engine/lib/sdl/test/Makefile.in b/Engine/lib/sdl/test/Makefile.in index bc9c24ab9..9ddd6ffe7 100644 --- a/Engine/lib/sdl/test/Makefile.in +++ b/Engine/lib/sdl/test/Makefile.in @@ -55,6 +55,7 @@ TARGETS = \ testrumble$(EXE) \ testscale$(EXE) \ testsem$(EXE) \ + testsensor$(EXE) \ testshader$(EXE) \ testshape$(EXE) \ testsprite2$(EXE) \ @@ -240,6 +241,9 @@ testscale$(EXE): $(srcdir)/testscale.c testsem$(EXE): $(srcdir)/testsem.c $(CC) -o $@ $^ $(CFLAGS) $(LIBS) +testsensor$(EXE): $(srcdir)/testsensor.c + $(CC) -o $@ $^ $(CFLAGS) $(LIBS) + testshader$(EXE): $(srcdir)/testshader.c $(CC) -o $@ $^ $(CFLAGS) $(LIBS) @GLLIB@ @MATHLIB@ @@ -313,7 +317,10 @@ distclean: clean %.wav: $(srcdir)/%.wav cp $< $@ -copydatafiles: copybmpfiles copywavfiles +%.dat: $(srcdir)/%.dat + cp $< $@ + +copydatafiles: copybmpfiles copywavfiles copydatfiles .PHONY : copydatafiles copybmpfiles: $(foreach bmp,$(wildcard $(srcdir)/*.bmp),$(notdir $(bmp))) @@ -322,3 +329,6 @@ copybmpfiles: $(foreach bmp,$(wildcard $(srcdir)/*.bmp),$(notdir $(bmp))) copywavfiles: $(foreach wav,$(wildcard $(srcdir)/*.wav),$(notdir $(wav))) .PHONY : copywavfiles +copydatfiles: $(foreach dat,$(wildcard $(srcdir)/*.dat),$(notdir $(dat))) +.PHONY : copydatfiles + diff --git a/Engine/lib/sdl/test/testcustomcursor.c b/Engine/lib/sdl/test/testcustomcursor.c index b99a10bba..469449818 100644 --- a/Engine/lib/sdl/test/testcustomcursor.c +++ b/Engine/lib/sdl/test/testcustomcursor.c @@ -73,6 +73,24 @@ init_color_cursor(const char *file) SDL_Cursor *cursor = NULL; SDL_Surface *surface = SDL_LoadBMP(file); if (surface) { + if (surface->format->palette) { + SDL_SetColorKey(surface, 1, *(Uint8 *) surface->pixels); + } else { + switch (surface->format->BitsPerPixel) { + case 15: + SDL_SetColorKey(surface, 1, (*(Uint16 *)surface->pixels) & 0x00007FFF); + break; + case 16: + SDL_SetColorKey(surface, 1, *(Uint16 *)surface->pixels); + break; + case 24: + SDL_SetColorKey(surface, 1, (*(Uint32 *)surface->pixels) & 0x00FFFFFF); + break; + case 32: + SDL_SetColorKey(surface, 1, *(Uint32 *)surface->pixels); + break; + } + } cursor = SDL_CreateColorCursor(surface, 0, 0); SDL_FreeSurface(surface); } @@ -116,7 +134,9 @@ init_system_cursor(const char *image[]) static SDLTest_CommonState *state; int done; -SDL_Cursor *cursor = NULL; +static SDL_Cursor *cursors[1+SDL_NUM_SYSTEM_CURSORS]; +static int current_cursor; +static int show_cursor; /* Call this instead of exit(), so we can clean up SDL: atexit() is evil. */ static void @@ -134,6 +154,18 @@ loop() /* Check for events */ while (SDL_PollEvent(&event)) { SDLTest_CommonEvent(state, &event, &done); + if (event.type == SDL_MOUSEBUTTONDOWN) { + if (event.button.button == SDL_BUTTON_LEFT) { + ++current_cursor; + if (current_cursor == SDL_arraysize(cursors)) { + current_cursor = 0; + } + SDL_SetCursor(cursors[current_cursor]); + } else { + show_cursor = !show_cursor; + SDL_ShowCursor(show_cursor); + } + } } for (i = 0; i < state->num_windows; ++i) { @@ -188,15 +220,22 @@ main(int argc, char *argv[]) } if (color_cursor) { - cursor = init_color_cursor(color_cursor); + cursors[0] = init_color_cursor(color_cursor); } else { - cursor = init_system_cursor(arrow); + cursors[0] = init_system_cursor(arrow); } - if (!cursor) { + if (!cursors[0]) { SDL_Log("Error, couldn't create cursor\n"); quit(2); } - SDL_SetCursor(cursor); + for (i = 0; i < SDL_NUM_SYSTEM_CURSORS; ++i) { + cursors[1+i] = SDL_CreateSystemCursor((SDL_SystemCursor)i); + if (!cursors[1+i]) { + SDL_Log("Error, couldn't create system cursor %d\n", i); + quit(2); + } + } + SDL_SetCursor(cursors[0]); /* Main render loop */ done = 0; @@ -208,9 +247,13 @@ main(int argc, char *argv[]) } #endif - SDL_FreeCursor(cursor); + for (i = 0; i < SDL_arraysize(cursors); ++i) { + SDL_FreeCursor(cursors[i]); + } quit(0); /* keep the compiler happy ... */ return(0); } + +/* vi: set ts=4 sw=4 expandtab: */ diff --git a/Engine/lib/sdl/test/testgamecontroller.c b/Engine/lib/sdl/test/testgamecontroller.c index ef3a02b2f..c8616d7cd 100644 --- a/Engine/lib/sdl/test/testgamecontroller.c +++ b/Engine/lib/sdl/test/testgamecontroller.c @@ -114,6 +114,11 @@ loop(void *arg) case SDL_CONTROLLERBUTTONDOWN: case SDL_CONTROLLERBUTTONUP: SDL_Log("Controller button %s %s\n", SDL_GameControllerGetStringForButton((SDL_GameControllerButton)event.cbutton.button), event.cbutton.state ? "pressed" : "released"); + /* First button triggers a 0.5 second full strength rumble */ + if (event.type == SDL_CONTROLLERBUTTONDOWN && + event.cbutton.button == SDL_CONTROLLER_BUTTON_A) { + SDL_GameControllerRumble(gamecontroller, 0xFFFF, 0xFFFF, 500); + } break; case SDL_KEYDOWN: if (event.key.keysym.sym != SDLK_ESCAPE) { diff --git a/Engine/lib/sdl/test/testjoystick.c b/Engine/lib/sdl/test/testjoystick.c index d2d1c4e2c..bca749244 100644 --- a/Engine/lib/sdl/test/testjoystick.c +++ b/Engine/lib/sdl/test/testjoystick.c @@ -90,6 +90,10 @@ loop(void *arg) case SDL_JOYBUTTONDOWN: SDL_Log("Joystick %d button %d down\n", event.jbutton.which, event.jbutton.button); + /* First button triggers a 0.5 second full strength rumble */ + if (event.jbutton.button == 0) { + SDL_JoystickRumble(joystick, 0xFFFF, 0xFFFF, 500); + } break; case SDL_JOYBUTTONUP: SDL_Log("Joystick %d button %d up\n", diff --git a/Engine/lib/sdl/test/testplatform.c b/Engine/lib/sdl/test/testplatform.c index 84efab319..1c1d2dcd8 100644 --- a/Engine/lib/sdl/test/testplatform.c +++ b/Engine/lib/sdl/test/testplatform.c @@ -380,6 +380,7 @@ TestCPUInfo(SDL_bool verbose) SDL_Log("SSE4.2 %s\n", SDL_HasSSE42()? "detected" : "not detected"); SDL_Log("AVX %s\n", SDL_HasAVX()? "detected" : "not detected"); SDL_Log("AVX2 %s\n", SDL_HasAVX2()? "detected" : "not detected"); + SDL_Log("AVX-512F %s\n", SDL_HasAVX512F()? "detected" : "not detected"); SDL_Log("NEON %s\n", SDL_HasNEON()? "detected" : "not detected"); SDL_Log("System RAM %d MB\n", SDL_GetSystemRAM()); } diff --git a/Engine/lib/sdl/test/testresample.c b/Engine/lib/sdl/test/testresample.c index bcdaa669a..4234d9e45 100644 --- a/Engine/lib/sdl/test/testresample.c +++ b/Engine/lib/sdl/test/testresample.c @@ -93,7 +93,7 @@ main(int argc, char **argv) SDL_WriteLE32(io, 0x45564157); /* WAVE */ SDL_WriteLE32(io, 0x20746D66); /* fmt */ SDL_WriteLE32(io, 16); /* chunk size */ - SDL_WriteLE16(io, 1); /* uncompressed */ + SDL_WriteLE16(io, SDL_AUDIO_ISFLOAT(spec.format) ? 3 : 1); /* uncompressed */ SDL_WriteLE16(io, cvtchans); /* channels */ SDL_WriteLE32(io, cvtfreq); /* sample rate */ SDL_WriteLE32(io, avgbytes); /* average bytes per second */ diff --git a/Engine/lib/sdl/test/testsensor.c b/Engine/lib/sdl/test/testsensor.c new file mode 100644 index 000000000..00bfd137d --- /dev/null +++ b/Engine/lib/sdl/test/testsensor.c @@ -0,0 +1,117 @@ +/* + Copyright (C) 1997-2018 Sam Lantinga + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely. +*/ + +/* Simple test of the SDL sensor code */ + +#include "SDL.h" + +static const char *GetSensorTypeString(SDL_SensorType type) +{ + static char unknown_type[64]; + + switch (type) + { + case SDL_SENSOR_INVALID: + return "SDL_SENSOR_INVALID"; + case SDL_SENSOR_UNKNOWN: + return "SDL_SENSOR_UNKNOWN"; + case SDL_SENSOR_ACCEL: + return "SDL_SENSOR_ACCEL"; + case SDL_SENSOR_GYRO: + return "SDL_SENSOR_GYRO"; + default: + SDL_snprintf(unknown_type, sizeof(unknown_type), "UNKNOWN (%d)", type); + return unknown_type; + } +} + +static void HandleSensorEvent(SDL_SensorEvent *event) +{ + SDL_Sensor *sensor = SDL_SensorFromInstanceID(event->which); + if (!sensor) { + SDL_Log("Couldn't get sensor for sensor event\n"); + return; + } + + switch (SDL_SensorGetType(sensor)) { + case SDL_SENSOR_ACCEL: + SDL_Log("Accelerometer update: %.2f, %.2f, %.2f\n", event->data[0], event->data[1], event->data[2]); + break; + case SDL_SENSOR_GYRO: + SDL_Log("Gyro update: %.2f, %.2f, %.2f\n", event->data[0], event->data[1], event->data[2]); + break; + default: + SDL_Log("Sensor update for sensor type %s\n", GetSensorTypeString(SDL_SensorGetType(sensor))); + break; + } +} + +int +main(int argc, char **argv) +{ + int i; + int num_sensors, num_opened; + + /* Load the SDL library */ + if (SDL_Init(SDL_INIT_SENSOR) < 0) { + SDL_Log("Couldn't initialize SDL: %s\n", SDL_GetError()); + return (1); + } + + num_sensors = SDL_NumSensors(); + num_opened = 0; + + SDL_Log("There are %d sensors available\n", num_sensors); + for (i = 0; i < num_sensors; ++i) { + SDL_Log("Sensor %d: %s, type %s, platform type %d\n", + SDL_SensorGetDeviceInstanceID(i), + SDL_SensorGetDeviceName(i), + GetSensorTypeString(SDL_SensorGetDeviceType(i)), + SDL_SensorGetDeviceNonPortableType(i)); + + if (SDL_SensorGetDeviceType(i) != SDL_SENSOR_UNKNOWN) { + SDL_Sensor *sensor = SDL_SensorOpen(i); + if (sensor == NULL) { + SDL_Log("Couldn't open sensor %d: %s\n", SDL_SensorGetDeviceInstanceID(i), SDL_GetError()); + } else { + ++num_opened; + } + } + } + SDL_Log("Opened %d sensors\n", num_opened); + + if (num_opened > 0) { + SDL_bool done = SDL_FALSE; + SDL_Event event; + + SDL_CreateWindow("Sensor Test", 0, 0, 0, 0, SDL_WINDOW_FULLSCREEN_DESKTOP); + while (!done) { + while (SDL_PollEvent(&event) > 0) { + switch (event.type) { + case SDL_SENSORUPDATE: + HandleSensorEvent(&event.sensor); + break; + case SDL_MOUSEBUTTONUP: + case SDL_KEYUP: + case SDL_QUIT: + done = SDL_TRUE; + break; + default: + break; + } + } + } + } + + SDL_Quit(); + return (0); +} diff --git a/Engine/lib/sdl/test/testthread.c b/Engine/lib/sdl/test/testthread.c index cdccfc499..4555a1e94 100644 --- a/Engine/lib/sdl/test/testthread.c +++ b/Engine/lib/sdl/test/testthread.c @@ -20,6 +20,7 @@ static SDL_TLSID tls; static int alive = 0; +static int testprio = 0; /* Call this instead of exit(), so we can clean up SDL: atexit() is evil. */ static void @@ -29,14 +30,37 @@ quit(int rc) exit(rc); } +static const char * +getprioritystr(SDL_ThreadPriority priority) +{ + switch(priority) + { + case SDL_THREAD_PRIORITY_LOW: return "SDL_THREAD_PRIORITY_LOW"; + case SDL_THREAD_PRIORITY_NORMAL: return "SDL_THREAD_PRIORITY_NORMAL"; + case SDL_THREAD_PRIORITY_HIGH: return "SDL_THREAD_PRIORITY_HIGH"; + case SDL_THREAD_PRIORITY_TIME_CRITICAL: return "SDL_THREAD_PRIORITY_TIME_CRITICAL"; + } + + return "???"; +} + int SDLCALL ThreadFunc(void *data) { + SDL_ThreadPriority prio = SDL_THREAD_PRIORITY_NORMAL; + SDL_TLSSet(tls, "baby thread", NULL); SDL_Log("Started thread %s: My thread id is %lu, thread data = %s\n", (char *) data, SDL_ThreadID(), (const char *)SDL_TLSGet(tls)); while (alive) { SDL_Log("Thread '%s' is alive!\n", (char *) data); + + if (testprio) { + SDL_Log("SDL_SetThreadPriority(%s):%d\n", getprioritystr(prio), SDL_SetThreadPriority(prio)); + if (++prio > SDL_THREAD_PRIORITY_TIME_CRITICAL) + prio = SDL_THREAD_PRIORITY_LOW; + } + SDL_Delay(1 * 1000); } SDL_Log("Thread '%s' exiting!\n", (char *) data); @@ -55,6 +79,7 @@ killed(int sig) int main(int argc, char *argv[]) { + int arg = 1; SDL_Thread *thread; /* Enable standard application logging */ @@ -66,6 +91,13 @@ main(int argc, char *argv[]) return (1); } + while (argv[arg] && *argv[arg] == '-') { + if (SDL_strcmp(argv[arg], "--prio") == 0) { + testprio = 1; + } + ++arg; + } + tls = SDL_TLSCreate(); SDL_assert(tls); SDL_TLSSet(tls, "main thread", NULL); diff --git a/Engine/lib/sdl/test/testvulkan.c b/Engine/lib/sdl/test/testvulkan.c index 73a21859a..cd682af1f 100644 --- a/Engine/lib/sdl/test/testvulkan.c +++ b/Engine/lib/sdl/test/testvulkan.c @@ -255,7 +255,7 @@ static void createInstance(void) appInfo.apiVersion = VK_API_VERSION_1_0; instanceCreateInfo.sType = VK_STRUCTURE_TYPE_INSTANCE_CREATE_INFO; instanceCreateInfo.pApplicationInfo = &appInfo; - if(!SDL_Vulkan_GetInstanceExtensions(state->windows[0], &extensionCount, NULL)) + if(!SDL_Vulkan_GetInstanceExtensions(NULL, &extensionCount, NULL)) { SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "SDL_Vulkan_GetInstanceExtensions(): %s\n", @@ -268,7 +268,7 @@ static void createInstance(void) SDL_OutOfMemory(); quit(2); } - if(!SDL_Vulkan_GetInstanceExtensions(state->windows[0], &extensionCount, extensions)) + if(!SDL_Vulkan_GetInstanceExtensions(NULL, &extensionCount, extensions)) { SDL_free((void*)extensions); SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, diff --git a/Engine/lib/sdl/test/testwm2.c b/Engine/lib/sdl/test/testwm2.c index 74a0fe028..5da387309 100644 --- a/Engine/lib/sdl/test/testwm2.c +++ b/Engine/lib/sdl/test/testwm2.c @@ -146,6 +146,9 @@ main(int argc, char *argv[]) quit(2); } + SDL_EventState(SDL_DROPFILE, SDL_ENABLE); + SDL_EventState(SDL_DROPTEXT, SDL_ENABLE); + for (i = 0; i < state->num_windows; ++i) { SDL_Renderer *renderer = state->renderers[i]; SDL_SetRenderDrawColor(renderer, 0xA0, 0xA0, 0xA0, 0xFF); diff --git a/Engine/source/T3D/systems/render/meshRenderSystem.h b/Engine/source/T3D/systems/render/meshRenderSystem.h index c4b57922b..727073b86 100644 --- a/Engine/source/T3D/systems/render/meshRenderSystem.h +++ b/Engine/source/T3D/systems/render/meshRenderSystem.h @@ -1,6 +1,6 @@ #pragma once #include "scene/sceneRenderState.h" -#include "core/util/SystemInterfaceList.h" +#include "core/util/systemInterfaceList.h" #include "ts/tsShape.h" #include "ts/tsShapeInstance.h" #include "T3D/assets/ShapeAsset.h" @@ -59,4 +59,4 @@ public: //Render our particular interface's data static void renderInterface(U32 interfaceIndex, SceneRenderState* state); -}; \ No newline at end of file +}; diff --git a/Engine/source/renderInstance/renderProbeMgr.h b/Engine/source/renderInstance/renderProbeMgr.h index fff0f3ba9..a601e99bd 100644 --- a/Engine/source/renderInstance/renderProbeMgr.h +++ b/Engine/source/renderInstance/renderProbeMgr.h @@ -126,7 +126,7 @@ public: void clear(); - inline bool ProbeRenderInst::operator ==(const ProbeRenderInst& b) const + inline bool operator ==(const ProbeRenderInst& b) const { return mProbeIdx == b.mProbeIdx; } diff --git a/Engine/source/shaderGen/HLSL/shaderFeatureHLSL.cpp b/Engine/source/shaderGen/HLSL/shaderFeatureHLSL.cpp index a191aaee7..d9ac96bd7 100644 --- a/Engine/source/shaderGen/HLSL/shaderFeatureHLSL.cpp +++ b/Engine/source/shaderGen/HLSL/shaderFeatureHLSL.cpp @@ -405,8 +405,11 @@ Var* ShaderFeatureHLSL::getInTexCoord( const char *name, texCoord->setType( type ); } - AssertFatal( dStrcmp( type, (const char*)texCoord->type ) == 0, - "ShaderFeatureHLSL::getInTexCoord - Type mismatch!" ); + //AssertFatal( dStrcmp( type, (const char*)texCoord->type ) == 0, + // "ShaderFeatureHLSL::getInTexCoord - Type mismatch!" ); + + if (dStrcmp(type, (const char*)texCoord->type) != 0) + return nullptr; return texCoord; } diff --git a/Templates/BaseGame/game/tools/gui/profiles.ed.cs b/Templates/BaseGame/game/tools/gui/profiles.ed.cs index 18bbabf42..336d203de 100644 --- a/Templates/BaseGame/game/tools/gui/profiles.ed.cs +++ b/Templates/BaseGame/game/tools/gui/profiles.ed.cs @@ -37,7 +37,7 @@ new GuiControlProfile (ToolsGuiDefaultProfile) mouseOverSelected = false; // fill color - opaque = true; + opaque = false; fillColor = EditorSettings.value("Theme/tabsColor"); fillColorHL = EditorSettings.value("Theme/tabsGLColor"); fillColorSEL = EditorSettings.value("Theme/tabsSELColor"); @@ -452,7 +452,7 @@ new GuiControlProfile( ToolsGuiScrollProfile ) if( !isObject( ToolsGuiOverlayProfile ) ) new GuiControlProfile( ToolsGuiOverlayProfile ) { - opaque = true; + opaque = false; fillColor = EditorSettings.value("Theme/windowBackgroundColor"); fontColor = EditorSettings.value("Theme/fieldTextColor"); fontColorHL = EditorSettings.value("Theme/fieldTextGLColor"); @@ -1111,6 +1111,7 @@ singleton GuiControlProfile( ToolsMenubarProfile : ToolsGuiDefaultProfile ) { bitmap = "./menubar"; category = "Editor"; + opaque=true; fillColor = EditorSettings.value("Theme/headerColor"); fontColor = EditorSettings.value("Theme/headerTextColor"); diff --git a/Templates/BaseGame/game/tools/settings.xml b/Templates/BaseGame/game/tools/settings.xml index 3c5aea189..c235de5b9 100644 --- a/Templates/BaseGame/game/tools/settings.xml +++ b/Templates/BaseGame/game/tools/settings.xml @@ -1,135 +1,148 @@ - - 15 - 0.8 - 0 - 100 - 0 - 0.8 - 1 - - 0 - 255 255 255 20 - 10 10 10 - 500 - 0 - 0 - - - - 72 70 68 255 - 178 175 172 255 - 72 70 68 255 - 240 240 240 255 - 50 49 48 255 - 32 31 30 255 - 96 94 92 255 - 17 16 15 255 - 59 58 57 255 - 43 43 43 255 - 37 36 35 255 - 50 49 48 255 - 236 234 232 255 - 50 49 48 255 - 100 98 96 255 - 59 58 57 255 - 234 232 230 255 - 255 255 255 255 - - 40 - WorldEditorInspectorPlugin - 0 - AssetWork_Debug.exe 1 - 6 - 50 + 0 + 40 screenCenter - - 0 - 1 - 8 - 20 - 255 - - - 1 - 51 51 51 100 - 255 255 255 100 - 102 102 102 100 - 0 - - - tools/worldEditor/images/SelectHandle - tools/worldEditor/images/DefaultHandle - tools/worldEditor/images/LockedHandle - - - 1 - 1 - 1 - 1 - 1 - + 6 + WorldEditorInspectorPlugin + 50 + AssetWork_Debug.exe - 0 - 2 - 1 - 1 0 0 + 0 + 1 100 + 2 + 1 - - 100 100 100 255 - 0 0 255 255 - Lime - 255 255 0 255 - 255 255 255 255 - 255 255 0 255 - 255 0 0 255 - - - 48 48 48 255 - 180 180 180 255 - 50 50 50 255 - 215 215 215 255 - 255 255 255 255 + + 0 + 255 + 20 + 8 + 1 - http://www.garagegames.com/products/torque-3d/documentation/user ../../../Documentation/Torque 3D - Script Manual.chm + http://www.garagegames.com/products/torque-3d/documentation/user http://www.garagegames.com/products/torque-3d/forums ../../../Documentation/Official Documentation.html + + 1 + 1 + 1 + 1 + 1 + + + 48 48 48 255 + 50 50 50 255 + 180 180 180 255 + 215 215 215 255 + 255 255 255 255 + + + 0 255 0 255 + 255 255 0 255 + 255 255 255 255 + 255 255 0 255 + 100 100 100 255 + 255 0 0 255 + 0 0 255 255 + + + 0 + 255 255 255 100 + 1 + 102 102 102 100 + 51 51 51 100 + + + tools/worldEditor/images/DefaultHandle + tools/worldEditor/images/SelectHandle + tools/worldEditor/images/LockedHandle + + + + 178 175 172 255 + 236 234 232 255 + 17 16 15 255 + 50 49 48 255 + 240 240 240 255 + 100 98 96 255 + 50 49 48 255 + 59 58 57 255 + 37 36 35 255 + 255 255 255 255 + 50 49 48 255 + 72 70 68 255 + 234 232 230 255 + 72 70 68 255 + 32 31 30 255 + 43 43 43 255 + 96 94 92 255 + 59 58 57 255 + + + raiseHeight + + 1 + ellipse + 1 1 + 40 40 + 1 + + + 90 + 0.1 + 50 + 100 + 0 + 1 + 10 + 1.000000 0.833333 0.666667 0.500000 0.333333 0.166667 0.000000 + 1 + 1.000000 0.833333 0.666667 0.500000 0.333333 0.166667 0.000000 + + + + 15 + 0 + 0 + 1 + 0.8 + 100 + 0.8 + + 10 10 10 + 0 + 255 255 255 20 + 500 + 0 + 0 + - 1024 768 tools/gui - - 1 - 1 - - - 0 - 0 - 0 + 1024 768 + + http://www.garagegames.com/products/torque-3d/documentation/user + ../../../Documentation/Torque 3D - Script Manual.chm + ../../../Documentation/Official Documentation.html - 1 - 8 - 1 - 1 1 - 2 - 1 0 - - - ../../../Documentation/Official Documentation.html - ../../../Documentation/Torque 3D - Script Manual.chm - http://www.garagegames.com/products/torque-3d/documentation/user + 2 + 1 + 8 + 1 + 1 + 1 Categorized @@ -137,22 +150,31 @@ 0 + + 0 + 0 + 0 + + + 1 + 1 + data/FPSGameplay/levels - - 25 - 5 + + 25 + - - AIPlayer - Grid_512_Orange + + AIPlayer + diff --git a/Tools/CMake/torque3d.cmake b/Tools/CMake/torque3d.cmake index e247da59b..bc4bac7cb 100644 --- a/Tools/CMake/torque3d.cmake +++ b/Tools/CMake/torque3d.cmake @@ -599,6 +599,10 @@ if( TORQUE_OPENGL ) endif() endif() +if(UNIX AND NOT APPLE) + set(CMAKE_EXE_LINKER_FLAGS "-no-pie") +endif() + ############################################################################### ############################################################################### finishExecutable()